diff --git a/.editorconfig b/.editorconfig index a0fa3eff170e..b8f6ef7f8e32 100644 --- a/.editorconfig +++ b/.editorconfig @@ -4,10 +4,10 @@ root = true [{*.patch,syntax_test_*}] trim_trailing_whitespace = false -[{*.c,*.cpp,*.h}] +[{*.c,*.cpp,*.h,*.ino}] charset = utf-8 -[{*.c,*.cpp,*.h,Makefile}] +[{*.c,*.cpp,*.h,*.ino,Makefile}] trim_trailing_whitespace = true insert_final_newline = true end_of_line = lf diff --git a/.github/workflows/test-builds.yml b/.github/workflows/test-builds.yml index 7549e3defcbb..e37f53240277 100644 --- a/.github/workflows/test-builds.yml +++ b/.github/workflows/test-builds.yml @@ -56,28 +56,33 @@ jobs: # STM32F1 (Maple) Environments - - STM32F103RC_btt - - STM32F103RC_btt_USB - - STM32F103RE_btt - - STM32F103RE_btt_USB - - STM32F103RC_fysetc + #- STM32F103RC_btt_maple + - STM32F103RC_btt_USB_maple + - STM32F103RC_fysetc_maple - STM32F103RC_meeb - - jgaurora_a5s_a1 - - STM32F103VE_longer - - mks_robin - - mks_robin_lite - - mks_robin_pro - - STM32F103RET6_creality - - mks_robin_nano35 + - jgaurora_a5s_a1_maple + - STM32F103VE_longer_maple + #- mks_robin_maple + - mks_robin_lite_maple + - mks_robin_pro_maple + #- mks_robin_nano35_maple + #- STM32F103RE_creality_maple + - STM32F103VE_ZM3E4V2_USB_maple # STM32 (ST) Environments + - STM32F103RC_btt + #- STM32F103RC_btt_USB + - STM32F103RE_btt + - STM32F103RE_btt_USB + - STM32F103RE_creality + - STM32F103VE_longer - STM32F407VE_black - STM32F401VE_STEVAL - BIGTREE_BTT002 - BIGTREE_SKR_PRO - BIGTREE_GTR_V1_0 - - mks_robin_stm32 + - mks_robin - ARMED - FYSETC_S6 - STM32F070CB_malyan @@ -86,9 +91,13 @@ jobs: - FLYF407ZG - rumba32 - LERDGEX - - mks_robin_nano35_stm32 + - LERDGEK + - mks_robin_nano35 - NUCLEO_F767ZI - REMRAM_V1 + - BTT_SKR_SE_BX + - chitu_f103 + - Index_Mobo_Rev03 # Put lengthy tests last @@ -98,12 +107,30 @@ jobs: # Non-working environment tests #- at90usb1286_cdc #- STM32F103CB_malyan + #- STM32F103RE #- mks_robin_mini steps: + - name: Check out the PR + uses: actions/checkout@v2 + + - name: Cache pip + uses: actions/cache@v2 + with: + path: ~/.cache/pip + key: ${{ runner.os }}-pip-${{ hashFiles('**/requirements.txt') }} + restore-keys: | + ${{ runner.os }}-pip- + + - name: Cache PlatformIO + uses: actions/cache@v2 + with: + path: ~/.platformio + key: ${{ runner.os }}-${{ hashFiles('**/lockfiles') }} + - name: Select Python 3.7 - uses: actions/setup-python@v1 + uses: actions/setup-python@v2 with: python-version: '3.7' # Version range or exact version of a Python version to use, using semvers version range syntax. architecture: 'x64' # optional x64 or x86. Defaults to x64 if not specified @@ -113,9 +140,6 @@ jobs: pip install -U https://github.com/platformio/platformio-core/archive/develop.zip platformio update - - name: Check out the PR - uses: actions/checkout@v2 - - name: Run ${{ matrix.test-platform }} Tests run: | make tests-single-ci TEST_TARGET=${{ matrix.test-platform }} diff --git a/.gitignore b/.gitignore index f7d49cc1ed28..0b852d767325 100755 --- a/.gitignore +++ b/.gitignore @@ -22,12 +22,16 @@ # Generated files _Version.h bdf2u8g +marlin_config.json +mczip.h +*.gen +*.sublime-workspace # # OS # applet/ -*.DS_Store +.DS_Store # # Misc @@ -37,7 +41,6 @@ applet/ *.rej *.bak *.idea -*.s *.i *.ii *.swp @@ -122,29 +125,6 @@ tags .gcc-flags.json /lib/ -# Workaround for Deviot+platformio quirks -Marlin/lib -Marlin/platformio.ini -Marlin/*/platformio.ini -Marlin/*/*/platformio.ini -Marlin/*/*/*/platformio.ini -Marlin/*/*/*/*/platformio.ini -Marlin/.travis.yml -Marlin/*/.travis.yml -Marlin/*/*/.travis.yml -Marlin/*/*/*/.travis.yml -Marlin/*/*/*/*/.travis.yml -Marlin/.gitignore -Marlin/*/.gitignore -Marlin/*/*/.gitignore -Marlin/*/*/*/.gitignore -Marlin/*/*/*/*/.gitignore -Marlin/readme.txt -Marlin/*/readme.txt -Marlin/*/*/readme.txt -Marlin/*/*/*/readme.txt -Marlin/*/*/*/*/readme.txt - # Secure Credentials Configuration_Secure.h @@ -160,16 +140,19 @@ __vm/ vc-fileutils.settings # Visual Studio Code -.vscode -.vscode/.browse.c_cpp.db* -.vscode/c_cpp_properties.json -.vscode/launch.json -.vscode/*.db +.vscode/* +!.vscode/extensions.json + +#Simulation +imgui.ini +eeprom.dat +spi_flash.bin -# cmake +#cmake CMakeLists.txt src/CMakeLists.txt CMakeListsPrivate.txt +build/ # CLion cmake-build-* @@ -186,7 +169,3 @@ __pycache__ # IOLogger logs *_log.csv - -# Simulation / Native -eeprom.dat -imgui.ini diff --git a/.vscode/extensions.json b/.vscode/extensions.json new file mode 100644 index 000000000000..f495d14f53e8 --- /dev/null +++ b/.vscode/extensions.json @@ -0,0 +1,11 @@ +{ + // See http://go.microsoft.com/fwlink/?LinkId=827846 + // for the documentation about the extensions.json format + "recommendations": [ + "marlinfirmware.auto-build", + "platformio.platformio-ide" + ], + "unwantedRecommendations": [ + "ms-vscode.cpptools-extension-pack" + ] +} diff --git a/Marlin/Configuration.h b/Marlin/Configuration.h index d7d8f23b4852..d6e6febe928f 100644 --- a/Marlin/Configuration.h +++ b/Marlin/Configuration.h @@ -37,7 +37,7 @@ * * Advanced settings can be found in Configuration_adv.h */ -#define CONFIGURATION_H_VERSION 020008 +#define CONFIGURATION_H_VERSION 02000903 //=========================================================================== //============================= Getting Started ============================= @@ -96,6 +96,11 @@ // @section machine +// Choose the name from boards.h that matches your setup +#ifndef MOTHERBOARD + #define MOTHERBOARD BOARD_RAMPS_14_EFB +#endif + /** * Select the serial port on the board to use for communication with the host. * This allows the connection of wireless adapters (for instance) to non-default port pins. @@ -113,8 +118,19 @@ */ #define SERIAL_PORT_2 0 +/** + * Select a third serial port on the board to use for communication with the host. + * Currently only supported for AVR, DUE, LPC1768/9 and STM32/STM32F1 + * :[-1, 0, 1, 2, 3, 4, 5, 6, 7] + */ +//#define SERIAL_PORT_3 1 + /** * This setting determines the communication speed of the printer. + + * Serial Port Baud Rate + * This is the default communication speed for all serial ports. + * Set the baud rate defaults for additional serial ports below. * * 250000 works in most cases, but you might try a lower speed if * you commonly experience drop-outs during host printing. @@ -123,6 +139,9 @@ * :[2400, 9600, 19200, 38400, 57600, 115200, 250000, 500000, 1000000] */ #define BAUDRATE 115200 +//#define BAUD_RATE_GCODE // Enable G-code M575 to set the baud rate +//#define BAUDRATE_2 250000 // Enable to override BAUDRATE +//#define BAUDRATE_3 250000 // Enable to override BAUDRATE // Enable the Bluetooth serial interface on AT90USB devices //#define BLUETOOTH @@ -139,6 +158,45 @@ // Choose your own or use a service like https://www.uuidgenerator.net/version4 //#define MACHINE_UUID "00000000-0000-0000-0000-000000000000" +/** + * Define the number of coordinated linear axes. + * See https://github.com/DerAndere1/Marlin/wiki + * Each linear axis gets its own stepper control and endstop: + * + * Steppers: *_STEP_PIN, *_ENABLE_PIN, *_DIR_PIN, *_ENABLE_ON + * Endstops: *_STOP_PIN, USE_*MIN_PLUG, USE_*MAX_PLUG + * Axes: *_MIN_POS, *_MAX_POS, INVERT_*_DIR + * Planner: DEFAULT_AXIS_STEPS_PER_UNIT, DEFAULT_MAX_FEEDRATE + * DEFAULT_MAX_ACCELERATION, AXIS_RELATIVE_MODES, + * MICROSTEP_MODES, MANUAL_FEEDRATE + * + * :[3, 4, 5, 6] + */ +//#define LINEAR_AXES 3 + +/** + * Axis codes for additional axes: + * This defines the axis code that is used in G-code commands to + * reference a specific axis. + * 'A' for rotational axis parallel to X + * 'B' for rotational axis parallel to Y + * 'C' for rotational axis parallel to Z + * 'U' for secondary linear axis parallel to X + * 'V' for secondary linear axis parallel to Y + * 'W' for secondary linear axis parallel to Z + * Regardless of the settings, firmware-internal axis IDs are + * I (AXIS4), J (AXIS5), K (AXIS6). + */ +#if LINEAR_AXES >= 4 + #define AXIS4_NAME 'A' // :['A', 'B', 'C', 'U', 'V', 'W'] +#endif +#if LINEAR_AXES >= 5 + #define AXIS5_NAME 'B' // :['A', 'B', 'C', 'U', 'V', 'W'] +#endif +#if LINEAR_AXES >= 6 + #define AXIS6_NAME 'C' // :['A', 'B', 'C', 'U', 'V', 'W'] +#endif + // @section extruder // This defines the number of extruders @@ -165,11 +223,11 @@ * Multi-Material Unit * Set to one of these predefined models: * - * PRUSA_MMU1 : Průša MMU1 (The "multiplexer" version) - * PRUSA_MMU2 : Průša MMU2 - * PRUSA_MMU2S : Průša MMU2S (Requires MK3S extruder with motion sensor, EXTRUDERS = 5) - * SMUFF_EMU_MMU2 : Technik Gegg SMuFF (Průša MMU2 emulation mode) - * SMUFF_EMU_MMU2S : Technik Gegg SMuFF (Průša MMU2S emulation mode) + * PRUSA_MMU1 : Průša MMU1 (The "multiplexer" version) + * PRUSA_MMU2 : Průša MMU2 + * PRUSA_MMU2S : Průša MMU2S (Requires MK3S extruder with motion sensor, EXTRUDERS = 5) + * EXTENDABLE_EMU_MMU2 : MMU with configurable number of filaments (ERCF, SMuFF or similar with Průša MMU2 compatible firmware) + * EXTENDABLE_EMU_MMU2S : MMUS with configurable number of filaments (ERCF, SMuFF or similar with Průša MMU2 compatible firmware) * * Requires NOZZLE_PARK_FEATURE to park print head in case MMU unit fails. * See additional options in Configuration_adv.h. @@ -214,7 +272,6 @@ #define PARKING_EXTRUDER_PARKING_X { -78, 184 } // X positions for parking the extruders #define PARKING_EXTRUDER_GRAB_DISTANCE 1 // (mm) Distance to move beyond the parking point to grab the extruder - //#define MANUAL_SOLENOID_CONTROL // Manual control of docking solenoids with M380 S / M381 #if ENABLED(PARKING_EXTRUDER) @@ -296,6 +353,7 @@ #define MIXING_VIRTUAL_TOOLS 16 // Use the Virtual Tool method with M163 and M164 //#define DIRECT_MIXING_IN_G1 // Allow ABCDHI mix factors in G1 movement commands //#define GRADIENT_MIX // Support for gradient mixing with M166 and LCD + //#define MIXING_PRESETS // Assign 8 default V-tool presets for 2 or 3 MIXING_STEPPERS #if ENABLED(GRADIENT_MIX) //#define GRADIENT_VTOOL // Add M166 T to use a V-tool index as a Gradient alias #endif @@ -320,10 +378,17 @@ //#define PSU_NAME "Power Supply" #if ENABLED(PSU_CONTROL) + //#define MKS_PWC // Using the MKS PWC add-on + //#define PS_OFF_CONFIRM // Confirm dialog when power off + //#define PS_OFF_SOUND // Beep 1s when power off #define PSU_ACTIVE_STATE LOW // Set 'LOW' for ATX, 'HIGH' for X-Box - //#define PSU_DEFAULT_OFF // Keep power off until enabled directly with M80 - //#define PSU_POWERUP_DELAY 250 // (ms) Delay for the PSU to warm up to full power + //#define PSU_DEFAULT_OFF // Keep power off until enabled directly with M80 + //#define PSU_POWERUP_DELAY 250 // (ms) Delay for the PSU to warm up to full power + //#define LED_POWEROFF_TIMEOUT 10000 // (ms) Turn off LEDs after power-off, with this amount of delay + + //#define POWER_OFF_TIMER // Enable M81 D to power off after a delay + //#define POWER_OFF_WAIT_FOR_COOLDOWN // Enable M81 S to power off only after cooldown //#define PSU_POWERUP_GCODE "M355 S1" // G-code to run after power-on (e.g., case light on) //#define PSU_POWEROFF_GCODE "M355 S0" // G-code to run before power-off (e.g., case light off) @@ -334,11 +399,15 @@ #define AUTO_POWER_E_FANS #define AUTO_POWER_CONTROLLERFAN #define AUTO_POWER_CHAMBER_FAN - //#define AUTO_POWER_E_TEMP 50 // (°C) Turn on PSU if any extruder is over this temperature - //#define AUTO_POWER_CHAMBER_TEMP 30 // (°C) Turn on PSU if the chamber is over this temperature + #define AUTO_POWER_COOLER_FAN #define POWER_TIMEOUT 30 // (s) Turn off power if the machine is idle for this duration //#define POWER_OFF_DELAY 60 // (s) Delay of poweroff after M81 command. Useful to let fans run for extra time. #endif + #if EITHER(AUTO_POWER_CONTROL, POWER_OFF_WAIT_FOR_COOLDOWN) + //#define AUTO_POWER_E_TEMP 50 // (°C) PSU on if any extruder is over this temperature + //#define AUTO_POWER_CHAMBER_TEMP 30 // (°C) PSU on if the chamber is over this temperature + //#define AUTO_POWER_COOLER_TEMP 26 // (°C) PSU on if the cooler is over this temperature + #endif #endif //=========================================================================== @@ -347,70 +416,96 @@ // @section temperature /** - * --NORMAL IS 4.7kohm PULLUP!-- 1kohm pullup can be used on hotend sensor, using correct resistor and table + * --NORMAL IS 4.7kΩ PULLUP!-- 1kΩ pullup can be used on hotend sensor, using correct resistor and table * * Temperature sensors available: * - * -5 : PT100 / PT1000 with MAX31865 (only for sensors 0-1) - * -3 : thermocouple with MAX31855 (only for sensors 0-1) - * -2 : thermocouple with MAX6675 (only for sensors 0-1) - * -4 : thermocouple with AD8495 - * -1 : thermocouple with AD595 + * SPI RTD/Thermocouple Boards - IMPORTANT: Read the NOTE below! + * ------- + * -5 : MAX31865 with Pt100/Pt1000, 2, 3, or 4-wire (only for sensors 0-1) + * NOTE: You must uncomment/set the MAX31865_*_OHMS_n defines below. + * -3 : MAX31855 with Thermocouple, -200°C to +700°C (only for sensors 0-1) + * -2 : MAX6675 with Thermocouple, 0°C to +700°C (only for sensors 0-1) + * + * NOTE: Ensure TEMP_n_CS_PIN is set in your pins file for each TEMP_SENSOR_n using an SPI Thermocouple. By default, + * Hardware SPI on the default serial bus is used. If you have also set TEMP_n_SCK_PIN and TEMP_n_MISO_PIN, + * Software SPI will be used on those ports instead. You can force Hardware SPI on the default bus in the + * Configuration_adv.h file. At this time, separate Hardware SPI buses for sensors are not supported. + * + * Analog Themocouple Boards + * ------- + * -4 : AD8495 with Thermocouple + * -1 : AD595 with Thermocouple + * + * Analog Thermistors - 4.7kΩ pullup - Normal + * ------- + * 1 : 100kΩ EPCOS - Best choice for EPCOS thermistors + * 331 : 100kΩ Same as #1, but 3.3V scaled for MEGA + * 332 : 100kΩ Same as #1, but 3.3V scaled for DUE + * 2 : 200kΩ ATC Semitec 204GT-2 + * 202 : 200kΩ Copymaster 3D + * 3 : ???Ω Mendel-parts thermistor + * 4 : 10kΩ Generic Thermistor !! DO NOT use for a hotend - it gives bad resolution at high temp. !! + * 5 : 100kΩ ATC Semitec 104GT-2/104NT-4-R025H42G - Used in ParCan, J-Head, and E3D, SliceEngineering 300°C + * 501 : 100kΩ Zonestar - Tronxy X3A + * 502 : 100kΩ Zonestar - used by hot bed in Zonestar Průša P802M + * 503 : 100kΩ Zonestar (Z8XM2) Heated Bed thermistor + * 504 : 100kΩ Zonestar P802QR2 (Part# QWG-104F-B3950) Hotend Thermistor + * 505 : 100kΩ Zonestar P802QR2 (Part# QWG-104F-3950) Bed Thermistor + * 512 : 100kΩ RPW-Ultra hotend + * 6 : 100kΩ EPCOS - Not as accurate as table #1 (created using a fluke thermocouple) + * 7 : 100kΩ Honeywell 135-104LAG-J01 + * 71 : 100kΩ Honeywell 135-104LAF-J01 + * 8 : 100kΩ Vishay 0603 SMD NTCS0603E3104FXT + * 9 : 100kΩ GE Sensing AL03006-58.2K-97-G1 + * 10 : 100kΩ RS PRO 198-961 + * 11 : 100kΩ Keenovo AC silicone mats, most Wanhao i3 machines - beta 3950, 1% + * 12 : 100kΩ Vishay 0603 SMD NTCS0603E3104FXT (#8) - calibrated for Makibox hot bed + * 13 : 100kΩ Hisens up to 300°C - for "Simple ONE" & "All In ONE" hotend - beta 3950, 1% + * 15 : 100kΩ Calibrated for JGAurora A5 hotend + * 18 : 200kΩ ATC Semitec 204GT-2 Dagoma.Fr - MKS_Base_DKU001327 + * 22 : 100kΩ GTM32 Pro vB - hotend - 4.7kΩ pullup to 3.3V and 220Ω to analog input + * 23 : 100kΩ GTM32 Pro vB - bed - 4.7kΩ pullup to 3.3v and 220Ω to analog input + * 30 : 100kΩ Kis3d Silicone heating mat 200W/300W with 6mm precision cast plate (EN AW 5083) NTC100K - beta 3950 + * 60 : 100kΩ Maker's Tool Works Kapton Bed Thermistor - beta 3950 + * 61 : 100kΩ Formbot/Vivedino 350°C Thermistor - beta 3950 + * 66 : 4.7MΩ Dyze Design High Temperature Thermistor + * 67 : 500kΩ SliceEngineering 450°C Thermistor + * 70 : 100kΩ bq Hephestos 2 + * 75 : 100kΩ Generic Silicon Heat Pad with NTC100K MGB18-104F39050L32 + * 2000 : 100kΩ Ultimachine Rambo TDK NTCG104LH104KT1 NTC100K motherboard Thermistor + * + * Analog Thermistors - 1kΩ pullup - Atypical, and requires changing out the 4.7kΩ pullup for 1kΩ. + * ------- (but gives greater accuracy and more stable PID) + * 51 : 100kΩ EPCOS (1kΩ pullup) + * 52 : 200kΩ ATC Semitec 204GT-2 (1kΩ pullup) + * 55 : 100kΩ ATC Semitec 104GT-2 - Used in ParCan & J-Head (1kΩ pullup) + * + * Analog Thermistors - 10kΩ pullup - Atypical + * ------- + * 99 : 100kΩ Found on some Wanhao i3 machines with a 10kΩ pull-up resistor + * + * Analog RTDs (Pt100/Pt1000) + * ------- + * 110 : Pt100 with 1kΩ pullup (atypical) + * 147 : Pt100 with 4.7kΩ pullup + * 1010 : Pt1000 with 1kΩ pullup (atypical) + * 1047 : Pt1000 with 4.7kΩ pullup (E3D) + * 20 : Pt100 with circuit in the Ultimainboard V2.x with mainboard ADC reference voltage = INA826 amplifier-board supply voltage. + * NOTE: (1) Must use an ADC input with no pullup. (2) Some INA826 amplifiers are unreliable at 3.3V so consider using sensor 147, 110, or 21. + * 21 : Pt100 with circuit in the Ultimainboard V2.x with 3.3v ADC reference voltage (STM32, LPC176x....) and 5V INA826 amplifier board supply. + * NOTE: ADC pins are not 5V tolerant. Not recommended because it's possible to damage the CPU by going over 500°C. + * 201 : Pt100 with circuit in Overlord, similar to Ultimainboard V2.x + * + * Custom/Dummy/Other Thermal Sensors + * ------ * 0 : not used - * 1 : 100k thermistor - best choice for EPCOS 100k (4.7k pullup) - * 331 : (3.3V scaled thermistor 1 table for MEGA) - * 332 : (3.3V scaled thermistor 1 table for DUE) - * 2 : 200k thermistor - ATC Semitec 204GT-2 (4.7k pullup) - * 202 : 200k thermistor - Copymaster 3D - * 3 : Mendel-parts thermistor (4.7k pullup) - * 4 : 10k thermistor !! do not use it for a hotend. It gives bad resolution at high temp. !! - * 5 : 100K thermistor - ATC Semitec 104GT-2/104NT-4-R025H42G (Used in ParCan, J-Head, and E3D) (4.7k pullup) - * 501 : 100K Zonestar (Tronxy X3A) Thermistor - * 502 : 100K Zonestar Thermistor used by hot bed in Zonestar Průša P802M - * 512 : 100k RPW-Ultra hotend thermistor (4.7k pullup) - * 6 : 100k EPCOS - Not as accurate as table 1 (created using a fluke thermocouple) (4.7k pullup) - * 7 : 100k Honeywell thermistor 135-104LAG-J01 (4.7k pullup) - * 71 : 100k Honeywell thermistor 135-104LAF-J01 (4.7k pullup) - * 8 : 100k 0603 SMD Vishay NTCS0603E3104FXT (4.7k pullup) - * 9 : 100k GE Sensing AL03006-58.2K-97-G1 (4.7k pullup) - * 10 : 100k RS thermistor 198-961 (4.7k pullup) - * 11 : 100k beta 3950 1% thermistor (Used in Keenovo AC silicone mats and most Wanhao i3 machines) (4.7k pullup) - * 12 : 100k 0603 SMD Vishay NTCS0603E3104FXT (4.7k pullup) (calibrated for Makibox hot bed) - * 13 : 100k Hisens 3950 1% up to 300°C for hotend "Simple ONE " & "Hotend "All In ONE" - * 15 : 100k thermistor calibration for JGAurora A5 hotend - * 18 : ATC Semitec 204GT-2 (4.7k pullup) Dagoma.Fr - MKS_Base_DKU001327 - * 20 : Pt100 with circuit in the Ultimainboard V2.x with mainboard ADC reference voltage = INA826 amplifier-board supply voltage. - * NOTES: (1) Must use an ADC input with no pullup. (2) Some INA826 amplifiers are unreliable at 3.3V so consider using sensor 147, 110, or 21. - * 21 : Pt100 with circuit in the Ultimainboard V2.x with 3.3v ADC reference voltage (STM32, LPC176x....) and 5V INA826 amplifier board supply. - * NOTE: ADC pins are not 5V tolerant. Not recommended because it's possible to damage the CPU by going over 500°C. - * 22 : 100k (hotend) with 4.7k pullup to 3.3V and 220R to analog input (as in GTM32 Pro vB) - * 23 : 100k (bed) with 4.7k pullup to 3.3v and 220R to analog input (as in GTM32 Pro vB) - * 30 : Kis3d Silicone heating mat 200W/300W with 6mm precision cast plate (EN AW 5083) NTC100K / B3950 (4.7k pullup) - * 201 : Pt100 with circuit in Overlord, similar to Ultimainboard V2.x - * 60 : 100k Maker's Tool Works Kapton Bed Thermistor beta=3950 - * 61 : 100k Formbot / Vivedino 3950 350C thermistor 4.7k pullup - * 66 : 4.7M High Temperature thermistor from Dyze Design - * 67 : 450C thermistor from SliceEngineering - * 70 : the 100K thermistor found in the bq Hephestos 2 - * 75 : 100k Generic Silicon Heat Pad with NTC 100K MGB18-104F39050L32 thermistor - * 99 : 100k thermistor with a 10K pull-up resistor (found on some Wanhao i3 machines) - * - * 1k ohm pullup tables - This is atypical, and requires changing out the 4.7k pullup for 1k. - * (but gives greater accuracy and more stable PID) - * 51 : 100k thermistor - EPCOS (1k pullup) - * 52 : 200k thermistor - ATC Semitec 204GT-2 (1k pullup) - * 55 : 100k thermistor - ATC Semitec 104GT-2 (Used in ParCan & J-Head) (1k pullup) - * - * 1047 : Pt1000 with 4k7 pullup (E3D) - * 1010 : Pt1000 with 1k pullup (non standard) - * 147 : Pt100 with 4k7 pullup - * 110 : Pt100 with 1k pullup (non standard) - * * 1000 : Custom - Specify parameters in Configuration_adv.h * - * Use these for Testing or Development purposes. NEVER for production machine. + * !!! Use these for Testing or Development purposes. NEVER for production machine. !!! * 998 : Dummy Table that ALWAYS reads 25°C or the temperature defined below. * 999 : Dummy Table that ALWAYS reads 100°C or the temperature defined below. + * */ #define TEMP_SENSOR_0 1 #define TEMP_SENSOR_1 0 @@ -423,6 +518,9 @@ #define TEMP_SENSOR_BED 1 #define TEMP_SENSOR_PROBE 0 #define TEMP_SENSOR_CHAMBER 0 +#define TEMP_SENSOR_COOLER 0 +#define TEMP_SENSOR_BOARD 0 +#define TEMP_SENSOR_REDUNDANT 0 // Dummy thermistor constant temperature readings, for use with 998 and 999 #define DUMMY_THERMISTOR_998_VALUE 25 @@ -430,26 +528,37 @@ // Resistor values when using MAX31865 sensors (-5) on TEMP_SENSOR_0 / 1 //#define MAX31865_SENSOR_OHMS_0 100 // (Ω) Typically 100 or 1000 (PT100 or PT1000) -//#define MAX31865_CALIBRATION_OHMS_0 430 // (Ω) Typically 430 for AdaFruit PT100; 4300 for AdaFruit PT1000 +//#define MAX31865_CALIBRATION_OHMS_0 430 // (Ω) Typically 430 for Adafruit PT100; 4300 for Adafruit PT1000 //#define MAX31865_SENSOR_OHMS_1 100 //#define MAX31865_CALIBRATION_OHMS_1 430 -// Use temp sensor 1 as a redundant sensor with sensor 0. If the readings -// from the two sensors differ too much the print will be aborted. -//#define TEMP_SENSOR_1_AS_REDUNDANT -#define MAX_REDUNDANT_TEMP_SENSOR_DIFF 10 +#define TEMP_RESIDENCY_TIME 10 // (seconds) Time to wait for hotend to "settle" in M109 +#define TEMP_WINDOW 1 // (°C) Temperature proximity for the "temperature reached" timer +#define TEMP_HYSTERESIS 3 // (°C) Temperature proximity considered "close enough" to the target -#define TEMP_RESIDENCY_TIME 10 // (seconds) Time to wait for hotend to "settle" in M109 -#define TEMP_WINDOW 1 // (°C) Temperature proximity for the "temperature reached" timer -#define TEMP_HYSTERESIS 3 // (°C) Temperature proximity considered "close enough" to the target - -#define TEMP_BED_RESIDENCY_TIME 10 // (seconds) Time to wait for bed to "settle" in M190 -#define TEMP_BED_WINDOW 1 // (°C) Temperature proximity for the "temperature reached" timer -#define TEMP_BED_HYSTERESIS 3 // (°C) Temperature proximity considered "close enough" to the target +#define TEMP_BED_RESIDENCY_TIME 10 // (seconds) Time to wait for bed to "settle" in M190 +#define TEMP_BED_WINDOW 1 // (°C) Temperature proximity for the "temperature reached" timer +#define TEMP_BED_HYSTERESIS 3 // (°C) Temperature proximity considered "close enough" to the target #define TEMP_CHAMBER_RESIDENCY_TIME 10 // (seconds) Time to wait for chamber to "settle" in M191 -#define TEMP_CHAMBER_WINDOW 1 // (°C) Temperature proximity for the "temperature reached" timer -#define TEMP_CHAMBER_HYSTERESIS 3 // (°C) Temperature proximity considered "close enough" to the target +#define TEMP_CHAMBER_WINDOW 1 // (°C) Temperature proximity for the "temperature reached" timer +#define TEMP_CHAMBER_HYSTERESIS 3 // (°C) Temperature proximity considered "close enough" to the target + +/** + * Redundant Temperature Sensor (TEMP_SENSOR_REDUNDANT) + * + * Use a temp sensor as a redundant sensor for another reading. Select an unused temperature sensor, and another + * sensor you'd like it to be redundant for. If the two thermistors differ by TEMP_SENSOR_REDUNDANT_MAX_DIFF (°C), + * the print will be aborted. Whichever sensor is selected will have its normal functions disabled; i.e. selecting + * the Bed sensor (-1) will disable bed heating/monitoring. + * + * For selecting source/target use: COOLER, PROBE, BOARD, CHAMBER, BED, E0, E1, E2, E3, E4, E5, E6, E7 + */ +#if TEMP_SENSOR_REDUNDANT + #define TEMP_SENSOR_REDUNDANT_SOURCE E1 // The sensor that will provide the redundant reading. + #define TEMP_SENSOR_REDUNDANT_TARGET E0 // The sensor that we are providing a redundant reading for. + #define TEMP_SENSOR_REDUNDANT_MAX_DIFF 10 // (°C) Temperature difference that will trigger a print abort. +#endif // Below this temperature the heater will be switched off // because it probably indicates a broken thermistor wire. @@ -478,6 +587,16 @@ #define BED_MAXTEMP 125 #define CHAMBER_MAXTEMP 60 +/** + * Thermal Overshoot + * During heatup (and printing) the temperature can often "overshoot" the target by many degrees + * (especially before PID tuning). Setting the target temperature too close to MAXTEMP guarantees + * a MAXTEMP shutdown! Use these values to forbid temperatures being set too close to MAXTEMP. + */ +#define HOTEND_OVERSHOOT 15 // (°C) Forbid temperatures over MAXTEMP - OVERSHOOT +#define BED_OVERSHOOT 10 // (°C) Forbid temperatures over MAXTEMP - OVERSHOOT +#define COOLER_OVERSHOOT 2 // (°C) Forbid temperatures closer than OVERSHOOT + //=========================================================================== //============================= PID Settings ================================ //=========================================================================== @@ -497,11 +616,11 @@ // Creality Ender-3 #if ENABLED(PID_PARAMS_PER_HOTEND) - // Specify between 1 and HOTENDS values per array. - // If fewer than EXTRUDER values are provided, the last element will be repeated. - #define DEFAULT_Kp_LIST { 21.73, 21.73 } - #define DEFAULT_Ki_LIST { 1.54, 1.54 } - #define DEFAULT_Kd_LIST { 76.55, 76.55 } + // Specify up to one value per hotend here, according to your setup. + // If there are fewer values, the last one applies to the remaining hotends. + #define DEFAULT_Kp_LIST { 22.20, 22.20 } + #define DEFAULT_Ki_LIST { 1.08, 1.08 } + #define DEFAULT_Kd_LIST { 114.00, 114.00 } #else #define DEFAULT_Kp 21.73 #define DEFAULT_Ki 1.54 @@ -640,6 +759,7 @@ #define THERMAL_PROTECTION_HOTENDS // Enable thermal protection for all extruders #define THERMAL_PROTECTION_BED // Enable thermal protection for the heated bed #define THERMAL_PROTECTION_CHAMBER // Enable thermal protection for the heated chamber +#define THERMAL_PROTECTION_COOLER // Enable thermal protection for the laser cooling //=========================================================================== //============================= Mechanical Settings ========================= @@ -656,6 +776,17 @@ //#define COREZX //#define COREZY //#define MARKFORGED_XY // MarkForged. See https://reprap.org/forum/read.php?152,504042 +//#define MARKFORGED_YX + +// Enable for a belt style printer with endless "Z" motion +//#define BELTPRINTER + +// Enable for Polargraph Kinematics +//#define POLARGRAPH +#if ENABLED(POLARGRAPH) + #define POLARGRAPH_MAX_BELT_LEN 1035.0 + #define POLAR_SEGMENTS_PER_SECOND 5 +#endif //=========================================================================== //============================== Endstop Settings =========================== @@ -669,20 +800,32 @@ #define USE_XMIN_PLUG #define USE_YMIN_PLUG #define USE_ZMIN_PLUG +//#define USE_IMIN_PLUG +//#define USE_JMIN_PLUG +//#define USE_KMIN_PLUG //#define USE_XMAX_PLUG //#define USE_YMAX_PLUG //#define USE_ZMAX_PLUG +//#define USE_IMAX_PLUG +//#define USE_JMAX_PLUG +//#define USE_KMAX_PLUG // Enable pullup for all endstops to prevent a floating state #define ENDSTOPPULLUPS #if DISABLED(ENDSTOPPULLUPS) // Disable ENDSTOPPULLUPS to set pullups individually - //#define ENDSTOPPULLUP_XMAX - //#define ENDSTOPPULLUP_YMAX - //#define ENDSTOPPULLUP_ZMAX //#define ENDSTOPPULLUP_XMIN //#define ENDSTOPPULLUP_YMIN //#define ENDSTOPPULLUP_ZMIN + //#define ENDSTOPPULLUP_IMIN + //#define ENDSTOPPULLUP_JMIN + //#define ENDSTOPPULLUP_KMIN + //#define ENDSTOPPULLUP_XMAX + //#define ENDSTOPPULLUP_YMAX + //#define ENDSTOPPULLUP_ZMAX + //#define ENDSTOPPULLUP_IMAX + //#define ENDSTOPPULLUP_JMAX + //#define ENDSTOPPULLUP_KMAX //#define ENDSTOPPULLUP_ZMIN_PROBE #endif @@ -690,12 +833,18 @@ //#define ENDSTOPPULLDOWNS #if DISABLED(ENDSTOPPULLDOWNS) // Disable ENDSTOPPULLDOWNS to set pulldowns individually - //#define ENDSTOPPULLDOWN_XMAX - //#define ENDSTOPPULLDOWN_YMAX - //#define ENDSTOPPULLDOWN_ZMAX //#define ENDSTOPPULLDOWN_XMIN //#define ENDSTOPPULLDOWN_YMIN //#define ENDSTOPPULLDOWN_ZMIN + //#define ENDSTOPPULLDOWN_IMIN + //#define ENDSTOPPULLDOWN_JMIN + //#define ENDSTOPPULLDOWN_KMIN + //#define ENDSTOPPULLDOWN_XMAX + //#define ENDSTOPPULLDOWN_YMAX + //#define ENDSTOPPULLDOWN_ZMAX + //#define ENDSTOPPULLDOWN_IMAX + //#define ENDSTOPPULLDOWN_JMAX + //#define ENDSTOPPULLDOWN_KMAX #define ENDSTOPPULLDOWN_ZMIN_PROBE #endif @@ -703,9 +852,15 @@ #define X_MIN_ENDSTOP_INVERTING false // Set to true to invert the logic of the endstop. #define Y_MIN_ENDSTOP_INVERTING false // Set to true to invert the logic of the endstop. #define Z_MIN_ENDSTOP_INVERTING false // Set to true to invert the logic of the endstop. +#define I_MIN_ENDSTOP_INVERTING false // Set to true to invert the logic of the endstop. +#define J_MIN_ENDSTOP_INVERTING false // Set to true to invert the logic of the endstop. +#define K_MIN_ENDSTOP_INVERTING false // Set to true to invert the logic of the endstop. #define X_MAX_ENDSTOP_INVERTING false // Set to true to invert the logic of the endstop. #define Y_MAX_ENDSTOP_INVERTING false // Set to true to invert the logic of the endstop. #define Z_MAX_ENDSTOP_INVERTING false // Set to true to invert the logic of the endstop. +#define I_MAX_ENDSTOP_INVERTING false // Set to true to invert the logic of the endstop. +#define J_MAX_ENDSTOP_INVERTING false // Set to true to invert the logic of the endstop. +#define K_MAX_ENDSTOP_INVERTING false // Set to true to invert the logic of the endstop. #define Z_MIN_PROBE_ENDSTOP_INVERTING true // Set to true to invert the logic of the probe. /** @@ -734,8 +889,11 @@ //#define Z2_DRIVER_TYPE A4988 //#define Z3_DRIVER_TYPE A4988 //#define Z4_DRIVER_TYPE A4988 +//#define I_DRIVER_TYPE A4988 +//#define J_DRIVER_TYPE A4988 +//#define K_DRIVER_TYPE A4988 #define E0_DRIVER_TYPE TMC2209 -// #define E1_DRIVER_TYPE TMC2208 +//#define E1_DRIVER_TYPE A4988 //#define E2_DRIVER_TYPE A4988 //#define E3_DRIVER_TYPE A4988 //#define E4_DRIVER_TYPE A4988 @@ -787,14 +945,14 @@ /** * Default Axis Steps Per Unit (steps/mm) * Override with M92 - * X, Y, Z, E0 [, E1[, E2...]] + * X, Y, Z [, I [, J [, K]]], E0 [, E1[, E2...]] */ #define DEFAULT_AXIS_STEPS_PER_UNIT { 80, 80, 400, 138 } /** * Default Max Feed Rate (mm/s) * Override with M203 - * X, Y, Z, E0 [, E1[, E2...]] + * X, Y, Z [, I [, J [, K]]], E0 [, E1[, E2...]] */ #define DEFAULT_MAX_FEEDRATE { 500, 500, 5, 25 } @@ -807,7 +965,7 @@ * Default Max Acceleration (change/s) change = mm/s * (Maximum start speed for accelerated moves) * Override with M201 - * X, Y, Z, E0 [, E1[, E2...]] + * X, Y, Z [, I [, J [, K]]], E0 [, E1[, E2...]] */ #define DEFAULT_MAX_ACCELERATION { 500, 500, 100, 5000 } @@ -841,6 +999,9 @@ #define DEFAULT_XJERK 10.0 #define DEFAULT_YJERK 10.0 #define DEFAULT_ZJERK 0.3 + //#define DEFAULT_IJERK 0.3 + //#define DEFAULT_JJERK 0.3 + //#define DEFAULT_KJERK 0.3 //#define TRAVEL_EXTRA_XYJERK 0.0 // Additional jerk allowance for all travel moves @@ -924,7 +1085,6 @@ * or (with LCD_BED_LEVELING) the LCD controller. */ //#define PROBE_MANUALLY -//#define MANUAL_PROBE_START_Z 0.2 /** * A Fix-Mounted Probe either doesn't deploy or needs manual deployment. @@ -955,6 +1115,17 @@ */ #define BLTOUCH +/** + * MagLev V4 probe by MDD + * + * This probe is deployed and activated by powering a built-in electromagnet. + */ +//#define MAGLEV4 +#if ENABLED(MAGLEV4) + //#define MAGLEV_TRIGGER_PIN 11 // Set to the connected digital output + #define MAGLEV_TRIGGER_DELAY 15 // Changing this risks overheating the coil +#endif + /** * Touch-MI Probe by hotends.fr * @@ -987,7 +1158,7 @@ #endif // Duet Smart Effector (for delta printers) - https://bit.ly/2ul5U7J -// When the pin is defined you can use M672 to set/reset the probe sensivity. +// When the pin is defined you can use M672 to set/reset the probe sensitivity. //#define DUET_SMART_EFFECTOR #if ENABLED(DUET_SMART_EFFECTOR) #define SMART_EFFECTOR_MOD_PIN -1 // Connect a GPIO pin to the Smart Effector MOD pin @@ -1087,6 +1258,15 @@ #endif #endif +/** + * Probe Enable / Disable + * The probe only provides a triggered signal when enabled. + */ +//#define PROBE_ENABLE_DISABLE +#if ENABLED(PROBE_ENABLE_DISABLE) + //#define PROBE_ENABLE_PIN -1 // Override the default pin here +#endif + /** * Multiple Probing * @@ -1146,7 +1326,8 @@ //#define WAIT_FOR_HOTEND // Wait for hotend to heat back up between probes (to improve accuracy & prevent cold extrude) #endif //#define PROBING_FANS_OFF // Turn fans off when probing -//#define PROBING_STEPPERS_OFF // Turn steppers off (unless needed to hold position) when probing +//#define PROBING_ESTEPPERS_OFF // Turn all extruder steppers off when probing +//#define PROBING_STEPPERS_OFF // Turn all steppers off (unless needed to hold position) when probing (including extruders) //#define DELAY_BEFORE_PROBING 200 // (ms) To prevent vibrations from triggering piezo sensors // Require minimum nozzle and/or bed temperature for probing @@ -1162,12 +1343,18 @@ #define Y_ENABLE_ON 0 #define Z_ENABLE_ON 0 #define E_ENABLE_ON 0 // For all extruders +//#define I_ENABLE_ON 0 +//#define J_ENABLE_ON 0 +//#define K_ENABLE_ON 0 // Disable axis steppers immediately when they're not being stepped. // WARNING: When motors turn off there is a chance of losing position accuracy! #define DISABLE_X false #define DISABLE_Y false #define DISABLE_Z false +//#define DISABLE_I false +//#define DISABLE_J false +//#define DISABLE_K false // Turn off the display blinking that warns about possible accuracy reduction //#define DISABLE_REDUCED_ACCURACY_WARNING @@ -1183,6 +1370,9 @@ #define INVERT_X_DIR true #define INVERT_Y_DIR true #define INVERT_Z_DIR false +//#define INVERT_I_DIR false +//#define INVERT_J_DIR false +//#define INVERT_K_DIR false // @section extruder @@ -1218,10 +1408,13 @@ #define X_HOME_DIR -1 #define Y_HOME_DIR -1 #define Z_HOME_DIR -1 +//#define I_HOME_DIR -1 +//#define J_HOME_DIR -1 +//#define K_HOME_DIR -1 // @section machine -// The size of the print bed +// The size of the printable area #define X_BED_SIZE 235 #define Y_BED_SIZE 235 @@ -1232,6 +1425,12 @@ #define X_MAX_POS X_BED_SIZE #define Y_MAX_POS Y_BED_SIZE #define Z_MAX_POS 250 +//#define I_MIN_POS 0 +//#define I_MAX_POS 50 +//#define J_MIN_POS 0 +//#define J_MAX_POS 50 +//#define K_MIN_POS 0 +//#define K_MAX_POS 50 /** * Software Endstops @@ -1248,6 +1447,9 @@ #define MIN_SOFTWARE_ENDSTOP_X #define MIN_SOFTWARE_ENDSTOP_Y #define MIN_SOFTWARE_ENDSTOP_Z + #define MIN_SOFTWARE_ENDSTOP_I + #define MIN_SOFTWARE_ENDSTOP_J + #define MIN_SOFTWARE_ENDSTOP_K #endif // Max software endstops constrain movement within maximum coordinate bounds @@ -1256,6 +1458,9 @@ #define MAX_SOFTWARE_ENDSTOP_X #define MAX_SOFTWARE_ENDSTOP_Y #define MAX_SOFTWARE_ENDSTOP_Z + #define MAX_SOFTWARE_ENDSTOP_I + #define MAX_SOFTWARE_ENDSTOP_J + #define MAX_SOFTWARE_ENDSTOP_K #endif #if EITHER(MIN_SOFTWARE_ENDSTOPS, MAX_SOFTWARE_ENDSTOPS) @@ -1405,6 +1610,11 @@ */ //#define DEBUG_LEVELING_FEATURE +#if ANY(MESH_BED_LEVELING, AUTO_BED_LEVELING_UBL, PROBE_MANUALLY) + // Set a height for the start of manual adjustment + #define MANUAL_PROBE_START_Z 0.2 // (mm) Comment out to use the last-measured height +#endif + #if ANY(MESH_BED_LEVELING, AUTO_BED_LEVELING_BILINEAR, AUTO_BED_LEVELING_UBL) // Gradually reduce leveling correction until a set height is reached, // at which point movement will be level to the machine's XY plane. @@ -1475,12 +1685,16 @@ #define GRID_MAX_POINTS_X 10 // Don't use more than 15 points per axis, implementation limited. #define GRID_MAX_POINTS_Y GRID_MAX_POINTS_X + //#define UBL_HILBERT_CURVE // Use Hilbert distribution for less travel when probing multiple points + #define UBL_MESH_EDIT_MOVES_Z // Sophisticated users prefer no movement of nozzle #define UBL_SAVE_ACTIVE_ON_M500 // Save the currently active mesh in the current slot on M500 //#define UBL_Z_RAISE_WHEN_OFF_MESH 2.5 // When the nozzle is off the mesh, this value is used // as the Z-Height correction value. + //#define UBL_MESH_WIZARD // Run several commands in a row to get a complete mesh + #elif ENABLED(MESH_BED_LEVELING) //=========================================================================== @@ -1558,16 +1772,17 @@ //#define MANUAL_X_HOME_POS 0 //#define MANUAL_Y_HOME_POS 0 //#define MANUAL_Z_HOME_POS 0 +//#define MANUAL_I_HOME_POS 0 +//#define MANUAL_J_HOME_POS 0 +//#define MANUAL_K_HOME_POS 0 -// Use "Z Safe Homing" to avoid homing with a Z probe outside the bed area. -// -// With this feature enabled: -// -// - Allow Z homing only after X and Y homing AND stepper drivers still enabled. -// - If stepper drivers time out, it will need X and Y homing again before Z homing. -// - Move the Z probe (or nozzle) to a defined XY point before Z Homing. -// - Prevent Z homing when the Z probe is outside bed area. -// +/** + * Use "Z Safe Homing" to avoid homing with a Z probe outside the bed area. + * + * - Moves the Z probe (or nozzle) to a defined XY point before Z homing. + * - Allows Z homing only when XY positions are known and trusted. + * - If stepper drivers sleep, XY homing may be required again before Z homing. + */ #define Z_SAFE_HOMING #if ENABLED(Z_SAFE_HOMING) @@ -1658,7 +1873,8 @@ #define EEPROM_CHITCHAT // Give feedback on EEPROM commands. Disable to save PROGMEM. #define EEPROM_BOOT_SILENT // Keep M503 quiet and only give errors during first load #if ENABLED(EEPROM_SETTINGS) - #define EEPROM_AUTO_INIT // Init EEPROM automatically on any errors. + //#define EEPROM_AUTO_INIT // Init EEPROM automatically on any errors. + //#define EEPROM_INIT_NOW // Init EEPROM on first boot after a new build. #endif // @@ -1713,8 +1929,7 @@ #if ENABLED(NOZZLE_PARK_FEATURE) // Specify a park position as { X, Y, Z_raise } #define NOZZLE_PARK_POINT { (X_MIN_POS + 10), (Y_MAX_POS - 10), 20 } - //#define NOZZLE_PARK_X_ONLY // X move only is required to park - //#define NOZZLE_PARK_Y_ONLY // Y move only is required to park + #define NOZZLE_PARK_MOVE 0 // Park motion: 0 = XY Move, 1 = X Only, 2 = Y Only, 3 = X before Y, 4 = Y before X #define NOZZLE_PARK_Z_RAISE_MIN 2 // (mm) Always raise Z by at least this distance #define NOZZLE_PARK_XY_FEEDRATE 100 // (mm/s) X and Y axes feedrate (also used for delta Z axis) #define NOZZLE_PARK_Z_FEEDRATE 5 // (mm/s) Z axis feedrate (not used for delta printers) @@ -1799,11 +2014,20 @@ /** * Print Job Timer * - * Automatically start and stop the print job timer on M104/M109/M190. + * Automatically start and stop the print job timer on M104/M109/M140/M190/M141/M191. + * The print job timer will only be stopped if the bed/chamber target temp is + * below BED_MINTEMP/CHAMBER_MINTEMP. * - * M104 (hotend, no wait) - high temp = none, low temp = stop timer - * M109 (hotend, wait) - high temp = start timer, low temp = stop timer - * M190 (bed, wait) - high temp = start timer, low temp = none + * M104 (hotend, no wait) - high temp = none, low temp = stop timer + * M109 (hotend, wait) - high temp = start timer, low temp = stop timer + * M140 (bed, no wait) - high temp = none, low temp = stop timer + * M190 (bed, wait) - high temp = start timer, low temp = none + * M141 (chamber, no wait) - high temp = none, low temp = stop timer + * M191 (chamber, wait) - high temp = start timer, low temp = none + * + * For M104/M109, high temp is anything over EXTRUDE_MINTEMP / 2. + * For M140/M190, high temp is anything over BED_MINTEMP. + * For M141/M191, high temp is anything over CHAMBER_MINTEMP. * * The timer can also be controlled with the following commands: * @@ -1872,10 +2096,10 @@ * * Select the language to display on the LCD. These languages are available: * - * en, an, bg, ca, cz, da, de, el, el_gr, es, eu, fi, fr, gl, hr, hu, it, + * en, an, bg, ca, cz, da, de, el, el_CY, es, eu, fi, fr, gl, hr, hu, it, * jp_kana, ko_KR, nl, pl, pt, pt_br, ro, ru, sk, sv, tr, uk, vi, zh_CN, zh_TW * - * :{ 'en':'English', 'an':'Aragonese', 'bg':'Bulgarian', 'ca':'Catalan', 'cz':'Czech', 'da':'Danish', 'de':'German', 'el':'Greek', 'el_gr':'Greek (Greece)', 'es':'Spanish', 'eu':'Basque-Euskera', 'fi':'Finnish', 'fr':'French', 'gl':'Galician', 'hr':'Croatian', 'hu':'Hungarian', 'it':'Italian', 'jp_kana':'Japanese', 'ko_KR':'Korean (South Korea)', 'nl':'Dutch', 'pl':'Polish', 'pt':'Portuguese', 'pt_br':'Portuguese (Brazilian)', 'ro':'Romanian', 'ru':'Russian', 'sk':'Slovak', 'sv':'Swedish', 'tr':'Turkish', 'uk':'Ukrainian', 'vi':'Vietnamese', 'zh_CN':'Chinese (Simplified)', 'zh_TW':'Chinese (Traditional)' } + * :{ 'en':'English', 'an':'Aragonese', 'bg':'Bulgarian', 'ca':'Catalan', 'cz':'Czech', 'da':'Danish', 'de':'German', 'el':'Greek (Greece)', 'el_CY':'Greek (Cyprus)', 'es':'Spanish', 'eu':'Basque-Euskera', 'fi':'Finnish', 'fr':'French', 'gl':'Galician', 'hr':'Croatian', 'hu':'Hungarian', 'it':'Italian', 'jp_kana':'Japanese', 'ko_KR':'Korean (South Korea)', 'nl':'Dutch', 'pl':'Polish', 'pt':'Portuguese', 'pt_br':'Portuguese (Brazilian)', 'ro':'Romanian', 'ru':'Russian', 'sk':'Slovak', 'sv':'Swedish', 'tr':'Turkish', 'uk':'Ukrainian', 'vi':'Vietnamese', 'zh_CN':'Chinese (Simplified)', 'zh_TW':'Chinese (Traditional)' } */ #define LCD_LANGUAGE en @@ -1986,7 +2210,8 @@ // // Add individual axis homing items (Home X, Home Y, and Home Z) to the LCD menu. // -#define INDIVIDUAL_AXIS_HOMING_MENU +//#define INDIVIDUAL_AXIS_HOMING_MENU +#define INDIVIDUAL_AXIS_HOMING_SUBMENU // // SPEAKER/BUZZER @@ -2177,6 +2402,11 @@ // //#define REPRAP_DISCOUNT_FULL_GRAPHIC_SMART_CONTROLLER +// +// K.3D Full Graphic Smart Controller +// +//#define K3D_FULL_GRAPHIC_SMART_CONTROLLER + // // ReprapWorld Graphical LCD // https://reprapworld.com/?products_details&products_id/1218 @@ -2191,6 +2421,11 @@ //#define VIKI2 //#define miniVIKI +// +// Alfawise Ex8 printer LCD marked as WYH L12864 COG +// +//#define WYH_L12864 + // // MakerLab Mini Panel with graphic // controller and SD support - https://reprap.org/wiki/Mini_panel @@ -2238,11 +2473,17 @@ // //#define MKS_MINI_12864 +// +// MKS MINI12864 V3 is an alias for FYSETC_MINI_12864_2_1. Type A/B. NeoPixel RGB Backlight. +// +//#define MKS_MINI_12864_V3 + // // MKS LCD12864A/B with graphic controller and SD support. Follows MKS_MINI_12864 pinout. // https://www.aliexpress.com/item/33018110072.html // -//#define MKS_LCD12864 +//#define MKS_LCD12864A +//#define MKS_LCD12864B // // FYSETC variant of the MINI12864 graphic controller with SD support @@ -2254,6 +2495,11 @@ //#define FYSETC_MINI_12864_2_1 // Type A/B. NeoPixel RGB Backlight //#define FYSETC_GENERIC_12864_1_1 // Larger display with basic ON/OFF backlight. +// +// BigTreeTech Mini 12864 V1.0 is an alias for FYSETC_MINI_12864_2_1. Type A/B. NeoPixel RGB Backlight. +// +//#define BTT_MINI_12864_V1 + // // Factory display for Creality CR-10 // https://www.aliexpress.com/item/32833148327.html @@ -2314,7 +2560,7 @@ //#define OLED_PANEL_TINYBOY2 // -// MKS OLED 1.3" 128×64 FULL GRAPHICS CONTROLLER +// MKS OLED 1.3" 128×64 Full Graphics Controller // https://reprap.org/wiki/MKS_12864OLED // // Tiny, but very sharp OLED display @@ -2323,7 +2569,7 @@ //#define MKS_12864OLED_SSD1306 // Uses the SSD1306 controller // -// Zonestar OLED 128×64 FULL GRAPHICS CONTROLLER +// Zonestar OLED 128×64 Full Graphics Controller // //#define ZONESTAR_12864LCD // Graphical (DOGM) with ST7920 controller //#define ZONESTAR_12864OLED // 1.3" OLED with SH1106 controller (default) @@ -2340,10 +2586,15 @@ //#define OVERLORD_OLED // -// FYSETC OLED 2.42" 128×64 FULL GRAPHICS CONTROLLER with WS2812 RGB +// FYSETC OLED 2.42" 128×64 Full Graphics Controller with WS2812 RGB // Where to find : https://www.aliexpress.com/item/4000345255731.html //#define FYSETC_242_OLED_12864 // Uses the SSD1309 controller +// +// K.3D SSD1309 OLED 2.42" 128×64 Full Graphics Controller +// +//#define K3D_242_OLED_CONTROLLER // Software SPI + //============================================================================= //========================== Extensible UI Displays =========================== //============================================================================= @@ -2352,11 +2603,42 @@ // DGUS Touch Display with DWIN OS. (Choose one.) // ORIGIN : https://www.aliexpress.com/item/32993409517.html // FYSETC : https://www.aliexpress.com/item/32961471929.html +// MKS : https://www.aliexpress.com/item/1005002008179262.html +// +// Flash display with DGUS Displays for Marlin: +// - Format the SD card to FAT32 with an allocation size of 4kb. +// - Download files as specified for your type of display. +// - Plug the microSD card into the back of the display. +// - Boot the display and wait for the update to complete. +// +// ORIGIN (Marlin DWIN_SET) +// - Download https://github.com/coldtobi/Marlin_DGUS_Resources +// - Copy the downloaded DWIN_SET folder to the SD card. +// +// FYSETC (Supplier default) +// - Download https://github.com/FYSETC/FYSTLCD-2.0 +// - Copy the downloaded SCREEN folder to the SD card. +// +// HIPRECY (Supplier default) +// - Download https://github.com/HiPrecy/Touch-Lcd-LEO +// - Copy the downloaded DWIN_SET folder to the SD card. +// +// MKS (MKS-H43) (Supplier default) +// - Download https://github.com/makerbase-mks/MKS-H43 +// - Copy the downloaded DWIN_SET folder to the SD card. +// +// RELOADED (T5UID1) +// - Download https://github.com/Desuuuu/DGUS-reloaded/releases +// - Copy the downloaded DWIN_SET folder to the SD card. // //#define DGUS_LCD_UI_ORIGIN //#define DGUS_LCD_UI_FYSETC //#define DGUS_LCD_UI_HIPRECY //#define DGUS_LCD_UI_MKS +//#define DGUS_LCD_UI_RELOADED +#if ENABLED(DGUS_LCD_UI_MKS) + #define USE_MKS_GREEN_UI +#endif // // CR-6 OEM touch screen. A DWIN display with touch. @@ -2387,6 +2669,14 @@ //#define ANYCUBIC_LCD_DEBUG #endif +// +// 320x240 Nextion 2.8" serial TFT Resistive Touch Screen NX3224T028 +// +//#define NEXTION_TFT +#if ENABLED(NEXTION_TFT) + #define LCD_SERIAL_PORT 1 // Default is 1 for Nextion +#endif + // // Third-party or vendor-customized controller interfaces. // Sources should be installed in 'src/lcd/extui'. @@ -2407,32 +2697,32 @@ */ // -// 480x320, 3.5", SPI Display From MKS -// Normally used in MKS Robin Nano V2 +// 480x320, 3.5", SPI Display with Rotary Encoder from MKS +// Usually paired with MKS Robin Nano V2 & V3 // //#define MKS_TS35_V2_0 // // 320x240, 2.4", FSMC Display From MKS -// Normally used in MKS Robin Nano V1.2 +// Usually paired with MKS Robin Nano V1.2 // //#define MKS_ROBIN_TFT24 // // 320x240, 2.8", FSMC Display From MKS -// Normally used in MKS Robin Nano V1.2 +// Usually paired with MKS Robin Nano V1.2 // //#define MKS_ROBIN_TFT28 // // 320x240, 3.2", FSMC Display From MKS -// Normally used in MKS Robin Nano V1.2 +// Usually paired with MKS Robin Nano V1.2 // //#define MKS_ROBIN_TFT32 // // 480x320, 3.5", FSMC Display From MKS -// Normally used in MKS Robin Nano V1.2 +// Usually paired with MKS Robin Nano V1.2 // //#define MKS_ROBIN_TFT35 @@ -2443,7 +2733,7 @@ // // 320x240, 3.2", FSMC Display From MKS -// Normally used in MKS Robin +// Usually paired with MKS Robin // //#define MKS_ROBIN_TFT_V1_1R @@ -2472,6 +2762,16 @@ // //#define ANET_ET5_TFT35 +// +// 1024x600, 7", RGB Stock Display with Rotary Encoder from BIQU-BX +// +//#define BIQU_BX_TFT70 + +// +// 480x320, 3.5", SPI Stock Display with Rotary Encoder from BIQU B1 SE Series +// +//#define BTT_TFT35_SPI_V1_0 + // // Generic TFT with detailed options // @@ -2488,6 +2788,7 @@ //#define TFT_RES_320x240 //#define TFT_RES_480x272 //#define TFT_RES_480x320 + //#define TFT_RES_1024x600 #endif /** @@ -2525,16 +2826,22 @@ // // Ender-3 v2 OEM display. A DWIN display with Rotary Encoder. // -//#define DWIN_CREALITY_LCD +//#define DWIN_CREALITY_LCD // Creality UI +//#define DWIN_CREALITY_LCD_ENHANCED // Enhanced UI +//#define DWIN_CREALITY_LCD_JYERSUI // Jyers UI by Jacob Myers +//#define DWIN_MARLINUI_PORTRAIT // MarlinUI (portrait orientation) +//#define DWIN_MARLINUI_LANDSCAPE // MarlinUI (landscape orientation) // -// ADS7843/XPT2046 ADC Touchscreen such as ILI9341 2.8 +// Touch Screen Settings // //#define TOUCH_SCREEN #if ENABLED(TOUCH_SCREEN) #define BUTTON_DELAY_EDIT 50 // (ms) Button repeat delay for edit screens #define BUTTON_DELAY_MENU 250 // (ms) Button repeat delay for menus + //#define TOUCH_IDLE_SLEEP 300 // (secs) Turn off the TFT backlight if set (5mn) + #define TOUCH_SCREEN_CALIBRATION //#define TOUCH_CALIBRATION_X 12316 @@ -2559,6 +2866,11 @@ //#define REPRAPWORLD_KEYPAD //#define REPRAPWORLD_KEYPAD_MOVE_STEP 10.0 // (mm) Distance to move per key-press +// +// EasyThreeD ET-4000+ with button input and status LED +// +//#define EASYTHREED_UI + //============================================================================= //=============================== Extra Features ============================== //============================================================================= @@ -2569,9 +2881,6 @@ // :[1,2,3,4,5,6,7,8] //#define NUM_M106_FANS 1 -// Increase the FAN PWM frequency. Removes the PWM noise but increases heating in the FET/Arduino -//#define FAST_PWM_FAN - // Use software PWM to drive the fan, as for the heaters. This uses a very low frequency // which is not as annoying as with the hardware PWM. On the other hand, if this frequency // is too low, you should also increment SOFT_PWM_SCALE. @@ -2642,7 +2951,7 @@ //#define NEOPIXEL_LED #if ENABLED(NEOPIXEL_LED) #define NEOPIXEL_TYPE NEO_GRBW // NEO_GRBW / NEO_GRB - four/three channel driver type (defined in Adafruit_NeoPixel.h) - #define NEOPIXEL_PIN 4 // LED driving pin + //#define NEOPIXEL_PIN 4 // LED driving pin //#define NEOPIXEL2_TYPE NEOPIXEL_TYPE //#define NEOPIXEL2_PIN 5 #define NEOPIXEL_PIXELS 30 // Number of LEDs in the strip. (Longest strip when NEOPIXEL2_SEPARATE is disabled.) @@ -2660,10 +2969,11 @@ //#define NEOPIXEL2_INSERIES // Default behavior is NeoPixel 2 in parallel #endif - // Use a single NeoPixel LED for static (background) lighting - //#define NEOPIXEL_BKGD_LED_INDEX 0 // Index of the LED to use - //#define NEOPIXEL_BKGD_COLOR { 255, 255, 255, 0 } // R, G, B, W - //#define NEOPIXEL_BKGD_ALWAYS_ON // Keep the backlight on when other NeoPixels are off + // Use some of the NeoPixel LEDs for static (background) lighting + //#define NEOPIXEL_BKGD_INDEX_FIRST 0 // Index of the first background LED + //#define NEOPIXEL_BKGD_INDEX_LAST 5 // Index of the last background LED + //#define NEOPIXEL_BKGD_COLOR { 255, 255, 255, 0 } // R, G, B, W + //#define NEOPIXEL_BKGD_ALWAYS_ON // Keep the backlight on when other NeoPixels are off #endif /** @@ -2688,9 +2998,9 @@ * Set this manually if there are extra servos needing manual control. * Set to 0 to turn off servo support. */ -//#define NUM_SERVOS 3 // Servo index starts with 0 for M280 command +//#define NUM_SERVOS 3 // Note: Servo index starts with 0 for M280-M282 commands -// (ms) Delay before the next move will start, to give the servo time to reach its target angle. +// (ms) Delay before the next move will start, to give the servo time to reach its target angle. // 300ms is a good value but you can try less delay. // If the servo can't reach the requested position, increase it. #define SERVO_DELAY { 300 } @@ -2700,3 +3010,6 @@ // Edit servo angles with M281 and save to EEPROM with M500 //#define EDITABLE_SERVO_ANGLES + +// Disable servo with M282 to reduce power consumption, noise, and heat when not in use +//#define SERVO_DETACH_GCODE diff --git a/Marlin/Configuration_adv.h b/Marlin/Configuration_adv.h index 04b48d839263..8e9017deb38a 100644 --- a/Marlin/Configuration_adv.h +++ b/Marlin/Configuration_adv.h @@ -32,7 +32,7 @@ * * Basic settings can be found in Configuration.h */ -#define CONFIGURATION_ADV_H_VERSION 020008 +#define CONFIGURATION_ADV_H_VERSION 02000903 //=========================================================================== //============================= Thermal Settings ============================ @@ -115,16 +115,51 @@ #define CHAMBER_BETA 3950 // Beta value #endif +#if TEMP_SENSOR_COOLER == 1000 + #define COOLER_PULLUP_RESISTOR_OHMS 4700 // Pullup resistor + #define COOLER_RESISTANCE_25C_OHMS 100000 // Resistance at 25C + #define COOLER_BETA 3950 // Beta value +#endif + #if TEMP_SENSOR_PROBE == 1000 #define PROBE_PULLUP_RESISTOR_OHMS 4700 // Pullup resistor #define PROBE_RESISTANCE_25C_OHMS 100000 // Resistance at 25C #define PROBE_BETA 3950 // Beta value #endif -// -// Hephestos 2 24V heated bed upgrade kit. -// https://store.bq.com/en/heated-bed-kit-hephestos2 -// +#if TEMP_SENSOR_BOARD == 1000 + #define BOARD_PULLUP_RESISTOR_OHMS 4700 // Pullup resistor + #define BOARD_RESISTANCE_25C_OHMS 100000 // Resistance at 25C + #define BOARD_BETA 3950 // Beta value +#endif + +#if TEMP_SENSOR_REDUNDANT == 1000 + #define REDUNDANT_PULLUP_RESISTOR_OHMS 4700 // Pullup resistor + #define REDUNDANT_RESISTANCE_25C_OHMS 100000 // Resistance at 25C + #define REDUNDANT_BETA 3950 // Beta value +#endif + +/** + * Thermocouple Options — for MAX6675 (-2), MAX31855 (-3), and MAX31865 (-5). + */ +//#define TEMP_SENSOR_FORCE_HW_SPI // Ignore SCK/MOSI/MISO pins; use CS and the default SPI bus. +//#define MAX31865_SENSOR_WIRES_0 2 // (2-4) Number of wires for the probe connected to a MAX31865 board. +//#define MAX31865_SENSOR_WIRES_1 2 + +//#define MAX31865_50HZ_FILTER // Use a 50Hz filter instead of the default 60Hz. +//#define MAX31865_USE_READ_ERROR_DETECTION // Treat value spikes (20°C delta in under 1s) as read errors. + +//#define MAX31865_USE_AUTO_MODE // Read faster and more often than 1-shot; bias voltage always on; slight effect on RTD temperature. +//#define MAX31865_MIN_SAMPLING_TIME_MSEC 100 // (ms) 1-shot: minimum read interval. Reduces bias voltage effects by leaving sensor unpowered for longer intervals. +//#define MAX31865_IGNORE_INITIAL_FAULTY_READS 10 // Ignore some read faults (keeping the temperature reading) to work around a possible issue (#23439). + +//#define MAX31865_WIRE_OHMS_0 0.95f // For 2-wire, set the wire resistances for more accurate readings. +//#define MAX31865_WIRE_OHMS_1 0.0f + +/** + * Hephestos 2 24V heated bed upgrade kit. + * https://store.bq.com/en/heated-bed-kit-hephestos2 + */ //#define HEPHESTOS2_HEATED_BED_KIT #if ENABLED(HEPHESTOS2_HEATED_BED_KIT) #undef TEMP_SENSOR_BED @@ -159,7 +194,8 @@ //#define CHAMBER_FAN // Enable a fan on the chamber #if ENABLED(CHAMBER_FAN) - #define CHAMBER_FAN_MODE 2 // Fan control mode: 0=Static; 1=Linear increase when temp is higher than target; 2=V-shaped curve. + //#define CHAMBER_FAN_INDEX 2 // Index of a fan to repurpose as the chamber fan. (Default: first unused fan) + #define CHAMBER_FAN_MODE 2 // Fan control mode: 0=Static; 1=Linear increase when temp is higher than target; 2=V-shaped curve; 3=similar to 1 but fan is always on. #if CHAMBER_FAN_MODE == 0 #define CHAMBER_FAN_BASE 255 // Chamber fan PWM (0-255) #elif CHAMBER_FAN_MODE == 1 @@ -168,6 +204,9 @@ #elif CHAMBER_FAN_MODE == 2 #define CHAMBER_FAN_BASE 128 // Minimum chamber fan PWM (0-255) #define CHAMBER_FAN_FACTOR 25 // PWM increase per °C difference from target + #elif CHAMBER_FAN_MODE == 3 + #define CHAMBER_FAN_BASE 128 // Base chamber fan PWM (0-255) + #define CHAMBER_FAN_FACTOR 25 // PWM increase per °C above target #endif #endif @@ -181,6 +220,51 @@ #endif #endif +// +// Laser Cooler options +// +#if TEMP_SENSOR_COOLER + #define COOLER_MINTEMP 8 // (°C) + #define COOLER_MAXTEMP 26 // (°C) + #define COOLER_DEFAULT_TEMP 16 // (°C) + #define TEMP_COOLER_HYSTERESIS 1 // (°C) Temperature proximity considered "close enough" to the target + #define COOLER_PIN 8 // Laser cooler on/off pin used to control power to the cooling element (e.g., TEC, External chiller via relay) + #define COOLER_INVERTING false + #define TEMP_COOLER_PIN 15 // Laser/Cooler temperature sensor pin. ADC is required. + #define COOLER_FAN // Enable a fan on the cooler, Fan# 0,1,2,3 etc. + #define COOLER_FAN_INDEX 0 // FAN number 0, 1, 2 etc. e.g. + #if ENABLED(COOLER_FAN) + #define COOLER_FAN_BASE 100 // Base Cooler fan PWM (0-255); turns on when Cooler temperature is above the target + #define COOLER_FAN_FACTOR 25 // PWM increase per °C above target + #endif +#endif + +// +// Motherboard Sensor options +// +#if TEMP_SENSOR_BOARD + #define THERMAL_PROTECTION_BOARD // Halt the printer if the board sensor leaves the temp range below. + #define BOARD_MINTEMP 8 // (°C) + #define BOARD_MAXTEMP 70 // (°C) + #ifndef TEMP_BOARD_PIN + //#define TEMP_BOARD_PIN -1 // Board temp sensor pin, if not set in pins file. + #endif +#endif + +// +// Laser Coolant Flow Meter +// +//#define LASER_COOLANT_FLOW_METER +#if ENABLED(LASER_COOLANT_FLOW_METER) + #define FLOWMETER_PIN 20 // Requires an external interrupt-enabled pin (e.g., RAMPS 2,3,18,19,20,21) + #define FLOWMETER_PPL 5880 // (pulses/liter) Flow meter pulses-per-liter on the input pin + #define FLOWMETER_INTERVAL 1000 // (ms) Flow rate calculation interval in milliseconds + #define FLOWMETER_SAFETY // Prevent running the laser without the minimum flow rate set below + #if ENABLED(FLOWMETER_SAFETY) + #define FLOWMETER_MIN_LITERS_PER_MINUTE 1.5 // (liters/min) Minimum flow required when enabled + #endif +#endif + /** * Thermal Protection provides additional protection to your printer from damage * and fire. Marlin always includes safe min and max temperature ranges which @@ -250,6 +334,24 @@ #define WATCH_CHAMBER_TEMP_INCREASE 2 // Degrees Celsius #endif +/** + * Thermal Protection parameters for the laser cooler. + */ +#if ENABLED(THERMAL_PROTECTION_COOLER) + #define THERMAL_PROTECTION_COOLER_PERIOD 10 // Seconds + #define THERMAL_PROTECTION_COOLER_HYSTERESIS 3 // Degrees Celsius + + /** + * Laser cooling watch settings (M143/M193). + */ + #define WATCH_COOLER_TEMP_PERIOD 60 // Seconds + #define WATCH_COOLER_TEMP_INCREASE 3 // Degrees Celsius +#endif + +#if ANY(THERMAL_PROTECTION_HOTENDS, THERMAL_PROTECTION_BED, THERMAL_PROTECTION_CHAMBER, THERMAL_PROTECTION_COOLER) + #define THERMAL_PROTECTION_VARIANCE_MONITOR // Detect a sensor malfunction preventing temperature updates +#endif + #if ENABLED(PIDTEMP) // Add an experimental additional term to the heater power, proportional to the extrusion speed. // A well-chosen Kc value should add just enough power to melt the increased material volume. @@ -325,7 +427,7 @@ */ #define AUTOTEMP #if ENABLED(AUTOTEMP) - #define AUTOTEMP_OLDWEIGHT 0.98 + #define AUTOTEMP_OLDWEIGHT 0.98 // Factor used to weight previous readings (0.0 < value < 1.0) // Turn on AUTOTEMP on M104/M109 by default using proportions set here //#define AUTOTEMP_PROPORTIONAL #if ENABLED(AUTOTEMP_PROPORTIONAL) @@ -409,16 +511,20 @@ */ //#define USE_CONTROLLER_FAN #if ENABLED(USE_CONTROLLER_FAN) - //#define CONTROLLER_FAN_PIN -1 // Set a custom pin for the controller fan - //#define CONTROLLER_FAN_USE_Z_ONLY // With this option only the Z axis is considered - //#define CONTROLLER_FAN_IGNORE_Z // Ignore Z stepper. Useful when stepper timeout is disabled. - #define CONTROLLERFAN_SPEED_MIN 0 // (0-255) Minimum speed. (If set below this value the fan is turned off.) - #define CONTROLLERFAN_SPEED_ACTIVE 255 // (0-255) Active speed, used when any motor is enabled - #define CONTROLLERFAN_SPEED_IDLE 0 // (0-255) Idle speed, used when motors are disabled - #define CONTROLLERFAN_IDLE_TIME 60 // (seconds) Extra time to keep the fan running after disabling motors - //#define CONTROLLER_FAN_EDITABLE // Enable M710 configurable settings + //#define CONTROLLER_FAN_PIN -1 // Set a custom pin for the controller fan + //#define CONTROLLER_FAN_USE_Z_ONLY // With this option only the Z axis is considered + //#define CONTROLLER_FAN_IGNORE_Z // Ignore Z stepper. Useful when stepper timeout is disabled. + #define CONTROLLERFAN_SPEED_MIN 0 // (0-255) Minimum speed. (If set below this value the fan is turned off.) + #define CONTROLLERFAN_SPEED_ACTIVE 255 // (0-255) Active speed, used when any motor is enabled + #define CONTROLLERFAN_SPEED_IDLE 0 // (0-255) Idle speed, used when motors are disabled + #define CONTROLLERFAN_IDLE_TIME 60 // (seconds) Extra time to keep the fan running after disabling motors + + // Use TEMP_SENSOR_BOARD as a trigger for enabling the controller fan + //#define CONTROLLER_FAN_MIN_BOARD_TEMP 40 // (°C) Turn on the fan if the board reaches this temperature + + //#define CONTROLLER_FAN_EDITABLE // Enable M710 configurable settings #if ENABLED(CONTROLLER_FAN_EDITABLE) - #define CONTROLLER_FAN_MENU // Enable the Controller Fan submenu + #define CONTROLLER_FAN_MENU // Enable the Controller Fan submenu #endif #endif @@ -446,32 +552,48 @@ //#define FAN_MAX_PWM 128 /** - * FAST PWM FAN Settings + * Fan Fast PWM * - * Use to change the FAST FAN PWM frequency (if enabled in Configuration.h) - * Combinations of PWM Modes, prescale values and TOP resolutions are used internally to produce a - * frequency as close as possible to the desired frequency. + * Combinations of PWM Modes, prescale values and TOP resolutions are used internally + * to produce a frequency as close as possible to the desired frequency. * - * FAST_PWM_FAN_FREQUENCY [undefined by default] + * FAST_PWM_FAN_FREQUENCY * Set this to your desired frequency. - * If left undefined this defaults to F = F_CPU/(2*255*1) - * i.e., F = 31.4kHz on 16MHz microcontrollers or F = 39.2kHz on 20MHz microcontrollers. - * These defaults are the same as with the old FAST_PWM_FAN implementation - no migration is required + * For AVR, if left undefined this defaults to F = F_CPU/(2*255*1) + * i.e., F = 31.4kHz on 16MHz microcontrollers or F = 39.2kHz on 20MHz microcontrollers. + * For non AVR, if left undefined this defaults to F = 1Khz. + * This F value is only to protect the hardware from an absence of configuration + * and not to complete it when users are not aware that the frequency must be specifically set to support the target board. + * * NOTE: Setting very low frequencies (< 10 Hz) may result in unexpected timer behavior. + * Setting very high frequencies can damage your hardware. * * USE_OCR2A_AS_TOP [undefined by default] * Boards that use TIMER2 for PWM have limitations resulting in only a few possible frequencies on TIMER2: - * 16MHz MCUs: [62.5KHz, 31.4KHz (default), 7.8KHz, 3.92KHz, 1.95KHz, 977Hz, 488Hz, 244Hz, 60Hz, 122Hz, 30Hz] - * 20MHz MCUs: [78.1KHz, 39.2KHz (default), 9.77KHz, 4.9KHz, 2.44KHz, 1.22KHz, 610Hz, 305Hz, 153Hz, 76Hz, 38Hz] + * 16MHz MCUs: [62.5kHz, 31.4kHz (default), 7.8kHz, 3.92kHz, 1.95kHz, 977Hz, 488Hz, 244Hz, 60Hz, 122Hz, 30Hz] + * 20MHz MCUs: [78.1kHz, 39.2kHz (default), 9.77kHz, 4.9kHz, 2.44kHz, 1.22kHz, 610Hz, 305Hz, 153Hz, 76Hz, 38Hz] * A greater range can be achieved by enabling USE_OCR2A_AS_TOP. But note that this option blocks the use of * PWM on pin OC2A. Only use this option if you don't need PWM on 0C2A. (Check your schematic.) * USE_OCR2A_AS_TOP sacrifices duty cycle control resolution to achieve this broader range of frequencies. */ +//#define FAST_PWM_FAN // Increase the fan PWM frequency. Removes the PWM noise but increases heating in the FET/Arduino #if ENABLED(FAST_PWM_FAN) - //#define FAST_PWM_FAN_FREQUENCY 31400 + //#define FAST_PWM_FAN_FREQUENCY 31400 // Define here to override the defaults below //#define USE_OCR2A_AS_TOP + #ifndef FAST_PWM_FAN_FREQUENCY + #ifdef __AVR__ + #define FAST_PWM_FAN_FREQUENCY ((F_CPU) / (2 * 255 * 1)) + #else + #define FAST_PWM_FAN_FREQUENCY 1000U + #endif + #endif #endif +/** + * Use one of the PWM fans as a redundant part-cooling fan + */ +//#define REDUNDANT_PART_COOLING_FAN 2 // Index of the fan to sync with FAN 0. + // @section extruder /** @@ -495,11 +617,49 @@ #define E6_AUTO_FAN_PIN -1 #define E7_AUTO_FAN_PIN -1 #define CHAMBER_AUTO_FAN_PIN -1 +#define COOLER_AUTO_FAN_PIN -1 +#define COOLER_FAN_PIN -1 #define EXTRUDER_AUTO_FAN_TEMPERATURE 50 #define EXTRUDER_AUTO_FAN_SPEED 255 // 255 == full speed #define CHAMBER_AUTO_FAN_TEMPERATURE 30 #define CHAMBER_AUTO_FAN_SPEED 255 +#define COOLER_AUTO_FAN_TEMPERATURE 18 +#define COOLER_AUTO_FAN_SPEED 255 + +/** + * Hotend Cooling Fans tachometers + * + * Define one or more tachometer pins to enable fan speed + * monitoring, and reporting of fan speeds with M123. + * + * NOTE: Only works with fans up to 7000 RPM. + */ +//#define FOURWIRES_FANS // Needed with AUTO_FAN when 4-wire PWM fans are installed +//#define E0_FAN_TACHO_PIN -1 +//#define E0_FAN_TACHO_PULLUP +//#define E0_FAN_TACHO_PULLDOWN +//#define E1_FAN_TACHO_PIN -1 +//#define E1_FAN_TACHO_PULLUP +//#define E1_FAN_TACHO_PULLDOWN +//#define E2_FAN_TACHO_PIN -1 +//#define E2_FAN_TACHO_PULLUP +//#define E2_FAN_TACHO_PULLDOWN +//#define E3_FAN_TACHO_PIN -1 +//#define E3_FAN_TACHO_PULLUP +//#define E3_FAN_TACHO_PULLDOWN +//#define E4_FAN_TACHO_PIN -1 +//#define E4_FAN_TACHO_PULLUP +//#define E4_FAN_TACHO_PULLDOWN +//#define E5_FAN_TACHO_PIN -1 +//#define E5_FAN_TACHO_PULLUP +//#define E5_FAN_TACHO_PULLDOWN +//#define E6_FAN_TACHO_PIN -1 +//#define E6_FAN_TACHO_PULLUP +//#define E6_FAN_TACHO_PULLDOWN +//#define E7_FAN_TACHO_PIN -1 +//#define E7_FAN_TACHO_PULLUP +//#define E7_FAN_TACHO_PULLDOWN /** * Part-Cooling Fan Multiplexer @@ -614,6 +774,12 @@ #endif #endif +// Drive the E axis with two synchronized steppers +//#define E_DUAL_STEPPER_DRIVERS +#if ENABLED(E_DUAL_STEPPER_DRIVERS) + //#define INVERT_E1_VS_E0_DIR // Enable if the E motors need opposite DIR states +#endif + /** * Dual X Carriage * @@ -677,7 +843,7 @@ * the position of the toolhead relative to the workspace. */ -#define SENSORLESS_BACKOFF_MM { 2, 2 } // (mm) Backoff from endstops before sensorless homing +#define SENSORLESS_BACKOFF_MM { 2, 2, 0 } // (mm) Backoff from endstops before sensorless homing #define HOMING_BUMP_MM { 2, 2, 2 } // (mm) Backoff from endstops after first bump #define HOMING_BUMP_DIVISOR { 2, 2, 4 } // Re-Bump Speed Divisor (Divides the Homing Feedrate) @@ -748,12 +914,14 @@ //#define BLTOUCH_FORCE_MODE_SET /** - * Use "HIGH SPEED" mode for probing. + * Enable "HIGH SPEED" option for probing. * Danger: Disable if your probe sometimes fails. Only suitable for stable well-adjusted systems. * This feature was designed for Deltabots with very fast Z moves; however, higher speed Cartesians * might be able to use it. If the machine can't raise Z fast enough the BLTouch may go into ALARM. + * + * Set the default state here, change with 'M401 S' or UI, use M500 to save, M502 to reset. */ - // #define BLTOUCH_HS_MODE + //#define BLTOUCH_HS_MODE true // Safety: Enable voltage mode settings in the LCD menu. // #define BLTOUCH_LCD_VOLTAGE_MENU @@ -863,6 +1031,9 @@ #define INVERT_X_STEP_PIN false #define INVERT_Y_STEP_PIN false #define INVERT_Z_STEP_PIN false +#define INVERT_I_STEP_PIN false +#define INVERT_J_STEP_PIN false +#define INVERT_K_STEP_PIN false #define INVERT_E_STEP_PIN false /** @@ -874,6 +1045,9 @@ #define DISABLE_INACTIVE_X true #define DISABLE_INACTIVE_Y true #define DISABLE_INACTIVE_Z true // Set 'false' if the nozzle could fall onto your printed part! +#define DISABLE_INACTIVE_I true +#define DISABLE_INACTIVE_J true +#define DISABLE_INACTIVE_K true #define DISABLE_INACTIVE_E true // Default Minimum Feedrates for printing and travel moves @@ -914,9 +1088,12 @@ #if ENABLED(BACKLASH_COMPENSATION) // Define values for backlash distance and correction. // If BACKLASH_GCODE is enabled these values are the defaults. - #define BACKLASH_DISTANCE_MM { 0, 0, 0 } // (mm) + #define BACKLASH_DISTANCE_MM { 0, 0, 0 } // (mm) One value for each linear axis #define BACKLASH_CORRECTION 0.0 // 0.0 = no correction; 1.0 = full correction + // Add steps for motor direction changes on CORE kinematics + //#define CORE_BACKLASH + // Set BACKLASH_SMOOTHING_MM to spread backlash correction over multiple segments // to reduce print artifacts. (Enabling this is costly in memory and computation!) //#define BACKLASH_SMOOTHING_MM 3 // (mm) @@ -982,6 +1159,13 @@ #define CALIBRATION_MEASURE_LEFT #define CALIBRATION_MEASURE_BACK + //#define CALIBRATION_MEASURE_IMIN + //#define CALIBRATION_MEASURE_IMAX + //#define CALIBRATION_MEASURE_JMIN + //#define CALIBRATION_MEASURE_JMAX + //#define CALIBRATION_MEASURE_KMIN + //#define CALIBRATION_MEASURE_KMAX + // Probing at the exact top center only works if the center is flat. If // probing on a screwhead or hollow washer, probe near the edges. //#define CALIBRATION_MEASURE_AT_TOP_EDGES @@ -1075,7 +1259,7 @@ // @section lcd -#if EITHER(IS_ULTIPANEL, EXTENSIBLE_UI) +#if HAS_MANUAL_MOVE_MENU #define MANUAL_FEEDRATE { 50*60, 50*60, 4*60, 2*60 } // (mm/min) Feedrates for manual moves along X, Y, Z, E from panel #define FINE_MANUAL_MOVE 0.025 // (mm) Smallest manual move (< 0.1mm) applying to Z on most machines #if IS_ULTIPANEL @@ -1098,21 +1282,35 @@ #define FEEDRATE_CHANGE_BEEP_FREQUENCY 440 #endif -#if HAS_LCD_MENU +#if HAS_BED_PROBE && EITHER(HAS_MARLINUI_MENU, HAS_TFT_LVGL_UI) + //#define PROBE_OFFSET_WIZARD // Add a Probe Z Offset calibration option to the LCD menu + #if ENABLED(PROBE_OFFSET_WIZARD) + /** + * Enable to init the Probe Z-Offset when starting the Wizard. + * Use a height slightly above the estimated nozzle-to-probe Z offset. + * For example, with an offset of -5, consider a starting height of -4. + */ + //#define PROBE_OFFSET_WIZARD_START_Z -4.0 + + // Set a convenient position to do the calibration (probing point and nozzle/bed-distance) + //#define PROBE_OFFSET_WIZARD_XY_POS { X_CENTER, Y_CENTER } + #endif +#endif - // Add Probe Z Offset calibration to the Z Probe Offsets menu - #if HAS_BED_PROBE - //#define PROBE_OFFSET_WIZARD - #if ENABLED(PROBE_OFFSET_WIZARD) - // - // Enable to init the Probe Z-Offset when starting the Wizard. - // Use a height slightly above the estimated nozzle-to-probe Z offset. - // For example, with an offset of -5, consider a starting height of -4. - // - //#define PROBE_OFFSET_WIZARD_START_Z -4.0 +#if HAS_MARLINUI_MENU - // Set a convenient position to do the calibration (probing point and nozzle/bed-distance) - //#define PROBE_OFFSET_WIZARD_XY_POS { X_CENTER, Y_CENTER } + #if BOTH(HAS_BED_PROBE, AUTO_BED_LEVELING_BILINEAR) + // Add calibration in the Probe Offsets menu to compensate for X-axis twist. + //#define X_AXIS_TWIST_COMPENSATION + #if ENABLED(X_AXIS_TWIST_COMPENSATION) + /** + * Enable to init the Probe Z-Offset when starting the Wizard. + * Use a height slightly above the estimated nozzle-to-probe Z offset. + * For example, with an offset of -5, consider a starting height of -4. + */ + #define XATC_START_Z 0.0 + #define XATC_MAX_POINTS 3 // Number of points to probe in the wizard + #define XATC_Y_POSITION Y_CENTER // (mm) Y position to probe #endif #endif @@ -1125,8 +1323,37 @@ // BACK menu items keep the highlight at the top //#define TURBO_BACK_MENU_ITEM - // Add a mute option to the LCD menu - //#define SOUND_MENU_ITEM + // Insert a menu for preheating at the top level to allow for quick access + //#define PREHEAT_SHORTCUT_MENU_ITEM + +#endif // HAS_MARLINUI_MENU + +#if ANY(HAS_DISPLAY, DWIN_CREALITY_LCD_ENHANCED, DWIN_CREALITY_LCD_JYERSUI) + //#define SOUND_MENU_ITEM // Add a mute option to the LCD menu +#endif + +#if EITHER(HAS_DISPLAY, DWIN_CREALITY_LCD_ENHANCED) + // The timeout (in ms) to return to the status screen from sub-menus + //#define LCD_TIMEOUT_TO_STATUS 15000 + + #if ENABLED(SHOW_BOOTSCREEN) + #define BOOTSCREEN_TIMEOUT 4000 // (ms) Total Duration to display the boot screen(s) + #if EITHER(HAS_MARLINUI_U8GLIB, TFT_COLOR_UI) + #define BOOT_MARLIN_LOGO_SMALL // Show a smaller Marlin logo on the Boot Screen (saving lots of flash) + #endif + #endif + + // Scroll a longer status message into view + #define STATUS_MESSAGE_SCROLLING + + // On the Info Screen, display XY with one decimal place when possible + //#define LCD_DECIMAL_SMALL_XY + + // Add an 'M73' G-code to set the current percentage + #define LCD_SET_PROGRESS_MANUALLY + + // Show the E position (filament used) during printing + #define LCD_SHOW_E_TOTAL /** * LED Control Menu @@ -1154,40 +1381,16 @@ #endif #endif - // Insert a menu for preheating at the top level to allow for quick access - //#define PREHEAT_SHORTCUT_MENU_ITEM - -#endif // HAS_LCD_MENU - -#if HAS_DISPLAY - // The timeout (in ms) to return to the status screen from sub-menus - //#define LCD_TIMEOUT_TO_STATUS 15000 - - #if ENABLED(SHOW_BOOTSCREEN) - #define BOOTSCREEN_TIMEOUT 4000 // (ms) Total Duration to display the boot screen(s) - #if EITHER(HAS_MARLINUI_U8GLIB, TFT_COLOR_UI) - #define BOOT_MARLIN_LOGO_SMALL // Show a smaller Marlin logo on the Boot Screen (saving lots of flash) - #endif - #endif - - // Scroll a longer status message into view - #define STATUS_MESSAGE_SCROLLING - - // On the Info Screen, display XY with one decimal place when possible - //#define LCD_DECIMAL_SMALL_XY - - // Add an 'M73' G-code to set the current percentage - #define LCD_SET_PROGRESS_MANUALLY - - // Show the E position (filament used) during printing - #define LCD_SHOW_E_TOTAL #endif -#if EITHER(SDSUPPORT, LCD_SET_PROGRESS_MANUALLY) && ANY(HAS_MARLINUI_U8GLIB, HAS_MARLINUI_HD44780, IS_TFTGLCD_PANEL, EXTENSIBLE_UI) - //#define SHOW_REMAINING_TIME // Display estimated time to completion - #if ENABLED(SHOW_REMAINING_TIME) - //#define USE_M73_REMAINING_TIME // Use remaining time from M73 command instead of estimation - //#define ROTATE_PROGRESS_DISPLAY // Display (P)rogress, (E)lapsed, and (R)emaining time +// LCD Print Progress options +#if EITHER(SDSUPPORT, LCD_SET_PROGRESS_MANUALLY) + #if CAN_SHOW_REMAINING_TIME + //#define SHOW_REMAINING_TIME // Display estimated time to completion + #if ENABLED(SHOW_REMAINING_TIME) + //#define USE_M73_REMAINING_TIME // Use remaining time from M73 command instead of estimation + //#define ROTATE_PROGRESS_DISPLAY // Display (P)rogress, (E)lapsed, and (R)emaining time + #endif #endif #if EITHER(HAS_MARLINUI_U8GLIB, EXTENSIBLE_UI) @@ -1243,6 +1446,8 @@ //#define BROWSE_MEDIA_ON_INSERT // Open the file browser when media is inserted + //#define MEDIA_MENU_AT_TOP // Force the media menu to be listed on the top of the main menu + #define EVENT_GCODE_SD_ABORT "G28XY" // G-code to run on SD Abort Print (e.g., "G28XY" or "G27") #if ENABLED(PRINTER_EVENT_LEDS) @@ -1322,33 +1527,23 @@ // LCD's font must contain the characters. Check your selected LCD language. //#define UTF_FILENAME_SUPPORT - // This allows hosts to request long names for files and folders with M33 - #define LONG_FILENAME_HOST_SUPPORT + #define LONG_FILENAME_HOST_SUPPORT // Get the long filename of a file/folder with 'M33 ' and list long filenames with 'M20 L' + //#define LONG_FILENAME_WRITE_SUPPORT // Create / delete files with long filenames via M28, M30, and Binary Transfer Protocol - // Enable this option to scroll long filenames in the SD card menu - #define SCROLL_LONG_FILENAMES + #define SCROLL_LONG_FILENAMES // Scroll long filenames in the SD card menu - // Leave the heaters on after Stop Print (not recommended!) - //#define SD_ABORT_NO_COOLDOWN + //#define SD_ABORT_NO_COOLDOWN // Leave the heaters on after Stop Print (not recommended!) /** - * This option allows you to abort SD printing when any endstop is triggered. - * This feature must be enabled with "M540 S1" or from the LCD menu. - * To have any effect, endstops must be enabled during SD printing. + * Abort SD printing when any endstop is triggered. + * This feature is enabled with 'M540 S1' or from the LCD menu. + * Endstops must be activated for this option to work. */ //#define SD_ABORT_ON_ENDSTOP_HIT - /** - * This option makes it easier to print the same SD Card file again. - * On print completion the LCD Menu will open with the file selected. - * You can just click to start the print, or navigate elsewhere. - */ - //#define SD_REPRINT_LAST_SELECTED_FILE + //#define SD_REPRINT_LAST_SELECTED_FILE // On print completion open the LCD Menu and select the same file - /** - * Auto-report SdCard status with M27 S - */ - //#define AUTO_REPORT_SD_STATUS + //#define AUTO_REPORT_SD_STATUS // Auto-report media status with 'M27 S' /** * Support for USB thumb drives using an Arduino USB Host Shield or @@ -1406,9 +1601,22 @@ #define SD_FIRMWARE_UPDATE_INACTIVE_VALUE 0xFF #endif + /** + * Enable this option if you have more than ~3K of unused flash space. + * Marlin will embed all settings in the firmware binary as compressed data. + * Use 'M503 C' to write the settings out to the SD Card as 'mc.zip'. + * See docs/ConfigEmbedding.md for details on how to use 'mc-apply.py'. + */ + //#define CONFIGURATION_EMBEDDING + // Add an optimized binary file transfer mode, initiated with 'M28 B1' //#define BINARY_FILE_TRANSFER + #if ENABLED(BINARY_FILE_TRANSFER) + // Include extra facilities (e.g., 'M20 F') supporting firmware upload via BINARY_FILE_TRANSFER + //#define CUSTOM_FIRMWARE_UPLOAD + #endif + /** * Set this option to one of the following (or the board's defaults apply): * @@ -1426,6 +1634,15 @@ // Enable if SD detect is rendered useless (e.g., by using an SD extender) //#define NO_SD_DETECT + // Multiple volume support - EXPERIMENTAL. + //#define MULTI_VOLUME + #if ENABLED(MULTI_VOLUME) + #define VOLUME_SD_ONBOARD + #define VOLUME_USB_FLASH_DRIVE + #define DEFAULT_VOLUME SV_SD_ONBOARD + #define DEFAULT_SHARED_VOLUME SV_USB_FLASH_DRIVE + #endif + #endif // SDSUPPORT /** @@ -1447,16 +1664,10 @@ * printing performance versus fast display updates. */ #if HAS_MARLINUI_U8GLIB - // Show SD percentage next to the progress bar - //#define DOGM_SD_PERCENT - // Save many cycles by drawing a hollow frame or no frame on the Info Screen //#define XYZ_NO_FRAME // #define XYZ_HOLLOW_FRAME - // Enable to save many cycles by drawing a hollow frame on Menu Screens - // #define MENU_HOLLOW_FRAME - // A bigger font is available for edit items. Costs 3120 bytes of PROGMEM. // Western only. Not available for Cyrillic, Kana, Turkish, Greek, or Chinese. //#define USE_BIG_EDIT_FONT @@ -1465,9 +1676,6 @@ // Western only. Not available for Cyrillic, Kana, Turkish, Greek, or Chinese. //#define USE_SMALL_INFOFONT - // Swap the CW/CCW indicators in the graphics overlay - //#define OVERLAY_GFX_REVERSE - /** * ST7920-based LCDs can emulate a 16 x 4 character display using * the ST7920 character-generator for very fast screen updates. @@ -1480,7 +1688,7 @@ * Set STATUS_EXPIRE_SECONDS to zero to never clear the status. * This will prevent position updates from being displayed. */ - #if ENABLED(U8GLIB_ST7920) + #if IS_U8GLIB_ST7920 // Enable this option and reduce the value to optimize screen updates. // The normal delay is 10µs. Use the lowest value that still gives a reliable display. //#define DOGM_SPI_DELAY_US 5 @@ -1498,16 +1706,18 @@ */ //#define STATUS_COMBINE_HEATERS // Use combined heater images instead of separate ones //#define STATUS_HOTEND_NUMBERLESS // Use plain hotend icons instead of numbered ones (with 2+ hotends) - #define STATUS_HOTEND_INVERTED // Show solid nozzle bitmaps when heating (Requires STATUS_HOTEND_ANIM) + #define STATUS_HOTEND_INVERTED // Show solid nozzle bitmaps when heating (Requires STATUS_HOTEND_ANIM for numbered hotends) #define STATUS_HOTEND_ANIM // Use a second bitmap to indicate hotend heating #define STATUS_BED_ANIM // Use a second bitmap to indicate bed heating #define STATUS_CHAMBER_ANIM // Use a second bitmap to indicate chamber heating //#define STATUS_CUTTER_ANIM // Use a second bitmap to indicate spindle / laser active + //#define STATUS_COOLER_ANIM // Use a second bitmap to indicate laser cooling + //#define STATUS_FLOWMETER_ANIM // Use multiple bitmaps to indicate coolant flow //#define STATUS_ALT_BED_BITMAP // Use the alternative bed bitmap //#define STATUS_ALT_FAN_BITMAP // Use the alternative fan bitmap //#define STATUS_FAN_FRAMES 3 // :[0,1,2,3,4] Number of fan animation frames //#define STATUS_HEAT_PERCENT // Show heating in a progress bar - #define BOOT_MARLIN_LOGO_ANIMATED // Animated Marlin logo. Costs ~‭3260 (or ~940) bytes of PROGMEM. + #define BOOT_MARLIN_LOGO_ANIMATED // Animated Marlin logo. Costs ~3260 (or ~940) bytes of PROGMEM. // Frivolous Game Options //#define MARLIN_BRICKOUT @@ -1517,6 +1727,17 @@ #endif // HAS_MARLINUI_U8GLIB +#if HAS_MARLINUI_U8GLIB || IS_DWIN_MARLINUI + // Show SD percentage next to the progress bar + //#define SHOW_SD_PERCENT + + // Enable to save many cycles by drawing a hollow frame on Menu Screens + #define MENU_HOLLOW_FRAME + + // Swap the CW/CCW indicators in the graphics overlay + //#define OVERLAY_GFX_REVERSE +#endif + // // Additional options for DGUS / DWIN displays // @@ -1554,10 +1775,35 @@ #endif #endif // HAS_DGUS_LCD +// +// Additional options for AnyCubic Chiron TFT displays +// +#if ENABLED(ANYCUBIC_LCD_CHIRON) + // By default the type of panel is automatically detected. + // Enable one of these options if you know the panel type. + //#define CHIRON_TFT_STANDARD + //#define CHIRON_TFT_NEW + + // Enable the longer Anycubic powerup startup tune + //#define AC_DEFAULT_STARTUP_TUNE + + /** + * Display Folders + * By default the file browser lists all G-code files (including those in subfolders) in a flat list. + * Enable this option to display a hierarchical file browser. + * + * NOTES: + * - Without this option it helps to enable SDCARD_SORT_ALPHA so files are sorted before/after folders. + * - When used with the "new" panel, folder names will also have '.gcode' appended to their names. + * This hack is currently required to force the panel to show folders. + */ + #define AC_SD_FOLDER_VIEW +#endif + // // Specify additional languages for the UI. Default specified by LCD_LANGUAGE. // -#if ANY(DOGLCD, TFT_COLOR_UI, TOUCH_UI_FTDI_EVE) +#if ANY(DOGLCD, TFT_COLOR_UI, TOUCH_UI_FTDI_EVE, IS_DWIN_MARLINUI) //#define LCD_LANGUAGE_2 fr //#define LCD_LANGUAGE_3 de //#define LCD_LANGUAGE_4 es @@ -1576,7 +1822,7 @@ //#define LCD_4DSYSTEMS_4DLCD_FT843 // 4D Systems 4.3" (480x272) //#define LCD_HAOYU_FT800CB // Haoyu with 4.3" or 5" (480x272) //#define LCD_HAOYU_FT810CB // Haoyu with 5" (800x480) - //#define LCD_ALEPHOBJECTS_CLCD_UI // Aleph Objects Color LCD UI + //#define LCD_LULZBOT_CLCD_UI // LulzBot Color LCD UI //#define LCD_FYSETC_TFT81050 // FYSETC with 5" (800x480) //#define LCD_EVE3_50G // Matrix Orbital 5.0", 800x480, BT815 //#define LCD_EVE2_50G // Matrix Orbital 5.0", 800x480, FT813 @@ -1587,8 +1833,8 @@ //#define TOUCH_UI_800x480 // Mappings for boards with a standard RepRapDiscount Display connector - //#define AO_EXP1_PINMAP // AlephObjects CLCD UI EXP1 mapping - //#define AO_EXP2_PINMAP // AlephObjects CLCD UI EXP2 mapping + //#define AO_EXP1_PINMAP // LulzBot CLCD UI EXP1 mapping + //#define AO_EXP2_PINMAP // LulzBot CLCD UI EXP2 mapping //#define CR10_TFT_PINMAP // Rudolph Riedel's CR10 pin mapping //#define S6_TFT_PINMAP // FYSETC S6 pin mapping //#define F6_TFT_PINMAP // FYSETC F6 pin mapping @@ -1751,7 +1997,8 @@ //#define EXTRA_LIN_ADVANCE_K // Enable for second linear advance constants #define LIN_ADVANCE_K 0.4 // Unit: mm compression per 1mm/s extruder speed //#define LA_DEBUG // If enabled, this will generate debug information output over USB. - #define EXPERIMENTAL_SCURVE // Enable this option to permit S-Curve Acceleration + #define EXPERIMENTAL_SCURVE // Enable this option to permit S-Curve Acceleration + //#define ALLOW_LOW_EJERK // Allow a DEFAULT_EJERK value of <10. Recommended for direct drive hotends. #endif // @section leveling @@ -1827,59 +2074,69 @@ /** * Thermal Probe Compensation - * Probe measurements are adjusted to compensate for temperature distortion. - * Use G76 to calibrate this feature. Use M871 to set values manually. - * For a more detailed explanation of the process see G76_M871.cpp. + * + * Adjust probe measurements to compensate for distortion associated with the temperature + * of the probe, bed, and/or hotend. + * Use G76 to automatically calibrate this feature for probe and bed temperatures. + * (Extruder temperature/offset values must be calibrated manually.) + * Use M871 to set temperature/offset values manually. + * For more details see https://marlinfw.org/docs/features/probe_temp_compensation.html */ -#if HAS_BED_PROBE && TEMP_SENSOR_PROBE && TEMP_SENSOR_BED - // Enable thermal first layer compensation using bed and probe temperatures - #define PROBE_TEMP_COMPENSATION +//#define PTC_PROBE // Compensate based on probe temperature +//#define PTC_BED // Compensate based on bed temperature +//#define PTC_HOTEND // Compensate based on hotend temperature - // Add additional compensation depending on hotend temperature - // Note: this values cannot be calibrated and have to be set manually - #if ENABLED(PROBE_TEMP_COMPENSATION) - // Park position to wait for probe cooldown - #define PTC_PARK_POS { 0, 0, 100 } - - // Probe position to probe and wait for probe to reach target temperature - #define PTC_PROBE_POS { 90, 100 } - - // Enable additional compensation using hotend temperature - // Note: this values cannot be calibrated automatically but have to be set manually - //#define USE_TEMP_EXT_COMPENSATION - - // Probe temperature calibration generates a table of values starting at PTC_SAMPLE_START - // (e.g. 30), in steps of PTC_SAMPLE_RES (e.g. 5) with PTC_SAMPLE_COUNT (e.g. 10) samples. - - //#define PTC_SAMPLE_START 30.0f - //#define PTC_SAMPLE_RES 5.0f - //#define PTC_SAMPLE_COUNT 10U +#if ANY(PTC_PROBE, PTC_BED, PTC_HOTEND) + /** + * If the probe is outside the defined range, use linear extrapolation with the closest + * point and the point with index PTC_LINEAR_EXTRAPOLATION. e.g., If set to 4 it will use the + * linear extrapolation between data[0] and data[4] for values below PTC_PROBE_START. + */ + //#define PTC_LINEAR_EXTRAPOLATION 4 + + #if ENABLED(PTC_PROBE) + // Probe temperature calibration generates a table of values starting at PTC_PROBE_START + // (e.g., 30), in steps of PTC_PROBE_RES (e.g., 5) with PTC_PROBE_COUNT (e.g., 10) samples. + #define PTC_PROBE_START 30 // (°C) + #define PTC_PROBE_RES 5 // (°C) + #define PTC_PROBE_COUNT 10 + #define PTC_PROBE_ZOFFS { 0 } // (µm) Z adjustments per sample + #endif + #if ENABLED(PTC_BED) // Bed temperature calibration builds a similar table. + #define PTC_BED_START 60 // (°C) + #define PTC_BED_RES 5 // (°C) + #define PTC_BED_COUNT 10 + #define PTC_BED_ZOFFS { 0 } // (µm) Z adjustments per sample + #endif - //#define BTC_SAMPLE_START 60.0f - //#define BTC_SAMPLE_RES 5.0f - //#define BTC_SAMPLE_COUNT 10U + #if ENABLED(PTC_HOTEND) + // Note: There is no automatic calibration for the hotend. Use M871. + #define PTC_HOTEND_START 180 // (°C) + #define PTC_HOTEND_RES 5 // (°C) + #define PTC_HOTEND_COUNT 20 + #define PTC_HOTEND_ZOFFS { 0 } // (µm) Z adjustments per sample + #endif - // The temperature the probe should be at while taking measurements during bed temperature - // calibration. - //#define BTC_PROBE_TEMP 30.0f + // G76 options + #if BOTH(PTC_PROBE, PTC_BED) + // Park position to wait for probe cooldown + #define PTC_PARK_POS { 0, 0, 100 } - // Height above Z=0.0f to raise the nozzle. Lowering this can help the probe to heat faster. - // Note: the Z=0.0f offset is determined by the probe offset which can be set using M851. - //#define PTC_PROBE_HEATING_OFFSET 0.5f + // Probe position to probe and wait for probe to reach target temperature + //#define PTC_PROBE_POS { 12.0f, 7.3f } // Example: MK52 magnetic heatbed + #define PTC_PROBE_POS { 90, 100 } - // Height to raise the Z-probe between heating and taking the next measurement. Some probes - // may fail to untrigger if they have been triggered for a long time, which can be solved by - // increasing the height the probe is raised to. - //#define PTC_PROBE_RAISE 15U + // The temperature the probe should be at while taking measurements during + // bed temperature calibration. + #define PTC_PROBE_TEMP 30 // (°C) - // If the probe is outside of the defined range, use linear extrapolation using the closest - // point and the PTC_LINEAR_EXTRAPOLATION'th next point. E.g. if set to 4 it will use data[0] - // and data[4] to perform linear extrapolation for values below PTC_SAMPLE_START. - //#define PTC_LINEAR_EXTRAPOLATION 4 + // Height above Z=0.0 to raise the nozzle. Lowering this can help the probe to heat faster. + // Note: The Z=0.0 offset is determined by the probe Z offset (e.g., as set with M851 Z). + #define PTC_PROBE_HEATING_OFFSET 0.5 #endif -#endif +#endif // PTC_PROBE || PTC_BED || PTC_HOTEND // @section extras @@ -1891,20 +2148,23 @@ // // G2/G3 Arc Support // -#define ARC_SUPPORT // Disable this feature to save ~3226 bytes +#define ARC_SUPPORT // Requires ~3226 bytes #if ENABLED(ARC_SUPPORT) - #define MM_PER_ARC_SEGMENT 1 // (mm) Length (or minimum length) of each arc segment - //#define ARC_SEGMENTS_PER_R 1 // Max segment length, MM_PER = Min - #define MIN_ARC_SEGMENTS 24 // Minimum number of segments in a complete circle - //#define ARC_SEGMENTS_PER_SEC 50 // Use feedrate to choose segment length (with MM_PER_ARC_SEGMENT as the minimum) - #define N_ARC_CORRECTION 25 // Number of interpolated segments between corrections - //#define ARC_P_CIRCLES // Enable the 'P' parameter to specify complete circles - //#define CNC_WORKSPACE_PLANES // Allow G2/G3 to operate in XY, ZX, or YZ planes - //#define SF_ARC_FIX // Enable only if using SkeinForge with "Arc Point" fillet procedure + #define MIN_ARC_SEGMENT_MM 0.1 // (mm) Minimum length of each arc segment + #define MAX_ARC_SEGMENT_MM 1.0 // (mm) Maximum length of each arc segment + #define MIN_CIRCLE_SEGMENTS 72 // Minimum number of segments in a complete circle + //#define ARC_SEGMENTS_PER_SEC 50 // Use the feedrate to choose the segment length + #define N_ARC_CORRECTION 25 // Number of interpolated segments between corrections + //#define ARC_P_CIRCLES // Enable the 'P' parameter to specify complete circles + //#define SF_ARC_FIX // Enable only if using SkeinForge with "Arc Point" fillet procedure #endif -// Support for G5 with XYZE destination and IJPQ offsets. Requires ~2666 bytes. -//#define BEZIER_CURVE_SUPPORT +// G5 Bézier Curve Support with XYZE destination and IJPQ offsets +//#define BEZIER_CURVE_SUPPORT // Requires ~2666 bytes + +#if EITHER(ARC_SUPPORT, BEZIER_CURVE_SUPPORT) + //#define CNC_WORKSPACE_PLANES // Allow G2/G3/G5 to operate in XY, ZX, or YZ planes +#endif /** * Direct Stepping @@ -1987,7 +2247,7 @@ // @section motion // The number of linear moves that can be in the planner at once. -// The value of BLOCK_BUFFER_SIZE must be a power of 2 (e.g. 8, 16, 32) +// The value of BLOCK_BUFFER_SIZE must be a power of 2 (e.g., 8, 16, 32) #if BOTH(SDSUPPORT, DIRECT_STEPPING) #define BLOCK_BUFFER_SIZE 8 #elif ENABLED(SDSUPPORT) @@ -2023,9 +2283,6 @@ //#define SERIAL_XON_XOFF #endif -// Add M575 G-code to change the baud rate -//#define BAUD_RATE_GCODE - #if ENABLED(SDSUPPORT) // Enable this option to collect and display the maximum // RX queue usage after transferring a file to SD. @@ -2052,6 +2309,26 @@ */ #define EMERGENCY_PARSER +/** + * Realtime Reporting (requires EMERGENCY_PARSER) + * + * - Report position and state of the machine (like Grbl). + * - Auto-report position during long moves. + * - Useful for CNC/LASER. + * + * Adds support for commands: + * S000 : Report State and Position while moving. + * P000 : Instant Pause / Hold while moving. + * R000 : Resume from Pause / Hold. + * + * - During Hold all Emergency Parser commands are available, as usual. + * - Enable NANODLP_Z_SYNC and NANODLP_ALL_AXIS for move command end-state reports. + */ +//#define REALTIME_REPORTING_COMMANDS +#if ENABLED(REALTIME_REPORTING_COMMANDS) + //#define FULL_REPORT_TO_HOST_FEATURE // Auto-report the machine status like Grbl CNC +#endif + // Bad Serial-connections can miss a received command by sending an 'ok' // Therefore some clients abort after 30 seconds in a timeout. // Some other clients start sending commands while receiving a 'wait'. @@ -2126,6 +2403,20 @@ //#define EVENT_GCODE_AFTER_TOOLCHANGE "G12X" // Extra G-code to run after tool-change #endif + /** + * Extra G-code to run while executing tool-change commands. Can be used to use an additional + * stepper motor (I axis, see option LINEAR_AXES in Configuration.h) to drive the tool-changer. + */ + //#define EVENT_GCODE_TOOLCHANGE_T0 "G28 A\nG1 A0" // Extra G-code to run while executing tool-change command T0 + //#define EVENT_GCODE_TOOLCHANGE_T1 "G1 A10" // Extra G-code to run while executing tool-change command T1 + //#define EVENT_GCODE_TOOLCHANGE_ALWAYS_RUN // Always execute above G-code sequences. Use with caution! + + /** + * Tool Sensors detect when tools have been picked up or dropped. + * Requires the pins TOOL_SENSOR1_PIN, TOOL_SENSOR2_PIN, etc. + */ + //#define TOOL_SENSOR + /** * Retract and prime filament on tool-change to reduce * ooze and stringing and to get cleaner transitions. @@ -2141,7 +2432,7 @@ // Longer prime to clean out a SINGLENOZZLE #define TOOLCHANGE_FS_EXTRA_PRIME 0 // (mm) Extra priming length #define TOOLCHANGE_FS_PRIME_SPEED (4.6*60) // (mm/min) Extra priming feedrate - #define TOOLCHANGE_FS_WIPE_RETRACT 0 // (mm/min) Retract before cooling for less stringing, better wipe, etc. + #define TOOLCHANGE_FS_WIPE_RETRACT 0 // (mm) Retract before cooling for less stringing, better wipe, etc. // Cool after prime to reduce stringing #define TOOLCHANGE_FS_FAN -1 // Fan index or -1 to skip @@ -2184,14 +2475,15 @@ #endif // HAS_MULTI_EXTRUDER /** - * Advanced Pause - * Experimental feature for filament change support and for parking the nozzle when paused. - * Adds the GCode M600 for initiating filament change. - * If PARK_HEAD_ON_PAUSE enabled, adds the GCode M125 to pause printing and park the nozzle. + * Advanced Pause for Filament Change + * - Adds the G-code M600 Filament Change to initiate a filament change. + * - This feature is required for the default FILAMENT_RUNOUT_SCRIPT. + * + * Requirements: + * - For Filament Change parking enable and configure NOZZLE_PARK_FEATURE. + * - For user interaction enable an LCD display, HOST_PROMPT_SUPPORT, or EMERGENCY_PARSER. * - * Requires an LCD display. - * Requires NOZZLE_PARK_FEATURE. - * This feature is required for the default FILAMENT_RUNOUT_SCRIPT. + * Enable PARK_HEAD_ON_PAUSE to add the G-code M125 Pause and Park. */ #define ADVANCED_PAUSE_FEATURE #if ENABLED(ADVANCED_PAUSE_FEATURE) @@ -2230,6 +2522,8 @@ #define PAUSE_PARK_NOZZLE_TIMEOUT 45 // (seconds) Time limit before the nozzle is turned off for safety. #define FILAMENT_CHANGE_ALERT_BEEPS 10 // Number of alert beeps to play when a response is needed. #define PAUSE_PARK_NO_STEPPER_TIMEOUT // Enable for XYZ steppers to stay powered on during filament change. + //#define FILAMENT_CHANGE_RESUME_ON_INSERT // Automatically continue / load filament when runout sensor is triggered again. + //#define PAUSE_REHEAT_FAST_RESUME // Reduce number of waits by not prompting again post-timeout before continuing. #define PARK_HEAD_ON_PAUSE // Park the nozzle during pause and filament change. #define HOME_BEFORE_FILAMENT_CHANGE // If needed, home before parking for filament change @@ -2296,6 +2590,24 @@ #define Z4_MICROSTEPS Z_MICROSTEPS #endif + #if AXIS_DRIVER_TYPE_I(TMC26X) + #define I_MAX_CURRENT 1000 + #define I_SENSE_RESISTOR 91 + #define I_MICROSTEPS 16 + #endif + + #if AXIS_DRIVER_TYPE_J(TMC26X) + #define J_MAX_CURRENT 1000 + #define J_SENSE_RESISTOR 91 + #define J_MICROSTEPS 16 + #endif + + #if AXIS_DRIVER_TYPE_K(TMC26X) + #define K_MAX_CURRENT 1000 + #define K_SENSE_RESISTOR 91 + #define K_MICROSTEPS 16 + #endif + #if AXIS_DRIVER_TYPE_E0(TMC26X) #define E0_MAX_CURRENT 1000 #define E0_SENSE_RESISTOR 91 @@ -2381,6 +2693,7 @@ #define X_RSENSE 0.11 #define X_CHAIN_POS -1 // -1..0: Not chained. 1: MCU MOSI connected. 2: Next in chain, ... //#define X_INTERPOLATE true // Enable to override 'INTERPOLATE' for the X axis + //#define X_HOLD_MULTIPLIER 0.5 // Enable to override 'HOLD_MULTIPLIER' for the X axis #endif #if AXIS_IS_TMC(X2) @@ -2390,6 +2703,7 @@ #define X2_RSENSE 0.11 #define X2_CHAIN_POS -1 //#define X2_INTERPOLATE true + //#define X2_HOLD_MULTIPLIER 0.5 #endif #if AXIS_IS_TMC(Y) @@ -2399,6 +2713,7 @@ #define Y_RSENSE 0.11 #define Y_CHAIN_POS -1 //#define Y_INTERPOLATE true + //#define Y_HOLD_MULTIPLIER 0.5 #endif #if AXIS_IS_TMC(Y2) @@ -2408,6 +2723,7 @@ #define Y2_RSENSE 0.11 #define Y2_CHAIN_POS -1 //#define Y2_INTERPOLATE true + //#define Y2_HOLD_MULTIPLIER 0.5 #endif #if AXIS_IS_TMC(Z) @@ -2417,6 +2733,7 @@ #define Z_RSENSE 0.11 #define Z_CHAIN_POS -1 //#define Z_INTERPOLATE true + //#define Z_HOLD_MULTIPLIER 0.5 #endif #if AXIS_IS_TMC(Z2) @@ -2426,6 +2743,7 @@ #define Z2_RSENSE 0.11 #define Z2_CHAIN_POS -1 //#define Z2_INTERPOLATE true + //#define Z2_HOLD_MULTIPLIER 0.5 #endif #if AXIS_IS_TMC(Z3) @@ -2435,6 +2753,7 @@ #define Z3_RSENSE 0.11 #define Z3_CHAIN_POS -1 //#define Z3_INTERPOLATE true + //#define Z3_HOLD_MULTIPLIER 0.5 #endif #if AXIS_IS_TMC(Z4) @@ -2444,6 +2763,37 @@ #define Z4_RSENSE 0.11 #define Z4_CHAIN_POS -1 //#define Z4_INTERPOLATE true + //#define Z4_HOLD_MULTIPLIER 0.5 + #endif + + #if AXIS_IS_TMC(I) + #define I_CURRENT 800 + #define I_CURRENT_HOME I_CURRENT + #define I_MICROSTEPS 16 + #define I_RSENSE 0.11 + #define I_CHAIN_POS -1 + //#define I_INTERPOLATE true + //#define I_HOLD_MULTIPLIER 0.5 + #endif + + #if AXIS_IS_TMC(J) + #define J_CURRENT 800 + #define J_CURRENT_HOME J_CURRENT + #define J_MICROSTEPS 16 + #define J_RSENSE 0.11 + #define J_CHAIN_POS -1 + //#define J_INTERPOLATE true + //#define J_HOLD_MULTIPLIER 0.5 + #endif + + #if AXIS_IS_TMC(K) + #define K_CURRENT 800 + #define K_CURRENT_HOME K_CURRENT + #define K_MICROSTEPS 16 + #define K_RSENSE 0.11 + #define K_CHAIN_POS -1 + //#define K_INTERPOLATE true + //#define K_HOLD_MULTIPLIER 0.5 #endif #if AXIS_IS_TMC(E0) @@ -2452,62 +2802,70 @@ #define E0_RSENSE 0.11 #define E0_CHAIN_POS -1 //#define E0_INTERPOLATE true + //#define E0_HOLD_MULTIPLIER 0.5 #endif #if AXIS_IS_TMC(E1) #define E1_CURRENT 800 - #define E1_MICROSTEPS E0_MICROSTEPS + #define E1_MICROSTEPS E0_MICROSTEPS #define E1_RSENSE 0.11 #define E1_CHAIN_POS -1 //#define E1_INTERPOLATE true + //#define E1_HOLD_MULTIPLIER 0.5 #endif #if AXIS_IS_TMC(E2) #define E2_CURRENT 800 - #define E2_MICROSTEPS E0_MICROSTEPS + #define E2_MICROSTEPS E0_MICROSTEPS #define E2_RSENSE 0.11 #define E2_CHAIN_POS -1 //#define E2_INTERPOLATE true + //#define E2_HOLD_MULTIPLIER 0.5 #endif #if AXIS_IS_TMC(E3) #define E3_CURRENT 800 - #define E3_MICROSTEPS E0_MICROSTEPS + #define E3_MICROSTEPS E0_MICROSTEPS #define E3_RSENSE 0.11 #define E3_CHAIN_POS -1 //#define E3_INTERPOLATE true + //#define E3_HOLD_MULTIPLIER 0.5 #endif #if AXIS_IS_TMC(E4) #define E4_CURRENT 800 - #define E4_MICROSTEPS E0_MICROSTEPS + #define E4_MICROSTEPS E0_MICROSTEPS #define E4_RSENSE 0.11 #define E4_CHAIN_POS -1 //#define E4_INTERPOLATE true + //#define E4_HOLD_MULTIPLIER 0.5 #endif #if AXIS_IS_TMC(E5) #define E5_CURRENT 800 - #define E5_MICROSTEPS E0_MICROSTEPS + #define E5_MICROSTEPS E0_MICROSTEPS #define E5_RSENSE 0.11 #define E5_CHAIN_POS -1 //#define E5_INTERPOLATE true + //#define E5_HOLD_MULTIPLIER 0.5 #endif #if AXIS_IS_TMC(E6) #define E6_CURRENT 800 - #define E6_MICROSTEPS E0_MICROSTEPS + #define E6_MICROSTEPS E0_MICROSTEPS #define E6_RSENSE 0.11 #define E6_CHAIN_POS -1 //#define E6_INTERPOLATE true + //#define E6_HOLD_MULTIPLIER 0.5 #endif #if AXIS_IS_TMC(E7) #define E7_CURRENT 800 - #define E7_MICROSTEPS E0_MICROSTEPS + #define E7_MICROSTEPS E0_MICROSTEPS #define E7_RSENSE 0.11 #define E7_CHAIN_POS -1 //#define E7_INTERPOLATE true + //#define E7_HOLD_MULTIPLIER 0.5 #endif /** @@ -2521,6 +2879,10 @@ //#define Y2_CS_PIN -1 //#define Z2_CS_PIN -1 //#define Z3_CS_PIN -1 + //#define Z4_CS_PIN -1 + //#define I_CS_PIN -1 + //#define J_CS_PIN -1 + //#define K_CS_PIN -1 //#define E0_CS_PIN -1 //#define E1_CS_PIN -1 //#define E2_CS_PIN -1 @@ -2560,6 +2922,9 @@ //#define Z2_SLAVE_ADDRESS 0 //#define Z3_SLAVE_ADDRESS 0 //#define Z4_SLAVE_ADDRESS 0 + //#define I_SLAVE_ADDRESS 0 + //#define J_SLAVE_ADDRESS 0 + //#define K_SLAVE_ADDRESS 0 //#define E0_SLAVE_ADDRESS 0 //#define E1_SLAVE_ADDRESS 0 //#define E2_SLAVE_ADDRESS 0 @@ -2584,6 +2949,9 @@ */ #define STEALTHCHOP_XY #define STEALTHCHOP_Z + #define STEALTHCHOP_I + #define STEALTHCHOP_J + #define STEALTHCHOP_K #define STEALTHCHOP_E /** @@ -2602,22 +2970,25 @@ * { , , hysteresis_start[1..8] } */ #define CHOPPER_TIMING CHOPPER_DEFAULT_24V // All axes (override below) - //#define CHOPPER_TIMING_X CHOPPER_DEFAULT_12V // For X Axes (override below) - //#define CHOPPER_TIMING_X2 CHOPPER_DEFAULT_12V - //#define CHOPPER_TIMING_Y CHOPPER_DEFAULT_12V // For Y Axes (override below) - //#define CHOPPER_TIMING_Y2 CHOPPER_DEFAULT_12V - //#define CHOPPER_TIMING_Z CHOPPER_DEFAULT_12V // For Z Axes (override below) - //#define CHOPPER_TIMING_Z2 CHOPPER_DEFAULT_12V - //#define CHOPPER_TIMING_Z3 CHOPPER_DEFAULT_12V - //#define CHOPPER_TIMING_Z4 CHOPPER_DEFAULT_12V - //#define CHOPPER_TIMING_E CHOPPER_DEFAULT_12V // For Extruders (override below) - //#define CHOPPER_TIMING_E1 CHOPPER_DEFAULT_12V - //#define CHOPPER_TIMING_E2 CHOPPER_DEFAULT_12V - //#define CHOPPER_TIMING_E3 CHOPPER_DEFAULT_12V - //#define CHOPPER_TIMING_E4 CHOPPER_DEFAULT_12V - //#define CHOPPER_TIMING_E5 CHOPPER_DEFAULT_12V - //#define CHOPPER_TIMING_E6 CHOPPER_DEFAULT_12V - //#define CHOPPER_TIMING_E7 CHOPPER_DEFAULT_12V + //#define CHOPPER_TIMING_X CHOPPER_TIMING // For X Axes (override below) + //#define CHOPPER_TIMING_X2 CHOPPER_TIMING_X + //#define CHOPPER_TIMING_Y CHOPPER_TIMING // For Y Axes (override below) + //#define CHOPPER_TIMING_Y2 CHOPPER_TIMING_Y + //#define CHOPPER_TIMING_Z CHOPPER_TIMING // For Z Axes (override below) + //#define CHOPPER_TIMING_Z2 CHOPPER_TIMING_Z + //#define CHOPPER_TIMING_Z3 CHOPPER_TIMING_Z + //#define CHOPPER_TIMING_Z4 CHOPPER_TIMING_Z + //#define CHOPPER_TIMING_I CHOPPER_TIMING + //#define CHOPPER_TIMING_J CHOPPER_TIMING + //#define CHOPPER_TIMING_K CHOPPER_TIMING + //#define CHOPPER_TIMING_E CHOPPER_TIMING // For Extruders (override below) + //#define CHOPPER_TIMING_E1 CHOPPER_TIMING_E + //#define CHOPPER_TIMING_E2 CHOPPER_TIMING_E + //#define CHOPPER_TIMING_E3 CHOPPER_TIMING_E + //#define CHOPPER_TIMING_E4 CHOPPER_TIMING_E + //#define CHOPPER_TIMING_E5 CHOPPER_TIMING_E + //#define CHOPPER_TIMING_E6 CHOPPER_TIMING_E + //#define CHOPPER_TIMING_E7 CHOPPER_TIMING_E /** * Monitor Trinamic drivers @@ -2655,6 +3026,9 @@ #define Z2_HYBRID_THRESHOLD 3 #define Z3_HYBRID_THRESHOLD 3 #define Z4_HYBRID_THRESHOLD 3 + #define I_HYBRID_THRESHOLD 3 + #define J_HYBRID_THRESHOLD 3 + #define K_HYBRID_THRESHOLD 3 #define E0_HYBRID_THRESHOLD 30 #define E1_HYBRID_THRESHOLD 30 #define E2_HYBRID_THRESHOLD 30 @@ -2680,7 +3054,7 @@ * * It is recommended to set HOMING_BUMP_MM to { 0, 0, 0 }. * - * SPI_ENDSTOPS *** Beta feature! *** TMC2130 Only *** + * SPI_ENDSTOPS *** Beta feature! *** TMC2130/TMC5160 Only *** * Poll the driver through SPI to determine load when homing. * Removes the need for a wire from DIAG1 to an endstop pin. * @@ -2701,6 +3075,9 @@ //#define Z2_STALL_SENSITIVITY Z_STALL_SENSITIVITY //#define Z3_STALL_SENSITIVITY Z_STALL_SENSITIVITY //#define Z4_STALL_SENSITIVITY Z_STALL_SENSITIVITY + //#define I_STALL_SENSITIVITY 8 + //#define J_STALL_SENSITIVITY 8 + //#define K_STALL_SENSITIVITY 8 //#define SPI_ENDSTOPS // TMC2130 only #define IMPROVE_HOMING_RELIABILITY #endif @@ -2725,7 +3102,7 @@ /** * Enable M122 debugging command for TMC stepper drivers. - * M122 S0/1 will enable continous reporting. + * M122 S0/1 will enable continuous reporting. */ #define TMC_DEBUG @@ -2841,6 +3218,33 @@ #define Z4_SLEW_RATE 1 #endif + #if AXIS_IS_L64XX(I) + #define I_MICROSTEPS 128 + #define I_OVERCURRENT 2000 + #define I_STALLCURRENT 1500 + #define I_MAX_VOLTAGE 127 + #define I_CHAIN_POS -1 + #define I_SLEW_RATE 1 + #endif + + #if AXIS_IS_L64XX(J) + #define J_MICROSTEPS 128 + #define J_OVERCURRENT 2000 + #define J_STALLCURRENT 1500 + #define J_MAX_VOLTAGE 127 + #define J_CHAIN_POS -1 + #define J_SLEW_RATE 1 + #endif + + #if AXIS_IS_L64XX(K) + #define K_MICROSTEPS 128 + #define K_OVERCURRENT 2000 + #define K_STALLCURRENT 1500 + #define K_MAX_VOLTAGE 127 + #define K_CHAIN_POS -1 + #define K_SLEW_RATE 1 + #endif + #if AXIS_IS_L64XX(E0) #define E0_MICROSTEPS 128 #define E0_OVERCURRENT 2000 @@ -3040,16 +3444,30 @@ //#define SPINDLE_FEATURE //#define LASER_FEATURE #if EITHER(SPINDLE_FEATURE, LASER_FEATURE) - #define SPINDLE_LASER_ACTIVE_STATE LOW // Set to "HIGH" if the on/off function is active HIGH - #define SPINDLE_LASER_PWM true // Set to "true" if your controller supports setting the speed/power - #define SPINDLE_LASER_PWM_INVERT false // Set to "true" if the speed/power goes up when you want it to go slower + #define SPINDLE_LASER_ACTIVE_STATE LOW // Set to "HIGH" if SPINDLE_LASER_ENA_PIN is active HIGH - #define SPINDLE_LASER_FREQUENCY 2500 // (Hz) Spindle/laser frequency (only on supported HALs: AVR and LPC) + #define SPINDLE_LASER_USE_PWM // Enable if your controller supports setting the speed/power + #if ENABLED(SPINDLE_LASER_USE_PWM) + #define SPINDLE_LASER_PWM_INVERT false // Set to "true" if the speed/power goes up when you want it to go slower + #define SPINDLE_LASER_FREQUENCY 2500 // (Hz) Spindle/laser frequency (only on supported HALs: AVR and LPC) + #endif + + //#define AIR_EVACUATION // Cutter Vacuum / Laser Blower motor control with G-codes M10-M11 + #if ENABLED(AIR_EVACUATION) + #define AIR_EVACUATION_ACTIVE LOW // Set to "HIGH" if the on/off function is active HIGH + //#define AIR_EVACUATION_PIN 42 // Override the default Cutter Vacuum or Laser Blower pin + #endif + + //#define AIR_ASSIST // Air Assist control with G-codes M8-M9 + #if ENABLED(AIR_ASSIST) + #define AIR_ASSIST_ACTIVE LOW // Active state on air assist pin + //#define AIR_ASSIST_PIN 44 // Override the default Air Assist pin + #endif - //#define SPINDLE_SERVO // A servo converting an angle to spindle power + //#define SPINDLE_SERVO // A servo converting an angle to spindle power #ifdef SPINDLE_SERVO - #define SPINDLE_SERVO_NR 0 // Index of servo used for spindle control - #define SPINDLE_SERVO_MIN 10 // Minimum angle for servo spindle + #define SPINDLE_SERVO_NR 0 // Index of servo used for spindle control + #define SPINDLE_SERVO_MIN 10 // Minimum angle for servo spindle #endif /** @@ -3088,17 +3506,21 @@ * Speed/Power = (PWMDC / 255 * 100 - SPEED_POWER_INTERCEPT) / SPEED_POWER_SLOPE * PWMDC = (spdpwr - SPEED_POWER_MIN) / (SPEED_POWER_MAX - SPEED_POWER_MIN) / SPEED_POWER_SLOPE */ - #define SPEED_POWER_INTERCEPT 0 // (%) 0-100 i.e., Minimum power percentage - #define SPEED_POWER_MIN 5000 // (RPM) - #define SPEED_POWER_MAX 30000 // (RPM) SuperPID router controller 0 - 30,000 RPM - #define SPEED_POWER_STARTUP 25000 // (RPM) M3/M4 speed/power default (with no arguments) + #if ENABLED(SPINDLE_LASER_USE_PWM) + #define SPEED_POWER_INTERCEPT 0 // (%) 0-100 i.e., Minimum power percentage + #define SPEED_POWER_MIN 5000 // (RPM) + #define SPEED_POWER_MAX 30000 // (RPM) SuperPID router controller 0 - 30,000 RPM + #define SPEED_POWER_STARTUP 25000 // (RPM) M3/M4 speed/power default (with no arguments) + #endif #else - #define SPEED_POWER_INTERCEPT 0 // (%) 0-100 i.e., Minimum power percentage - #define SPEED_POWER_MIN 0 // (%) 0-100 - #define SPEED_POWER_MAX 100 // (%) 0-100 - #define SPEED_POWER_STARTUP 80 // (%) M3/M4 speed/power default (with no arguments) + #if ENABLED(SPINDLE_LASER_USE_PWM) + #define SPEED_POWER_INTERCEPT 0 // (%) 0-100 i.e., Minimum power percentage + #define SPEED_POWER_MIN 0 // (%) 0-100 + #define SPEED_POWER_MAX 100 // (%) 0-100 + #define SPEED_POWER_STARTUP 80 // (%) M3/M4 speed/power default (with no arguments) + #endif // Define the minimum and maximum test pulse time values for a laser test fire function #define LASER_TEST_PULSE_MIN 1 // Used with Laser Control Menu @@ -3178,8 +3600,30 @@ #define SPINDLE_LASER_POWERDOWN_DELAY 50 // (ms) Delay to allow the spindle to stop #endif + + // + // Laser I2C Ammeter (High precision INA226 low/high side module) + // + //#define I2C_AMMETER + #if ENABLED(I2C_AMMETER) + #define I2C_AMMETER_IMAX 0.1 // (Amps) Calibration value for the expected current range + #define I2C_AMMETER_SHUNT_RESISTOR 0.1 // (Ohms) Calibration shunt resistor value + #endif + #endif -#endif +#endif // SPINDLE_FEATURE || LASER_FEATURE + +/** + * Synchronous Laser Control with M106/M107 + * + * Marlin normally applies M106/M107 fan speeds at a time "soon after" processing + * a planner block. This is too inaccurate for a PWM/TTL laser attached to the fan + * header (as with some add-on laser kits). Enable this option to set fan/laser + * speeds with much more exact timing for improved print fidelity. + * + * NOTE: This option sacrifices some cooling fan speed options. + */ +//#define LASER_SYNCHRONOUS_M106_M107 /** * Synchronous Laser Control with M106/M107 @@ -3250,13 +3694,27 @@ */ //#define POWER_MONITOR_CURRENT // Monitor the system current //#define POWER_MONITOR_VOLTAGE // Monitor the system voltage -#if EITHER(POWER_MONITOR_CURRENT, POWER_MONITOR_VOLTAGE) - #define POWER_MONITOR_VOLTS_PER_AMP 0.05000 // Input voltage to the MCU analog pin per amp - DO NOT apply more than ADC_VREF! - #define POWER_MONITOR_CURRENT_OFFSET -1 // Offset value for current sensors with linear function output - #define POWER_MONITOR_VOLTS_PER_VOLT 0.11786 // Input voltage to the MCU analog pin per volt - DO NOT apply more than ADC_VREF! + +#if ENABLED(POWER_MONITOR_CURRENT) + #define POWER_MONITOR_VOLTS_PER_AMP 0.05000 // Input voltage to the MCU analog pin per amp - DO NOT apply more than ADC_VREF! + #define POWER_MONITOR_CURRENT_OFFSET 0 // Offset (in amps) applied to the calculated current #define POWER_MONITOR_FIXED_VOLTAGE 13.6 // Voltage for a current sensor with no voltage sensor (for power display) #endif +#if ENABLED(POWER_MONITOR_VOLTAGE) + #define POWER_MONITOR_VOLTS_PER_VOLT 0.077933 // Input voltage to the MCU analog pin per volt - DO NOT apply more than ADC_VREF! + #define POWER_MONITOR_VOLTAGE_OFFSET 0 // Offset (in volts) applied to the calculated voltage +#endif + +/** + * Stepper Driver Anti-SNAFU Protection + * + * If the SAFE_POWER_PIN is defined for your board, Marlin will check + * that stepper drivers are properly plugged in before applying power. + * Disable protection if your stepper drivers don't support the feature. + */ +//#define DISABLE_DRIVER_SAFE_POWER_PROTECT + /** * CNC Coordinate Systems * @@ -3265,11 +3723,22 @@ */ //#define CNC_COORDINATE_SYSTEMS +/** + * Auto-report fan speed with M123 S + * Requires fans with tachometer pins + */ +//#define AUTO_REPORT_FANS + /** * Auto-report temperatures with M155 S */ #define AUTO_REPORT_TEMPERATURES +/** + * Auto-report position with M154 S + */ +//#define AUTO_REPORT_POSITION + /** * Include capabilities in M115 output */ @@ -3339,7 +3808,7 @@ #define PROPORTIONAL_FONT_RATIO 1.0 /** - * Spend 28 bytes of SRAM to optimize the GCode parser + * Spend 28 bytes of SRAM to optimize the G-code parser */ #define FASTER_GCODE_PARSER @@ -3347,7 +3816,9 @@ //#define GCODE_QUOTED_STRINGS // Support for quoted string parameters #endif -//#define MEATPACK // Support for MeatPack G-code compression (https://github.com/scottmudge/OctoPrint-MeatPack) +// Support for MeatPack G-code compression (https://github.com/scottmudge/OctoPrint-MeatPack) +//#define MEATPACK_ON_SERIAL_PORT_1 +//#define MEATPACK_ON_SERIAL_PORT_2 //#define GCODE_CASE_INSENSITIVE // Accept G-code sent to the firmware in lowercase @@ -3387,6 +3858,71 @@ #define GCODE_MACROS_SLOT_SIZE 50 // Maximum length of a single macro #endif +/** + * User-defined menu items to run custom G-code. + * Up to 25 may be defined, but the actual number is LCD-dependent. + */ + +// Custom Menu: Main Menu +//#define CUSTOM_MENU_MAIN +#if ENABLED(CUSTOM_MENU_MAIN) + //#define CUSTOM_MENU_MAIN_TITLE "Custom Commands" + #define CUSTOM_MENU_MAIN_SCRIPT_DONE "M117 User Script Done" + #define CUSTOM_MENU_MAIN_SCRIPT_AUDIBLE_FEEDBACK + //#define CUSTOM_MENU_MAIN_SCRIPT_RETURN // Return to status screen after a script + #define CUSTOM_MENU_MAIN_ONLY_IDLE // Only show custom menu when the machine is idle + + #define MAIN_MENU_ITEM_1_DESC "Home & UBL Info" + #define MAIN_MENU_ITEM_1_GCODE "G28\nG29 W" + //#define MAIN_MENU_ITEM_1_CONFIRM // Show a confirmation dialog before this action + + #define MAIN_MENU_ITEM_2_DESC "Preheat for " PREHEAT_1_LABEL + #define MAIN_MENU_ITEM_2_GCODE "M140 S" STRINGIFY(PREHEAT_1_TEMP_BED) "\nM104 S" STRINGIFY(PREHEAT_1_TEMP_HOTEND) + //#define MAIN_MENU_ITEM_2_CONFIRM + + //#define MAIN_MENU_ITEM_3_DESC "Preheat for " PREHEAT_2_LABEL + //#define MAIN_MENU_ITEM_3_GCODE "M140 S" STRINGIFY(PREHEAT_2_TEMP_BED) "\nM104 S" STRINGIFY(PREHEAT_2_TEMP_HOTEND) + //#define MAIN_MENU_ITEM_3_CONFIRM + + //#define MAIN_MENU_ITEM_4_DESC "Heat Bed/Home/Level" + //#define MAIN_MENU_ITEM_4_GCODE "M140 S" STRINGIFY(PREHEAT_2_TEMP_BED) "\nG28\nG29" + //#define MAIN_MENU_ITEM_4_CONFIRM + + //#define MAIN_MENU_ITEM_5_DESC "Home & Info" + //#define MAIN_MENU_ITEM_5_GCODE "G28\nM503" + //#define MAIN_MENU_ITEM_5_CONFIRM +#endif + +// Custom Menu: Configuration Menu +//#define CUSTOM_MENU_CONFIG +#if ENABLED(CUSTOM_MENU_CONFIG) + //#define CUSTOM_MENU_CONFIG_TITLE "Custom Commands" + #define CUSTOM_MENU_CONFIG_SCRIPT_DONE "M117 Wireless Script Done" + #define CUSTOM_MENU_CONFIG_SCRIPT_AUDIBLE_FEEDBACK + //#define CUSTOM_MENU_CONFIG_SCRIPT_RETURN // Return to status screen after a script + #define CUSTOM_MENU_CONFIG_ONLY_IDLE // Only show custom menu when the machine is idle + + #define CONFIG_MENU_ITEM_1_DESC "Wifi ON" + #define CONFIG_MENU_ITEM_1_GCODE "M118 [ESP110] WIFI-STA pwd=12345678" + //#define CONFIG_MENU_ITEM_1_CONFIRM // Show a confirmation dialog before this action + + #define CONFIG_MENU_ITEM_2_DESC "Bluetooth ON" + #define CONFIG_MENU_ITEM_2_GCODE "M118 [ESP110] BT pwd=12345678" + //#define CONFIG_MENU_ITEM_2_CONFIRM + + //#define CONFIG_MENU_ITEM_3_DESC "Radio OFF" + //#define CONFIG_MENU_ITEM_3_GCODE "M118 [ESP110] OFF pwd=12345678" + //#define CONFIG_MENU_ITEM_3_CONFIRM + + //#define CONFIG_MENU_ITEM_4_DESC "Wifi ????" + //#define CONFIG_MENU_ITEM_4_GCODE "M118 ????" + //#define CONFIG_MENU_ITEM_4_CONFIRM + + //#define CONFIG_MENU_ITEM_5_DESC "Wifi ????" + //#define CONFIG_MENU_ITEM_5_GCODE "M118 ????" + //#define CONFIG_MENU_ITEM_5_CONFIRM +#endif + /** * User-defined buttons to run custom G-code. * Up to 25 may be defined. @@ -3394,7 +3930,7 @@ //#define CUSTOM_USER_BUTTONS #if ENABLED(CUSTOM_USER_BUTTONS) //#define BUTTON1_PIN -1 - #if PIN_EXISTS(BUTTON1_PIN) + #if PIN_EXISTS(BUTTON1) #define BUTTON1_HIT_STATE LOW // State of the triggered button. NC=LOW. NO=HIGH. #define BUTTON1_WHEN_PRINTING false // Button allowed to trigger during printing? #define BUTTON1_GCODE "G28" @@ -3402,7 +3938,7 @@ #endif //#define BUTTON2_PIN -1 - #if PIN_EXISTS(BUTTON2_PIN) + #if PIN_EXISTS(BUTTON2) #define BUTTON2_HIT_STATE LOW #define BUTTON2_WHEN_PRINTING false #define BUTTON2_GCODE "M140 S" STRINGIFY(PREHEAT_1_TEMP_BED) "\nM104 S" STRINGIFY(PREHEAT_1_TEMP_HOTEND) @@ -3410,7 +3946,7 @@ #endif //#define BUTTON3_PIN -1 - #if PIN_EXISTS(BUTTON3_PIN) + #if PIN_EXISTS(BUTTON3) #define BUTTON3_HIT_STATE LOW #define BUTTON3_WHEN_PRINTING false #define BUTTON3_GCODE "M140 S" STRINGIFY(PREHEAT_2_TEMP_BED) "\nM104 S" STRINGIFY(PREHEAT_2_TEMP_HOTEND) @@ -3418,33 +3954,6 @@ #endif #endif -/** - * User-defined menu items to run custom G-code. - * Up to 25 may be defined, but the actual number is LCD-dependent. - */ -//#define CUSTOM_USER_MENUS -#if ENABLED(CUSTOM_USER_MENUS) - //#define CUSTOM_USER_MENU_TITLE "Custom Commands" - #define USER_SCRIPT_DONE "M117 User Script Done" - #define USER_SCRIPT_AUDIBLE_FEEDBACK - //#define USER_SCRIPT_RETURN // Return to status screen after a script - - #define USER_DESC_1 "Home & UBL Info" - #define USER_GCODE_1 "G28\nG29W" - - #define USER_DESC_2 "Preheat for " PREHEAT_1_LABEL - #define USER_GCODE_2 "M140 S" STRINGIFY(PREHEAT_1_TEMP_BED) "\nM104 S" STRINGIFY(PREHEAT_1_TEMP_HOTEND) - - #define USER_DESC_3 "Preheat for " PREHEAT_2_LABEL - #define USER_GCODE_3 "M140 S" STRINGIFY(PREHEAT_2_TEMP_BED) "\nM104 S" STRINGIFY(PREHEAT_2_TEMP_HOTEND) - - #define USER_DESC_4 "Heat Bed/Home/Level" - #define USER_GCODE_4 "M140 S" STRINGIFY(PREHEAT_2_TEMP_BED) "\nG28\nG29" - - #define USER_DESC_5 "Home & Info" - #define USER_GCODE_5 "G28\nM503" -#endif - /** * Host Action Commands * @@ -3461,8 +3970,13 @@ */ //#define HOST_ACTION_COMMANDS #if ENABLED(HOST_ACTION_COMMANDS) - //#define HOST_PROMPT_SUPPORT - //#define HOST_START_MENU_ITEM // Add a menu item that tells the host to start + //#define HOST_PAUSE_M76 // Tell the host to pause in response to M76 + //#define HOST_PROMPT_SUPPORT // Initiate host prompts to get user feedback + #if ENABLED(HOST_PROMPT_SUPPORT) + //#define HOST_STATUS_NOTIFICATIONS // Send some status messages to the host as notifications + #endif + //#define HOST_START_MENU_ITEM // Add a menu item that tells the host to start + //#define HOST_SHUTDOWN_MENU_ITEM // Add a menu item that tells the host to shut down #endif /** @@ -3471,6 +3985,9 @@ * Implement M486 to allow Marlin to skip objects */ //#define CANCEL_OBJECTS +#if ENABLED(CANCEL_OBJECTS) + #define CANCEL_OBJECTS_REPORTING // Emit the current object as a status message +#endif /** * I2C position encoders for closed loop control. @@ -3547,7 +4064,7 @@ */ #define I2CPE_MIN_UPD_TIME_MS 4 // (ms) Minimum time between encoder checks. - // Use a rolling average to identify persistant errors that indicate skips, as opposed to vibration and noise. + // Use a rolling average to identify persistent errors that indicate skips, as opposed to vibration and noise. #define I2CPE_ERR_ROLLING_AVERAGE #endif // I2C_POSITION_ENCODERS @@ -3592,6 +4109,16 @@ #define GANTRY_CALIBRATION_COMMANDS_POST "G28" // G28 highly recommended to ensure an accurate position #endif +/** + * Instant freeze / unfreeze functionality + * Specified pin has pullup and connecting to ground will instantly pause motion. + * Potentially useful for emergency stop that allows being resumed. + */ +//#define FREEZE_FEATURE +#if ENABLED(FREEZE_FEATURE) + //#define FREEZE_PIN 41 // Override the default (KILL) pin here +#endif + /** * MAX7219 Debug Matrix * @@ -3628,14 +4155,13 @@ /** * NanoDLP Sync support * - * Add support for Synchronized Z moves when using with NanoDLP. G0/G1 axis moves will output "Z_move_comp" - * string to enable synchronization with DLP projector exposure. This change will allow to use - * [[WaitForDoneMessage]] instead of populating your gcode with M400 commands + * Support for Synchronized Z moves when used with NanoDLP. G0/G1 axis moves will + * output a "Z_move_comp" string to enable synchronization with DLP projector exposure. + * This feature allows you to use [[WaitForDoneMessage]] instead of M400 commands. */ //#define NANODLP_Z_SYNC #if ENABLED(NANODLP_Z_SYNC) - //#define NANODLP_ALL_AXIS // Enables "Z_move_comp" output on any axis move. - // Default behavior is limited to Z axis only. + //#define NANODLP_ALL_AXIS // Send a "Z_move_comp" report for any axis move (not just Z). #endif /** @@ -3804,9 +4330,26 @@ // Enable Marlin dev mode which adds some special commands //#define MARLIN_DEV_MODE +#if ENABLED(MARLIN_DEV_MODE) + /** + * D576 - Buffer Monitoring + * To help diagnose print quality issues stemming from empty command buffers. + */ + //#define BUFFER_MONITORING +#endif + /** * Postmortem Debugging captures misbehavior and outputs the CPU status and backtrace to serial. * When running in the debugger it will break for debugging. This is useful to help understand * a crash from a remote location. Requires ~400 bytes of SRAM and 5Kb of flash. */ //#define POSTMORTEM_DEBUGGING + +/** + * Software Reset options + */ +//#define SOFT_RESET_VIA_SERIAL // 'KILL' and '^X' commands will soft-reset the controller +//#define SOFT_RESET_ON_KILL // Use a digital button to soft-reset the controller after KILL + +// Report uncleaned reset reason from register r2 instead of MCUSR. Supported by Optiboot on AVR. +//#define OPTIBOOT_RESET_REASON diff --git a/Marlin/Makefile b/Marlin/Makefile index 19c2d0b329a7..eee1403b537c 100644 --- a/Marlin/Makefile +++ b/Marlin/Makefile @@ -110,7 +110,7 @@ LIQUID_TWI2 ?= 0 WIRE ?= 0 # This defines if Tone is needed (i.e SPEAKER is defined in Configuration.h) -# Disabling this (and SPEAKER) saves approximatively 350 bytes of memory. +# Disabling this (and SPEAKER) saves approximately 350 bytes of memory. TONE ?= 1 # This defines if U8GLIB is needed (may require RELOC_WORKAROUND) @@ -191,6 +191,134 @@ else ifeq ($(HARDWARE_MOTHERBOARD),1034) # RAMPS Derivatives - ATmega1280, ATmega2560 # +# 3Drag Controller +else ifeq ($(HARDWARE_MOTHERBOARD),1100) +# Velleman K8200 Controller (derived from 3Drag Controller) +else ifeq ($(HARDWARE_MOTHERBOARD),1101) +# Velleman K8400 Controller (derived from 3Drag Controller) +else ifeq ($(HARDWARE_MOTHERBOARD),1102) +# Velleman K8600 Controller (Vertex Nano) +else ifeq ($(HARDWARE_MOTHERBOARD),1103) +# Velleman K8800 Controller (Vertex Delta) +else ifeq ($(HARDWARE_MOTHERBOARD),1104) +# 2PrintBeta BAM&DICE with STK drivers +else ifeq ($(HARDWARE_MOTHERBOARD),1105) +# 2PrintBeta BAM&DICE Due with STK drivers +else ifeq ($(HARDWARE_MOTHERBOARD),1106) +# MKS BASE v1.0 +else ifeq ($(HARDWARE_MOTHERBOARD),1107) +# MKS BASE v1.4 with Allegro A4982 stepper drivers +else ifeq ($(HARDWARE_MOTHERBOARD),1108) +# MKS BASE v1.5 with Allegro A4982 stepper drivers +else ifeq ($(HARDWARE_MOTHERBOARD),1109) +# MKS BASE v1.6 with Allegro A4982 stepper drivers +else ifeq ($(HARDWARE_MOTHERBOARD),1110) +# MKS BASE 1.0 with Heroic HR4982 stepper drivers +else ifeq ($(HARDWARE_MOTHERBOARD),1111) +# MKS GEN v1.3 or 1.4 +else ifeq ($(HARDWARE_MOTHERBOARD),1112) +# MKS GEN L +else ifeq ($(HARDWARE_MOTHERBOARD),1113) +# BigTreeTech or BIQU KFB2.0 +else ifeq ($(HARDWARE_MOTHERBOARD),1114) +# zrib V2.0 (Chinese RAMPS replica) +else ifeq ($(HARDWARE_MOTHERBOARD),1115) +# zrib V5.2 (Chinese RAMPS replica) +else ifeq ($(HARDWARE_MOTHERBOARD),1116) +# Felix 2.0+ Electronics Board (RAMPS like) +else ifeq ($(HARDWARE_MOTHERBOARD),1117) +# Invent-A-Part RigidBoard +else ifeq ($(HARDWARE_MOTHERBOARD),1118) +# Invent-A-Part RigidBoard V2 +else ifeq ($(HARDWARE_MOTHERBOARD),1119) +# Sainsmart 2-in-1 board +else ifeq ($(HARDWARE_MOTHERBOARD),1120) +# Ultimaker +else ifeq ($(HARDWARE_MOTHERBOARD),1121) +# Ultimaker (Older electronics. Pre 1.5.4. This is rare) +else ifeq ($(HARDWARE_MOTHERBOARD),1122) + MCU ?= atmega1280 + PROG_MCU ?= m1280 +# Azteeg X3 +else ifeq ($(HARDWARE_MOTHERBOARD),1123) +# Azteeg X3 Pro +else ifeq ($(HARDWARE_MOTHERBOARD),1124) +# Ultimainboard 2.x (Uses TEMP_SENSOR 20) +else ifeq ($(HARDWARE_MOTHERBOARD),1125) +# Rumba +else ifeq ($(HARDWARE_MOTHERBOARD),1126) +# Raise3D N series Rumba derivative +else ifeq ($(HARDWARE_MOTHERBOARD),1127) +# Rapide Lite 200 (v1, low-cost RUMBA clone with drv) +else ifeq ($(HARDWARE_MOTHERBOARD),1128) +# Formbot T-Rex 2 Plus +else ifeq ($(HARDWARE_MOTHERBOARD),1129) +# Formbot T-Rex 3 +else ifeq ($(HARDWARE_MOTHERBOARD),1130) +# Formbot Raptor +else ifeq ($(HARDWARE_MOTHERBOARD),1131) +# Formbot Raptor 2 +else ifeq ($(HARDWARE_MOTHERBOARD),1132) +# bq ZUM Mega 3D +else ifeq ($(HARDWARE_MOTHERBOARD),1133) +# MakeBoard Mini v2.1.2 by MicroMake +else ifeq ($(HARDWARE_MOTHERBOARD),1134) +# TriGorilla Anycubic version 1.3-based on RAMPS EFB +else ifeq ($(HARDWARE_MOTHERBOARD),1135) +# ... Ver 1.4 +else ifeq ($(HARDWARE_MOTHERBOARD),1136) +# ... Rev 1.1 (new servo pin order) +else ifeq ($(HARDWARE_MOTHERBOARD),1137) +# Creality: Ender-4, CR-8 +else ifeq ($(HARDWARE_MOTHERBOARD),1138) +# Creality: CR10S, CR20, CR-X +else ifeq ($(HARDWARE_MOTHERBOARD),1139) +# Dagoma F5 +else ifeq ($(HARDWARE_MOTHERBOARD),1140) +# FYSETC F6 1.3 +else ifeq ($(HARDWARE_MOTHERBOARD),1141) +# FYSETC F6 1.4 +else ifeq ($(HARDWARE_MOTHERBOARD),1142) +# Wanhao Duplicator i3 Plus +else ifeq ($(HARDWARE_MOTHERBOARD),1143) +# VORON Design +else ifeq ($(HARDWARE_MOTHERBOARD),1144) +# Tronxy TRONXY-V3-1.0 +else ifeq ($(HARDWARE_MOTHERBOARD),1145) +# Z-Bolt X Series +else ifeq ($(HARDWARE_MOTHERBOARD),1146) +# TT OSCAR +else ifeq ($(HARDWARE_MOTHERBOARD),1147) +# Overlord/Overlord Pro +else ifeq ($(HARDWARE_MOTHERBOARD),1148) +# ADIMLab Gantry v1 +else ifeq ($(HARDWARE_MOTHERBOARD),1149) +# ADIMLab Gantry v2 +else ifeq ($(HARDWARE_MOTHERBOARD),1150) +# BIQU Tango V1 +else ifeq ($(HARDWARE_MOTHERBOARD),1151) +# MKS GEN L V2 +else ifeq ($(HARDWARE_MOTHERBOARD),1152) +# MKS GEN L V2.1 +else ifeq ($(HARDWARE_MOTHERBOARD),1153) +# Copymaster 3D +else ifeq ($(HARDWARE_MOTHERBOARD),1154) +# Ortur 4 +else ifeq ($(HARDWARE_MOTHERBOARD),1155) +# Tenlog D3 Hero IDEX printer +else ifeq ($(HARDWARE_MOTHERBOARD),1156) +# Ramps S 1.2 by Sakul.cz (Power outputs: Hotend0, Hotend1, Fan, Bed) +else ifeq ($(HARDWARE_MOTHERBOARD),1157) +# Ramps S 1.2 by Sakul.cz (Power outputs: Hotend0, Hotend1, Hotend2, Bed) +else ifeq ($(HARDWARE_MOTHERBOARD),1158) +# Ramps S 1.2 by Sakul.cz (Power outputs: Hotend, Fan0, Fan1, Bed) +else ifeq ($(HARDWARE_MOTHERBOARD),1159) +# Longer LK1 PRO / Alfawise U20 Pro (PRO version) +else ifeq ($(HARDWARE_MOTHERBOARD),1160) +# Longer LKx PRO / Alfawise Uxx Pro (PRO version) +else ifeq ($(HARDWARE_MOTHERBOARD),1161) + + # 3Drag Controller else ifeq ($(HARDWARE_MOTHERBOARD),1100) # Velleman K8200 Controller (derived from 3Drag Controller) @@ -219,7 +347,7 @@ else ifeq ($(HARDWARE_MOTHERBOARD),1111) else ifeq ($(HARDWARE_MOTHERBOARD),1112) # MKS GEN L else ifeq ($(HARDWARE_MOTHERBOARD),1113) -# zrib V2.0 control board (Chinese knock off RAMPS replica) +# zrib V2.0 control board (Chinese RAMPS replica) else ifeq ($(HARDWARE_MOTHERBOARD),1114) # BigTreeTech or BIQU KFB2.0 else ifeq ($(HARDWARE_MOTHERBOARD),1115) @@ -358,20 +486,38 @@ else ifeq ($(HARDWARE_MOTHERBOARD),1311) else ifeq ($(HARDWARE_MOTHERBOARD),1312) # Mega controller else ifeq ($(HARDWARE_MOTHERBOARD),1313) -# Geeetech GT2560 Rev B for Mecreator2 +# Geeetech GT2560 Rev A else ifeq ($(HARDWARE_MOTHERBOARD),1314) -# Geeetech GT2560 Rev. A +# Geeetech GT2560 Rev A+ (with auto level probe) else ifeq ($(HARDWARE_MOTHERBOARD),1315) -# Geeetech GT2560 Rev. A+ (with auto level probe) +# Geeetech GT2560 Rev B else ifeq ($(HARDWARE_MOTHERBOARD),1316) -# Geeetech GT2560 Rev B for A10(M/D) +# Geeetech GT2560 Rev B for A10(M/T/D) else ifeq ($(HARDWARE_MOTHERBOARD),1317) -# Geeetech GT2560 Rev B for A20(M/D) +# Geeetech GT2560 Rev B for A10(M/T/D) else ifeq ($(HARDWARE_MOTHERBOARD),1318) -# Einstart retrofit +# Geeetech GT2560 Rev B for Mecreator2 else ifeq ($(HARDWARE_MOTHERBOARD),1319) -# Wanhao 0ne+ i3 Mini +# Geeetech GT2560 Rev B for A20(M/T/D) else ifeq ($(HARDWARE_MOTHERBOARD),1320) +# Einstart retrofit +else ifeq ($(HARDWARE_MOTHERBOARD),1321) +# Wanhao 0ne+ i3 Mini +else ifeq ($(HARDWARE_MOTHERBOARD),1322) +# Leapfrog Xeed 2015 +else ifeq ($(HARDWARE_MOTHERBOARD),1323) +# PICA Shield (original version) +else ifeq ($(HARDWARE_MOTHERBOARD),1324) +# PICA Shield (rev C or later) +else ifeq ($(HARDWARE_MOTHERBOARD),1325) +# Intamsys 4.0 (Funmat HT) +else ifeq ($(HARDWARE_MOTHERBOARD),1326) +# Malyan M180 Mainboard Version 2 (no display function, direct gcode only) +else ifeq ($(HARDWARE_MOTHERBOARD),1327) +# Geeetech GT2560 Rev B for A20(M/T/D) +else ifeq ($(HARDWARE_MOTHERBOARD),1328) +# Mega controller & Protoneer CNC Shield V3.00 +else ifeq ($(HARDWARE_MOTHERBOARD),1329) # # ATmega1281, ATmega2561 @@ -445,6 +591,11 @@ else ifeq ($(HARDWARE_MOTHERBOARD),1510) HARDWARE_VARIANT ?= Sanguino MCU ?= atmega1284p PROG_MCU ?= m1284p +# ZoneStar ZMIB V2 +else ifeq ($(HARDWARE_MOTHERBOARD),1511) + HARDWARE_VARIANT ?= Sanguino + MCU ?= atmega1284p + PROG_MCU ?= m1284p # # Other ATmega644P, ATmega644, ATmega1284P @@ -993,5 +1144,5 @@ clean: .PHONY: all build elf hex eep lss sym program coff extcoff clean depend sizebefore sizeafter -# Automaticaly include the dependency files created by gcc +# Automatically include the dependency files created by gcc -include ${patsubst %.o, %.d, ${OBJ}} diff --git a/Marlin/Version.h b/Marlin/Version.h index eb2f9f9a14d4..93f951117c33 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 "2019-07-10" +//#define STRING_DISTRIBUTION_DATE "2022-02-19" /** * Defines a generic printer name to be output to the LCD after booting Marlin. diff --git a/Marlin/src/HAL/AVR/HAL.cpp b/Marlin/src/HAL/AVR/HAL.cpp index 4c45a5d78e65..7c39c5200b4d 100644 --- a/Marlin/src/HAL/AVR/HAL.cpp +++ b/Marlin/src/HAL/AVR/HAL.cpp @@ -25,7 +25,7 @@ #include "HAL.h" #ifdef USBCON - DefaultSerial MSerial(false, Serial); + DefaultSerial1 MSerial0(false, Serial); #ifdef BLUETOOTH BTSerial btSerial(false, bluetoothSerial); #endif @@ -35,13 +35,32 @@ // Public Variables // ------------------------ -//uint8_t MCUSR; +// Don't initialize/override variable (which would happen in .init4) +uint8_t MarlinHAL::reset_reason __attribute__((section(".noinit"))); // ------------------------ // Public functions // ------------------------ -void HAL_init() { +__attribute__((naked)) // Don't output function pro- and epilogue +__attribute__((used)) // Output the function, even if "not used" +__attribute__((section(".init3"))) // Put in an early user definable section +void save_reset_reason() { + #if ENABLED(OPTIBOOT_RESET_REASON) + __asm__ __volatile__( + A("STS %0, r2") + : "=m"(hal.reset_reason) + ); + #else + hal.reset_reason = MCUSR; + #endif + + // Clear within 16ms since WDRF bit enables a 16ms watchdog timer -> Boot loop + hal.clear_reset_source(); + wdt_disable(); +} + +void MarlinHAL::init() { // Init Servo Pins #define INIT_SERVO(N) OUT_WRITE(SERVO##N##_PIN, LOW) #if HAS_SERVO_0 @@ -56,6 +75,17 @@ void HAL_init() { #if HAS_SERVO_3 INIT_SERVO(3); #endif + + init_pwm_timers(); // Init user timers to default frequency - 1000HZ +} + +void MarlinHAL::reboot() { + #if ENABLED(USE_WATCHDOG) + while (1) { /* run out the watchdog */ } + #else + void (*resetFunc)() = 0; // Declare resetFunc() at address 0 + resetFunc(); // Jump to address 0 + #endif } #if ENABLED(SDSUPPORT) @@ -65,20 +95,20 @@ void HAL_init() { #else // !SDSUPPORT -extern "C" { - extern char __bss_end; - extern char __heap_start; - extern void* __brkval; - - int freeMemory() { - int free_memory; - if ((int)__brkval == 0) - free_memory = ((int)&free_memory) - ((int)&__bss_end); - else - free_memory = ((int)&free_memory) - ((int)__brkval); - return free_memory; + extern "C" { + extern char __bss_end; + extern char __heap_start; + extern void* __brkval; + + int freeMemory() { + int free_memory; + if ((int)__brkval == 0) + free_memory = ((int)&free_memory) - ((int)&__bss_end); + else + free_memory = ((int)&free_memory) - ((int)__brkval); + return free_memory; + } } -} #endif // !SDSUPPORT diff --git a/Marlin/src/HAL/AVR/HAL.h b/Marlin/src/HAL/AVR/HAL.h index 5e22ac0836e2..e825b4def3af 100644 --- a/Marlin/src/HAL/AVR/HAL.h +++ b/Marlin/src/HAL/AVR/HAL.h @@ -39,6 +39,19 @@ #include #include +// +// Default graphical display delays +// +#if F_CPU >= 20000000 + #define CPU_ST7920_DELAY_1 150 + #define CPU_ST7920_DELAY_2 0 + #define CPU_ST7920_DELAY_3 150 +#elif F_CPU == 16000000 + #define CPU_ST7920_DELAY_1 125 + #define CPU_ST7920_DELAY_2 0 + #define CPU_ST7920_DELAY_3 188 +#endif + #ifndef pgm_read_ptr // Compatibility for avr-libc 1.8.0-4.1 included with Ubuntu for // Windows Subsystem for Linux on Windows 10 as of 10/18/2019 @@ -61,9 +74,9 @@ #define CRITICAL_SECTION_START() unsigned char _sreg = SREG; cli() #define CRITICAL_SECTION_END() SREG = _sreg #endif -#define ISRS_ENABLED() TEST(SREG, SREG_I) -#define ENABLE_ISRS() sei() -#define DISABLE_ISRS() cli() + +#define HAL_CAN_SET_PWM_FREQ // This HAL supports PWM Frequency adjustment +#define PWM_FREQUENCY 1000 // Default PWM frequency when set_pwm_duty() is called without set_pwm_frequency() // ------------------------ // Types @@ -71,50 +84,56 @@ typedef int8_t pin_t; -#define SHARED_SERVOS HAS_SERVOS -#define HAL_SERVO_LIB Servo +#define SHARED_SERVOS HAS_SERVOS // Use shared/servos.cpp + +class Servo; +typedef Servo hal_servo_t; // ------------------------ -// Public Variables +// Serial ports // ------------------------ -//extern uint8_t MCUSR; - -// Serial ports #ifdef USBCON #include "../../core/serial_hook.h" - typedef ForwardSerial0Type< decltype(Serial) > DefaultSerial; - extern DefaultSerial MSerial; + typedef ForwardSerial1Class< decltype(Serial) > DefaultSerial1; + extern DefaultSerial1 MSerial0; #ifdef BLUETOOTH - typedef ForwardSerial0Type< decltype(bluetoothSerial) > BTSerial; + typedef ForwardSerial1Class< decltype(bluetoothSerial) > BTSerial; extern BTSerial btSerial; #endif - #define MYSERIAL0 TERN(BLUETOOTH, btSerial, MSerial) + #define MYSERIAL1 TERN(BLUETOOTH, btSerial, MSerial0) #else #if !WITHIN(SERIAL_PORT, -1, 3) - #error "SERIAL_PORT must be from -1 to 3. Please update your configuration." + #error "SERIAL_PORT must be from 0 to 3, or -1 for USB Serial." #endif - #define MYSERIAL0 customizedSerial1 + #define MYSERIAL1 customizedSerial1 #ifdef SERIAL_PORT_2 #if !WITHIN(SERIAL_PORT_2, -1, 3) - #error "SERIAL_PORT_2 must be from -1 to 3. Please update your configuration." + #error "SERIAL_PORT_2 must be from 0 to 3, or -1 for USB Serial." + #endif + #define MYSERIAL2 customizedSerial2 + #endif + + #ifdef SERIAL_PORT_3 + #if !WITHIN(SERIAL_PORT_3, -1, 3) + #error "SERIAL_PORT_3 must be from 0 to 3, or -1 for USB Serial." #endif - #define MYSERIAL1 customizedSerial2 + #define MYSERIAL3 customizedSerial3 #endif #endif #ifdef MMU2_SERIAL_PORT #if !WITHIN(MMU2_SERIAL_PORT, -1, 3) - #error "MMU2_SERIAL_PORT must be from -1 to 3. Please update your configuration." + #error "MMU2_SERIAL_PORT must be from 0 to 3, or -1 for USB Serial." #endif #define MMU2_SERIAL mmuSerial #endif #ifdef LCD_SERIAL_PORT #if !WITHIN(LCD_SERIAL_PORT, -1, 3) - #error "LCD_SERIAL_PORT must be from -1 to 3. Please update your configuration." + #error "LCD_SERIAL_PORT must be from 0 to 3, or -1 for USB Serial." #endif #define LCD_SERIAL lcdSerial #if HAS_DGUS_LCD @@ -122,89 +141,131 @@ typedef int8_t pin_t; #endif #endif -// ------------------------ -// Public functions -// ------------------------ +// +// ADC +// +#define HAL_ADC_VREF 5.0 +#define HAL_ADC_RESOLUTION 10 -void HAL_init(); +// +// Pin Mapping for M42, M43, M226 +// +#define GET_PIN_MAP_PIN(index) index +#define GET_PIN_MAP_INDEX(pin) pin +#define PARSED_PIN_INDEX(code, dval) parser.intval(code, dval) -//void cli(); +#define HAL_SENSITIVE_PINS 0, 1, -//void _delay_ms(const int delay); +#ifdef __AVR_AT90USB1286__ + #define JTAG_DISABLE() do{ MCUCR = 0x80; MCUCR = 0x80; }while(0) +#endif -inline void HAL_clear_reset_source() { MCUSR = 0; } -inline uint8_t HAL_get_reset_source() { return MCUSR; } +// AVR compatibility +#define strtof strtod -inline void HAL_reboot() {} // reboot the board or restart the bootloader +// ------------------------ +// Class Utilities +// ------------------------ +#pragma GCC diagnostic push #if GCC_VERSION <= 50000 - #pragma GCC diagnostic push #pragma GCC diagnostic ignored "-Wunused-function" #endif extern "C" int freeMemory(); -#if GCC_VERSION <= 50000 - #pragma GCC diagnostic pop -#endif +#pragma GCC diagnostic pop -// ADC -#ifdef DIDR2 - #define HAL_ANALOG_SELECT(ind) do{ if (ind < 8) SBI(DIDR0, ind); else SBI(DIDR2, ind & 0x07); }while(0) -#else - #define HAL_ANALOG_SELECT(ind) SBI(DIDR0, ind); -#endif +// ------------------------ +// MarlinHAL Class +// ------------------------ -inline void HAL_adc_init() { - ADCSRA = _BV(ADEN) | _BV(ADSC) | _BV(ADIF) | 0x07; - DIDR0 = 0; - #ifdef DIDR2 - DIDR2 = 0; - #endif -} +class MarlinHAL { +public: -#define SET_ADMUX_ADCSRA(ch) ADMUX = _BV(REFS0) | (ch & 0x07); SBI(ADCSRA, ADSC) -#ifdef MUX5 - #define HAL_START_ADC(ch) if (ch > 7) ADCSRB = _BV(MUX5); else ADCSRB = 0; SET_ADMUX_ADCSRA(ch) -#else - #define HAL_START_ADC(ch) ADCSRB = 0; SET_ADMUX_ADCSRA(ch) -#endif + // Earliest possible init, before setup() + MarlinHAL() {} -#define HAL_ADC_VREF 5.0 -#define HAL_ADC_RESOLUTION 10 -#define HAL_READ_ADC() ADC -#define HAL_ADC_READY() !TEST(ADCSRA, ADSC) + static void init(); // Called early in setup() + static void init_board() {} // Called less early in setup() + static void reboot(); // Restart the firmware from 0x0 -#define GET_PIN_MAP_PIN(index) index -#define GET_PIN_MAP_INDEX(pin) pin -#define PARSED_PIN_INDEX(code, dval) parser.intval(code, dval) + // Interrupts + static bool isr_state() { return TEST(SREG, SREG_I); } + static void isr_on() { sei(); } + static void isr_off() { cli(); } -#define HAL_SENSITIVE_PINS 0, 1 + static void delay_ms(const int ms) { _delay_ms(ms); } -#ifdef __AVR_AT90USB1286__ - #define JTAG_DISABLE() do{ MCUCR = 0x80; MCUCR = 0x80; }while(0) -#endif + // Tasks, called from idle() + static void idletask() {} -// AVR compatibility -#define strtof strtod + // Reset + static uint8_t reset_reason; + static uint8_t get_reset_source() { return reset_reason; } + static void clear_reset_source() { MCUSR = 0; } -#define HAL_CAN_SET_PWM_FREQ // This HAL supports PWM Frequency adjustment + // Free SRAM + static int freeMemory() { return ::freeMemory(); } -/** - * set_pwm_frequency - * Sets the frequency of the timer corresponding to the provided pin - * as close as possible to the provided desired frequency. Internally - * calculates the required waveform generation mode, prescaler and - * resolution values required and sets the timer registers accordingly. - * NOTE that the frequency is applied to all pins on the timer (Ex OC3A, OC3B and OC3B) - * NOTE that there are limitations, particularly if using TIMER2. (see Configuration_adv.h -> FAST FAN PWM Settings) - */ -void set_pwm_frequency(const pin_t pin, int f_desired); + // + // ADC Methods + // -/** - * set_pwm_duty - * Sets the PWM duty cycle of the provided pin to the provided value - * Optionally allows inverting the duty cycle [default = false] - * Optionally allows changing the maximum size of the provided value to enable finer PWM duty control [default = 255] - */ -void set_pwm_duty(const pin_t pin, const uint16_t v, const uint16_t v_size=255, const bool invert=false); + // Called by Temperature::init once at startup + static void adc_init() { + ADCSRA = _BV(ADEN) | _BV(ADSC) | _BV(ADIF) | 0x07; + DIDR0 = 0; + #ifdef DIDR2 + DIDR2 = 0; + #endif + } + + // Called by Temperature::init for each sensor at startup + static void adc_enable(const uint8_t ch) { + #ifdef DIDR2 + if (ch > 7) { SBI(DIDR2, ch & 0x07); return; } + #endif + SBI(DIDR0, ch); + } + + // Begin ADC sampling on the given channel + static void adc_start(const uint8_t ch) { + #ifdef MUX5 + ADCSRB = ch > 7 ? _BV(MUX5) : 0; + #else + ADCSRB = 0; + #endif + ADMUX = _BV(REFS0) | (ch & 0x07); + SBI(ADCSRA, ADSC); + } + + // Is the ADC ready for reading? + static bool adc_ready() { return !TEST(ADCSRA, ADSC); } + + // The current value of the ADC register + static __typeof__(ADC) adc_value() { return ADC; } + + /** + * init_pwm_timers + * Set the default frequency for timers 2-5 to 1000HZ + */ + static void init_pwm_timers(); + + /** + * Set the PWM duty cycle for the pin to the given value. + * Optionally invert the duty cycle [default = false] + * Optionally change the scale of the provided value to enable finer PWM duty control [default = 255] + */ + static void set_pwm_duty(const pin_t pin, const uint16_t v, const uint16_t v_size=255, const bool invert=false); + + /** + * Set the frequency of the timer for the given pin as close as + * possible to the provided desired frequency. Internally calculate + * the required waveform generation mode, prescaler, and resolution + * values and set timer registers accordingly. + * NOTE that the frequency is applied to all pins on the timer (Ex OC3A, OC3B and OC3B) + * NOTE that there are limitations, particularly if using TIMER2. (see Configuration_adv.h -> FAST_PWM_FAN Settings) + */ + static void set_pwm_frequency(const pin_t pin, const uint16_t f_desired); +}; diff --git a/Marlin/src/HAL/AVR/HAL_SPI.cpp b/Marlin/src/HAL/AVR/HAL_SPI.cpp index 3e5572e559fa..dc98f2f79e71 100644 --- a/Marlin/src/HAL/AVR/HAL_SPI.cpp +++ b/Marlin/src/HAL/AVR/HAL_SPI.cpp @@ -34,21 +34,21 @@ #include "../../inc/MarlinConfig.h" void spiBegin() { - OUT_WRITE(SD_SS_PIN, HIGH); + #if PIN_EXISTS(SD_SS) + // Do not init HIGH for boards with pin 4 used as Fans or Heaters or otherwise, not likely to have multiple SPI devices anyway. + #if defined(__AVR_ATmega644__) || defined(__AVR_ATmega644P__) || defined(__AVR_ATmega644PA__) || defined(__AVR_ATmega1284P__) + // SS must be in output mode even it is not chip select + SET_OUTPUT(SD_SS_PIN); + #else + // set SS high - may be chip select for another SPI device + OUT_WRITE(SD_SS_PIN, HIGH); + #endif + #endif SET_OUTPUT(SD_SCK_PIN); SET_INPUT(SD_MISO_PIN); SET_OUTPUT(SD_MOSI_PIN); - #if DISABLED(SOFTWARE_SPI) - // SS must be in output mode even it is not chip select - //SET_OUTPUT(SD_SS_PIN); - // set SS high - may be chip select for another SPI device - //#if SET_SPI_SS_HIGH - //WRITE(SD_SS_PIN, HIGH); - //#endif - // set a default rate - spiInit(1); - #endif + IF_DISABLED(SOFTWARE_SPI, spiInit(SPI_HALF_SPEED)); } #if NONE(SOFTWARE_SPI, FORCE_SOFT_SPI) @@ -74,7 +74,8 @@ void spiBegin() { #elif defined(PRR0) PRR0 #endif - , PRSPI); + , PRSPI + ); SPCR = _BV(SPE) | _BV(MSTR) | (spiRate >> 1); SPSR = spiRate & 1 || spiRate == 6 ? 0 : _BV(SPI2X); @@ -88,7 +89,7 @@ void spiBegin() { } /** SPI read data */ - void spiRead(uint8_t* buf, uint16_t nbyte) { + void spiRead(uint8_t *buf, uint16_t nbyte) { if (nbyte-- == 0) return; SPDR = 0xFF; for (uint16_t i = 0; i < nbyte; i++) { @@ -107,7 +108,7 @@ void spiBegin() { } /** SPI send block */ - void spiSendBlock(uint8_t token, const uint8_t* buf) { + void spiSendBlock(uint8_t token, const uint8_t *buf) { SPDR = token; for (uint16_t i = 0; i < 512; i += 2) { while (!TEST(SPSR, SPIF)) { /* Intentionally left empty */ } @@ -215,7 +216,7 @@ void spiBegin() { } // Soft SPI read data - void spiRead(uint8_t* buf, uint16_t nbyte) { + void spiRead(uint8_t *buf, uint16_t nbyte) { for (uint16_t i = 0; i < nbyte; i++) buf[i] = spiRec(); } @@ -242,7 +243,7 @@ void spiBegin() { } // Soft SPI send block - void spiSendBlock(uint8_t token, const uint8_t* buf) { + void spiSendBlock(uint8_t token, const uint8_t *buf) { spiSend(token); for (uint16_t i = 0; i < 512; i++) spiSend(buf[i]); diff --git a/Marlin/src/HAL/AVR/MarlinSPI.h b/Marlin/src/HAL/AVR/MarlinSPI.h new file mode 100644 index 000000000000..0c447ba4cb3d --- /dev/null +++ b/Marlin/src/HAL/AVR/MarlinSPI.h @@ -0,0 +1,26 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2021 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + * + */ +#pragma once + +#include + +using MarlinSPI = SPIClass; diff --git a/Marlin/src/HAL/AVR/MarlinSerial.cpp b/Marlin/src/HAL/AVR/MarlinSerial.cpp index 81503e1fe936..986462437c8f 100644 --- a/Marlin/src/HAL/AVR/MarlinSerial.cpp +++ b/Marlin/src/HAL/AVR/MarlinSerial.cpp @@ -454,7 +454,7 @@ void MarlinSerial::flush() { } template -size_t MarlinSerial::write(const uint8_t c) { +void MarlinSerial::write(const uint8_t c) { if (Cfg::TX_SIZE == 0) { _written = true; @@ -480,13 +480,13 @@ size_t MarlinSerial::write(const uint8_t c) { // location". This makes sure flush() won't return until the bytes // actually got written B_TXC = 1; - return 1; + return; } const uint8_t i = (tx_buffer.head + 1) & (Cfg::TX_SIZE - 1); // If global interrupts are disabled (as the result of being called from an ISR)... - if (!ISRS_ENABLED()) { + if (!hal.isr_state()) { // Make room by polling if it is possible to transmit, and do so! while (i == tx_buffer.tail) { @@ -510,7 +510,6 @@ size_t MarlinSerial::write(const uint8_t c) { // Enable TX ISR - Non atomic, but it will eventually enable TX ISR B_UDRIE = 1; } - return 1; } template @@ -535,7 +534,7 @@ void MarlinSerial::flushTX() { if (!_written) return; // If global interrupts are disabled (as the result of being called from an ISR)... - if (!ISRS_ENABLED()) { + if (!hal.isr_state()) { // Wait until everything was transmitted - We must do polling, as interrupts are disabled while (tx_buffer.head != tx_buffer.tail || !B_TXC) { @@ -568,7 +567,7 @@ ISR(SERIAL_REGNAME(USART, SERIAL_PORT, _UDRE_vect)) { // Because of the template definition above, it's required to instantiate the template to have all methods generated template class MarlinSerial< MarlinSerialCfg >; -MSerialT customizedSerial1(MSerialT::HasEmergencyParser); +MSerialT1 customizedSerial1(MSerialT1::HasEmergencyParser); #ifdef SERIAL_PORT_2 @@ -583,7 +582,24 @@ MSerialT customizedSerial1(MSerialT::HasEmergencyParser); template class MarlinSerial< MarlinSerialCfg >; MSerialT2 customizedSerial2(MSerialT2::HasEmergencyParser); -#endif + +#endif // SERIAL_PORT_2 + +#ifdef SERIAL_PORT_3 + + // Hookup ISR handlers + ISR(SERIAL_REGNAME(USART, SERIAL_PORT_3, _RX_vect)) { + MarlinSerial>::store_rxd_char(); + } + + ISR(SERIAL_REGNAME(USART, SERIAL_PORT_3, _UDRE_vect)) { + MarlinSerial>::_tx_udr_empty_irq(); + } + + template class MarlinSerial< MarlinSerialCfg >; + MSerialT3 customizedSerial3(MSerialT3::HasEmergencyParser); + +#endif // SERIAL_PORT_3 #ifdef MMU2_SERIAL_PORT @@ -596,8 +612,9 @@ MSerialT customizedSerial1(MSerialT::HasEmergencyParser); } template class MarlinSerial< MMU2SerialCfg >; - MSerialT3 mmuSerial(MSerialT3::HasEmergencyParser); -#endif + MSerialMMU2 mmuSerial(MSerialMMU2::HasEmergencyParser); + +#endif // MMU2_SERIAL_PORT #ifdef LCD_SERIAL_PORT @@ -610,7 +627,7 @@ MSerialT customizedSerial1(MSerialT::HasEmergencyParser); } template class MarlinSerial< LCDSerialCfg >; - MSerialT4 lcdSerial(MSerialT4::HasEmergencyParser); + MSerialLCD lcdSerial(MSerialLCD::HasEmergencyParser); #if HAS_DGUS_LCD template @@ -623,13 +640,13 @@ MSerialT customizedSerial1(MSerialT::HasEmergencyParser); } #endif -#endif +#endif // LCD_SERIAL_PORT #endif // !USBCON && (UBRRH || UBRR0H || UBRR1H || UBRR2H || UBRR3H) // For AT90USB targets use the UART for BT interfacing #if defined(USBCON) && ENABLED(BLUETOOTH) - MSerialT5 bluetoothSerial(false); + MSerialBT bluetoothSerial(false); #endif #endif // __AVR__ diff --git a/Marlin/src/HAL/AVR/MarlinSerial.h b/Marlin/src/HAL/AVR/MarlinSerial.h index 9abc3dbed0c0..7eb76000d66e 100644 --- a/Marlin/src/HAL/AVR/MarlinSerial.h +++ b/Marlin/src/HAL/AVR/MarlinSerial.h @@ -191,13 +191,13 @@ rx_framing_errors; static ring_buffer_pos_t rx_max_enqueued; - static FORCE_INLINE ring_buffer_pos_t atomic_read_rx_head(); + FORCE_INLINE static ring_buffer_pos_t atomic_read_rx_head(); static volatile bool rx_tail_value_not_stable; static volatile uint16_t rx_tail_value_backup; - static FORCE_INLINE void atomic_set_rx_tail(ring_buffer_pos_t value); - static FORCE_INLINE ring_buffer_pos_t atomic_read_rx_tail(); + FORCE_INLINE static void atomic_set_rx_tail(ring_buffer_pos_t value); + FORCE_INLINE static ring_buffer_pos_t atomic_read_rx_tail(); public: FORCE_INLINE static void store_rxd_char(); @@ -210,14 +210,14 @@ static int read(); static void flush(); static ring_buffer_pos_t available(); - static size_t write(const uint8_t c); + static void write(const uint8_t c); static void flushTX(); #if HAS_DGUS_LCD static ring_buffer_pos_t get_tx_buffer_free(); #endif enum { HasEmergencyParser = Cfg::EMERGENCYPARSER }; - static inline bool emergency_parser_enabled() { return Cfg::EMERGENCYPARSER; } + static bool emergency_parser_enabled() { return Cfg::EMERGENCYPARSER; } FORCE_INLINE static uint8_t dropped() { return Cfg::DROPPED_RX ? rx_dropped_bytes : 0; } FORCE_INLINE static uint8_t buffer_overruns() { return Cfg::RX_OVERRUNS ? rx_buffer_overruns : 0; } @@ -238,66 +238,60 @@ static constexpr bool MAX_RX_QUEUED = ENABLED(SERIAL_STATS_MAX_RX_QUEUED); }; - typedef Serial0Type< MarlinSerial< MarlinSerialCfg > > MSerialT; - extern MSerialT customizedSerial1; + typedef Serial1Class< MarlinSerial< MarlinSerialCfg > > MSerialT1; + extern MSerialT1 customizedSerial1; #ifdef SERIAL_PORT_2 - typedef Serial0Type< MarlinSerial< MarlinSerialCfg > > MSerialT2; + typedef Serial1Class< MarlinSerial< MarlinSerialCfg > > MSerialT2; extern MSerialT2 customizedSerial2; #endif + #ifdef SERIAL_PORT_3 + typedef Serial1Class< MarlinSerial< MarlinSerialCfg > > MSerialT3; + extern MSerialT3 customizedSerial3; + #endif + #endif // !USBCON #ifdef MMU2_SERIAL_PORT template struct MMU2SerialCfg { static constexpr int PORT = serial; + static constexpr unsigned int RX_SIZE = 32; + static constexpr unsigned int TX_SIZE = 32; static constexpr bool XONOFF = false; static constexpr bool EMERGENCYPARSER = false; static constexpr bool DROPPED_RX = false; static constexpr bool RX_FRAMING_ERRORS = false; static constexpr bool MAX_RX_QUEUED = false; - static constexpr unsigned int RX_SIZE = 32; - static constexpr unsigned int TX_SIZE = 32; static constexpr bool RX_OVERRUNS = false; }; - typedef Serial0Type< MarlinSerial< MMU2SerialCfg > > MSerialT3; - extern MSerialT3 mmuSerial; + typedef Serial1Class< MarlinSerial< MMU2SerialCfg > > MSerialMMU2; + extern MSerialMMU2 mmuSerial; #endif #ifdef LCD_SERIAL_PORT template struct LCDSerialCfg { - static constexpr int PORT = serial; - static constexpr bool XONOFF = false; - static constexpr bool EMERGENCYPARSER = ENABLED(EMERGENCY_PARSER); - static constexpr bool DROPPED_RX = false; - static constexpr bool RX_FRAMING_ERRORS = false; - static constexpr bool MAX_RX_QUEUED = false; - #if HAS_DGUS_LCD - static constexpr unsigned int RX_SIZE = DGUS_RX_BUFFER_SIZE; - static constexpr unsigned int TX_SIZE = DGUS_TX_BUFFER_SIZE; - static constexpr bool RX_OVERRUNS = ENABLED(SERIAL_STATS_RX_BUFFER_OVERRUNS); - #elif EITHER(ANYCUBIC_LCD_I3MEGA, ANYCUBIC_LCD_CHIRON) - static constexpr unsigned int RX_SIZE = 64; - static constexpr unsigned int TX_SIZE = 128; - static constexpr bool RX_OVERRUNS = false; - #else - static constexpr unsigned int RX_SIZE = 64; - static constexpr unsigned int TX_SIZE = 128; - static constexpr bool RX_OVERRUNS = false - #endif + static constexpr int PORT = serial; + static constexpr unsigned int RX_SIZE = TERN(HAS_DGUS_LCD, DGUS_RX_BUFFER_SIZE, 64); + static constexpr unsigned int TX_SIZE = TERN(HAS_DGUS_LCD, DGUS_TX_BUFFER_SIZE, 128); + static constexpr bool XONOFF = false; + static constexpr bool EMERGENCYPARSER = ENABLED(EMERGENCY_PARSER); + static constexpr bool DROPPED_RX = false; + static constexpr bool RX_FRAMING_ERRORS = false; + static constexpr bool MAX_RX_QUEUED = false; + static constexpr bool RX_OVERRUNS = BOTH(HAS_DGUS_LCD, SERIAL_STATS_RX_BUFFER_OVERRUNS); }; - - typedef Serial0Type< MarlinSerial< LCDSerialCfg > > MSerialT4; - extern MSerialT4 lcdSerial; + typedef Serial1Class< MarlinSerial< LCDSerialCfg > > MSerialLCD; + extern MSerialLCD lcdSerial; #endif // Use the UART for Bluetooth in AT90USB configurations #if defined(USBCON) && ENABLED(BLUETOOTH) - typedef Serial0Type MSerialT5; - extern MSerialT5 bluetoothSerial; + typedef Serial1Class MSerialBT; + extern MSerialBT bluetoothSerial; #endif diff --git a/Marlin/src/HAL/AVR/eeprom.cpp b/Marlin/src/HAL/AVR/eeprom.cpp index ee2a73e410ad..8d084dec7fdf 100644 --- a/Marlin/src/HAL/AVR/eeprom.cpp +++ b/Marlin/src/HAL/AVR/eeprom.cpp @@ -40,13 +40,13 @@ bool PersistentStore::access_start() { return true; } bool PersistentStore::access_finish() { return true; } bool PersistentStore::write_data(int &pos, const uint8_t *value, size_t size, uint16_t *crc) { + uint16_t written = 0; while (size--) { uint8_t * const p = (uint8_t * const)pos; uint8_t v = *value; - // EEPROM has only ~100,000 write cycles, - // so only write bytes that have changed! - if (v != eeprom_read_byte(p)) { + if (v != eeprom_read_byte(p)) { // EEPROM has only ~100,000 write cycles, so only write bytes that have changed! eeprom_write_byte(p, v); + if (++written & 0x7F) delay(2); else safe_delay(2); // Avoid triggering watchdog during long EEPROM writes if (eeprom_read_byte(p) != v) { SERIAL_ECHO_MSG(STR_ERR_EEPROM_WRITE); return true; diff --git a/Marlin/src/HAL/AVR/endstop_interrupts.h b/Marlin/src/HAL/AVR/endstop_interrupts.h index 9fd9c38b86ad..0ce8574c53d9 100644 --- a/Marlin/src/HAL/AVR/endstop_interrupts.h +++ b/Marlin/src/HAL/AVR/endstop_interrupts.h @@ -168,6 +168,51 @@ void setup_endstop_interrupts() { pciSetup(Z_MIN_PIN); #endif #endif + #if HAS_I_MAX + #if (digitalPinToInterrupt(I_MAX_PIN) != NOT_AN_INTERRUPT) + _ATTACH(I_MAX_PIN); + #else + static_assert(digitalPinHasPCICR(I_MAX_PIN), "I_MAX_PIN is not interrupt-capable"); + pciSetup(I_MAX_PIN); + #endif + #elif HAS_I_MIN + #if (digitalPinToInterrupt(I_MIN_PIN) != NOT_AN_INTERRUPT) + _ATTACH(I_MIN_PIN); + #else + static_assert(digitalPinHasPCICR(I_MIN_PIN), "I_MIN_PIN is not interrupt-capable"); + pciSetup(I_MIN_PIN); + #endif + #endif + #if HAS_J_MAX + #if (digitalPinToInterrupt(J_MAX_PIN) != NOT_AN_INTERRUPT) + _ATTACH(J_MAX_PIN); + #else + static_assert(digitalPinHasPCICR(J_MAX_PIN), "J_MAX_PIN is not interrupt-capable"); + pciSetup(J_MAX_PIN); + #endif + #elif HAS_J_MIN + #if (digitalPinToInterrupt(J_MIN_PIN) != NOT_AN_INTERRUPT) + _ATTACH(J_MIN_PIN); + #else + static_assert(digitalPinHasPCICR(J_MIN_PIN), "J_MIN_PIN is not interrupt-capable"); + pciSetup(J_MIN_PIN); + #endif + #endif + #if HAS_K_MAX + #if (digitalPinToInterrupt(K_MAX_PIN) != NOT_AN_INTERRUPT) + _ATTACH(K_MAX_PIN); + #else + static_assert(digitalPinHasPCICR(K_MAX_PIN), "K_MAX_PIN is not interrupt-capable"); + pciSetup(K_MAX_PIN); + #endif + #elif HAS_K_MIN + #if (digitalPinToInterrupt(K_MIN_PIN) != NOT_AN_INTERRUPT) + _ATTACH(K_MIN_PIN); + #else + static_assert(digitalPinHasPCICR(K_MIN_PIN), "K_MIN_PIN is not interrupt-capable"); + pciSetup(K_MIN_PIN); + #endif + #endif #if HAS_X2_MAX #if (digitalPinToInterrupt(X2_MAX_PIN) != NOT_AN_INTERRUPT) _ATTACH(X2_MAX_PIN); diff --git a/Marlin/src/HAL/AVR/fast_pwm.cpp b/Marlin/src/HAL/AVR/fast_pwm.cpp index 238c1124adeb..0a384172c32a 100644 --- a/Marlin/src/HAL/AVR/fast_pwm.cpp +++ b/Marlin/src/HAL/AVR/fast_pwm.cpp @@ -21,11 +21,7 @@ */ #ifdef __AVR__ -#include "../../inc/MarlinConfigPre.h" - -#if NEEDS_HARDWARE_PWM // Specific meta-flag for features that mandate PWM - -#include "HAL.h" +#include "../../inc/MarlinConfig.h" struct Timer { volatile uint8_t* TCCRnQ[3]; // max 3 TCCR registers per timer @@ -33,250 +29,194 @@ struct Timer { volatile uint16_t* ICRn; // max 1 ICR register per timer uint8_t n; // the timer number [0->5] uint8_t q; // the timer output [0->2] (A->C) + bool isPWM; // True if pin is a "hardware timer" + bool isProtected; // True if timer is protected }; +// Macros for the Timer structure +#define _SET_WGMnQ(T, V) do{ \ + *(T.TCCRnQ)[0] = (*(T.TCCRnQ)[0] & ~(0x3 << 0)) | (( int(V) & 0x3) << 0); \ + *(T.TCCRnQ)[1] = (*(T.TCCRnQ)[1] & ~(0x3 << 3)) | (((int(V) >> 2) & 0x3) << 3); \ + }while(0) + +// Set TCCR CS bits +#define _SET_CSn(T, V) (*(T.TCCRnQ)[1] = (*(T.TCCRnQ[1]) & ~(0x7 << 0)) | ((int(V) & 0x7) << 0)) + +// Set TCCR COM bits +#define _SET_COMnQ(T, Q, V) (*(T.TCCRnQ)[0] = (*(T.TCCRnQ)[0] & ~(0x3 << (6-2*(Q)))) | (int(V) << (6-2*(Q)))) + +// Set OCRnQ register +#define _SET_OCRnQ(T, Q, V) (*(T.OCRnQ)[Q] = int(V) & 0xFFFF) + +// Set ICRn register (one per timer) +#define _SET_ICRn(T, V) (*(T.ICRn) = int(V) & 0xFFFF) + /** - * get_pwm_timer - * Get the timer information and register of the provided pin. - * Return a Timer struct containing this information. - * Used by set_pwm_frequency, set_pwm_duty + * Return a Timer struct describing a pin's timer. */ -Timer get_pwm_timer(const pin_t pin) { +const Timer get_pwm_timer(const pin_t pin) { + uint8_t q = 0; + switch (digitalPinToTimer(pin)) { - // Protect reserved timers (TIMER0 & TIMER1) #ifdef TCCR0A - #if !AVR_AT90USB1286_FAMILY - case TIMER0A: - #endif - case TIMER0B: + IF_DISABLED(AVR_AT90USB1286_FAMILY, case TIMER0A:) #endif #ifdef TCCR1A case TIMER1A: case TIMER1B: #endif - break; - #if defined(TCCR2) || defined(TCCR2A) - #ifdef TCCR2 - case TIMER2: { - Timer timer = { - /*TCCRnQ*/ { &TCCR2, nullptr, nullptr }, - /*OCRnQ*/ { (uint16_t*)&OCR2, nullptr, nullptr }, - /*ICRn*/ nullptr, - /*n, q*/ 2, 0 - }; - } - #elif defined(TCCR2A) - #if ENABLED(USE_OCR2A_AS_TOP) - case TIMER2A: break; // protect TIMER2A - case TIMER2B: { - Timer timer = { - /*TCCRnQ*/ { &TCCR2A, &TCCR2B, nullptr }, - /*OCRnQ*/ { (uint16_t*)&OCR2A, (uint16_t*)&OCR2B, nullptr }, - /*ICRn*/ nullptr, - /*n, q*/ 2, 1 - }; - return timer; - } - #else - case TIMER2B: ++q; - case TIMER2A: { - Timer timer = { - /*TCCRnQ*/ { &TCCR2A, &TCCR2B, nullptr }, - /*OCRnQ*/ { (uint16_t*)&OCR2A, (uint16_t*)&OCR2B, nullptr }, - /*ICRn*/ nullptr, - 2, q - }; - return timer; - } - #endif - #endif + + break; // Protect reserved timers (TIMER0 & TIMER1) + + #ifdef TCCR0A + case TIMER0B: // Protected timer, but allow setting the duty cycle on OCR0B for pin D4 only + return Timer({ { &TCCR0A, nullptr, nullptr }, { (uint16_t*)&OCR0A, (uint16_t*)&OCR0B, nullptr }, nullptr, 0, 1, true, true }); + #endif + + #if HAS_TCCR2 + case TIMER2: + return Timer({ { &TCCR2, nullptr, nullptr }, { (uint16_t*)&OCR2, nullptr, nullptr }, nullptr, 2, 0, true, false }); + #elif ENABLED(USE_OCR2A_AS_TOP) + case TIMER2A: break; // Protect TIMER2A since its OCR is used by TIMER2B + case TIMER2B: + return Timer({ { &TCCR2A, &TCCR2B, nullptr }, { (uint16_t*)&OCR2A, (uint16_t*)&OCR2B, nullptr }, nullptr, 2, 1, true, false }); + #elif defined(TCCR2A) + case TIMER2B: ++q; case TIMER2A: + return Timer({ { &TCCR2A, &TCCR2B, nullptr }, { (uint16_t*)&OCR2A, (uint16_t*)&OCR2B, nullptr }, nullptr, 2, q, true, false }); #endif + #ifdef OCR3C - case TIMER3C: ++q; - case TIMER3B: ++q; - case TIMER3A: { - Timer timer = { - /*TCCRnQ*/ { &TCCR3A, &TCCR3B, &TCCR3C }, - /*OCRnQ*/ { &OCR3A, &OCR3B, &OCR3C }, - /*ICRn*/ &ICR3, - /*n, q*/ 3, q - }; - return timer; - } + case TIMER3C: ++q; case TIMER3B: ++q; case TIMER3A: + return Timer({ { &TCCR3A, &TCCR3B, &TCCR3C }, { &OCR3A, &OCR3B, &OCR3C }, &ICR3, 3, q, true, false }); #elif defined(OCR3B) - case TIMER3B: ++q; - case TIMER3A: { - Timer timer = { - /*TCCRnQ*/ { &TCCR3A, &TCCR3B, nullptr }, - /*OCRnQ*/ { &OCR3A, &OCR3B, nullptr }, - /*ICRn*/ &ICR3, - /*n, q*/ 3, q - }; - return timer; - } + case TIMER3B: ++q; case TIMER3A: + return Timer({ { &TCCR3A, &TCCR3B, nullptr }, { &OCR3A, &OCR3B, nullptr }, &ICR3, 3, q, true, false }); #endif + #ifdef TCCR4A - case TIMER4C: ++q; - case TIMER4B: ++q; - case TIMER4A: { - Timer timer = { - /*TCCRnQ*/ { &TCCR4A, &TCCR4B, &TCCR4C }, - /*OCRnQ*/ { &OCR4A, &OCR4B, &OCR4C }, - /*ICRn*/ &ICR4, - /*n, q*/ 4, q - }; - return timer; - } + case TIMER4C: ++q; case TIMER4B: ++q; case TIMER4A: + return Timer({ { &TCCR4A, &TCCR4B, &TCCR4C }, { &OCR4A, &OCR4B, &OCR4C }, &ICR4, 4, q, true, false }); #endif + #ifdef TCCR5A - case TIMER5C: ++q; - case TIMER5B: ++q; - case TIMER5A: { - Timer timer = { - /*TCCRnQ*/ { &TCCR5A, &TCCR5B, &TCCR5C }, - /*OCRnQ*/ { &OCR5A, &OCR5B, &OCR5C }, - /*ICRn*/ &ICR5, - /*n, q*/ 5, q - }; - return timer; - } + case TIMER5C: ++q; case TIMER5B: ++q; case TIMER5A: + return Timer({ { &TCCR5A, &TCCR5B, &TCCR5C }, { &OCR5A, &OCR5B, &OCR5C }, &ICR5, 5, q, true, false }); #endif } - Timer timer = { - /*TCCRnQ*/ { nullptr, nullptr, nullptr }, - /*OCRnQ*/ { nullptr, nullptr, nullptr }, - /*ICRn*/ nullptr, - 0, 0 - }; - return timer; + + return Timer(); } -void set_pwm_frequency(const pin_t pin, int f_desired) { - Timer timer = get_pwm_timer(pin); - if (timer.n == 0) return; // Don't proceed if protected timer or not recognised - uint16_t size; - if (timer.n == 2) size = 255; else size = 65535; +void MarlinHAL::set_pwm_frequency(const pin_t pin, const uint16_t f_desired) { + const Timer timer = get_pwm_timer(pin); + if (timer.isProtected || !timer.isPWM) return; // Don't proceed if protected timer or not recognized + + const bool is_timer2 = timer.n == 2; + const uint16_t maxtop = is_timer2 ? 0xFF : 0xFFFF; - uint16_t res = 255; // resolution (TOP value) - uint8_t j = 0; // prescaler index - uint8_t wgm = 1; // waveform generation mode + uint16_t res = 0xFF; // resolution (TOP value) + uint8_t j = CS_NONE; // prescaler index + uint8_t wgm = WGM_PWM_PC_8; // waveform generation mode // Calculating the prescaler and resolution to use to achieve closest frequency if (f_desired != 0) { - int f = (F_CPU) / (2 * 1024 * size) + 1; // Initialize frequency as lowest (non-zero) achievable - uint16_t prescaler[] = { 0, 1, 8, /*TIMER2 ONLY*/32, 64, /*TIMER2 ONLY*/128, 256, 1024 }; - - // loop over prescaler values - LOOP_S_L_N(i, 1, 8) { - uint16_t res_temp_fast = 255, res_temp_phase_correct = 255; - if (timer.n == 2) { - // No resolution calculation for TIMER2 unless enabled USE_OCR2A_AS_TOP - #if ENABLED(USE_OCR2A_AS_TOP) - const uint16_t rtf = (F_CPU) / (prescaler[i] * f_desired); - res_temp_fast = rtf - 1; - res_temp_phase_correct = rtf / 2; + constexpr uint16_t prescaler[] = { 1, 8, (32), 64, (128), 256, 1024 }; // (*) are Timer 2 only + uint16_t f = (F_CPU) / (2 * 1024 * maxtop) + 1; // Start with the lowest non-zero frequency achievable (1 or 31) + + LOOP_L_N(i, COUNT(prescaler)) { // Loop through all prescaler values + const uint16_t p = prescaler[i]; + uint16_t res_fast_temp, res_pc_temp; + if (is_timer2) { + #if ENABLED(USE_OCR2A_AS_TOP) // No resolution calculation for TIMER2 unless enabled USE_OCR2A_AS_TOP + const uint16_t rft = (F_CPU) / (p * f_desired); + res_fast_temp = rft - 1; + res_pc_temp = rft / 2; + #else + res_fast_temp = res_pc_temp = maxtop; #endif } else { - // Skip TIMER2 specific prescalers when not TIMER2 - if (i == 3 || i == 5) continue; - const uint16_t rtf = (F_CPU) / (prescaler[i] * f_desired); - res_temp_fast = rtf - 1; - res_temp_phase_correct = rtf / 2; + if (p == 32 || p == 128) continue; // Skip TIMER2 specific prescalers when not TIMER2 + const uint16_t rft = (F_CPU) / (p * f_desired); + res_fast_temp = rft - 1; + res_pc_temp = rft / 2; } - LIMIT(res_temp_fast, 1U, size); - LIMIT(res_temp_phase_correct, 1U, size); + LIMIT(res_fast_temp, 1U, maxtop); + LIMIT(res_pc_temp, 1U, maxtop); + // Calculate frequencies of test prescaler and resolution values - const int f_temp_fast = (F_CPU) / (prescaler[i] * (1 + res_temp_fast)), - f_temp_phase_correct = (F_CPU) / (2 * prescaler[i] * res_temp_phase_correct), - f_diff = ABS(f - f_desired), - f_fast_diff = ABS(f_temp_fast - f_desired), - f_phase_diff = ABS(f_temp_phase_correct - f_desired); + const uint32_t f_diff = _MAX(f, f_desired) - _MIN(f, f_desired), + f_fast_temp = (F_CPU) / (p * (1 + res_fast_temp)), + f_fast_diff = _MAX(f_fast_temp, f_desired) - _MIN(f_fast_temp, f_desired), + f_pc_temp = (F_CPU) / (2 * p * res_pc_temp), + f_pc_diff = _MAX(f_pc_temp, f_desired) - _MIN(f_pc_temp, f_desired); - // If FAST values are closest to desired f - if (f_fast_diff < f_diff && f_fast_diff <= f_phase_diff) { - // Remember this combination - f = f_temp_fast; - res = res_temp_fast; - j = i; + if (f_fast_diff < f_diff && f_fast_diff <= f_pc_diff) { // FAST values are closest to desired f // Set the Wave Generation Mode to FAST PWM - if (timer.n == 2) { - wgm = ( - #if ENABLED(USE_OCR2A_AS_TOP) - WGM2_FAST_PWM_OCR2A - #else - WGM2_FAST_PWM - #endif - ); - } - else wgm = WGM_FAST_PWM_ICRn; + wgm = is_timer2 ? uint8_t(TERN(USE_OCR2A_AS_TOP, WGM2_FAST_PWM_OCR2A, WGM2_FAST_PWM)) : uint8_t(WGM_FAST_PWM_ICRn); + // Remember this combination + f = f_fast_temp; res = res_fast_temp; j = i + 1; } - // If PHASE CORRECT values are closes to desired f - else if (f_phase_diff < f_diff) { - f = f_temp_phase_correct; - res = res_temp_phase_correct; - j = i; + else if (f_pc_diff < f_diff) { // PHASE CORRECT values are closes to desired f // Set the Wave Generation Mode to PWM PHASE CORRECT - if (timer.n == 2) { - wgm = ( - #if ENABLED(USE_OCR2A_AS_TOP) - WGM2_PWM_PC_OCR2A - #else - WGM2_PWM_PC - #endif - ); - } - else wgm = WGM_PWM_PC_ICRn; + wgm = is_timer2 ? uint8_t(TERN(USE_OCR2A_AS_TOP, WGM2_PWM_PC_OCR2A, WGM2_PWM_PC)) : uint8_t(WGM_PWM_PC_ICRn); + f = f_pc_temp; res = res_pc_temp; j = i + 1; } } } - _SET_WGMnQ(timer.TCCRnQ, wgm); - _SET_CSn(timer.TCCRnQ, j); - if (timer.n == 2) { - #if ENABLED(USE_OCR2A_AS_TOP) - _SET_OCRnQ(timer.OCRnQ, 0, res); // Set OCR2A value (TOP) = res - #endif + _SET_WGMnQ(timer, wgm); + _SET_CSn(timer, j); + + if (is_timer2) { + TERN_(USE_OCR2A_AS_TOP, _SET_OCRnQ(timer, 0, res)); // Set OCR2A value (TOP) = res } else - _SET_ICRn(timer.ICRn, res); // Set ICRn value (TOP) = res + _SET_ICRn(timer, res); // Set ICRn value (TOP) = res } -void set_pwm_duty(const pin_t pin, const uint16_t v, const uint16_t v_size/*=255*/, const bool invert/*=false*/) { +void MarlinHAL::set_pwm_duty(const pin_t pin, const uint16_t v, const uint16_t v_size/*=255*/, const bool invert/*=false*/) { // If v is 0 or v_size (max), digitalWrite to LOW or HIGH. - // Note that digitalWrite also disables pwm output for us (sets COM bit to 0) + // Note that digitalWrite also disables PWM output for us (sets COM bit to 0) if (v == 0) digitalWrite(pin, invert); else if (v == v_size) digitalWrite(pin, !invert); else { - Timer timer = get_pwm_timer(pin); - if (timer.n == 0) return; // Don't proceed if protected timer or not recognised - // Set compare output mode to CLEAR -> SET or SET -> CLEAR (if inverted) - _SET_COMnQ(timer.TCCRnQ, (timer.q - #ifdef TCCR2 - + (timer.q == 2) // COM20 is on bit 4 of TCCR2, thus requires q + 1 in the macro - #endif - ), COM_CLEAR_SET + invert - ); - - uint16_t top; - if (timer.n == 2) { // if TIMER2 - top = ( - #if ENABLED(USE_OCR2A_AS_TOP) - *timer.OCRnQ[0] // top = OCR2A - #else - 255 // top = 0xFF (max) - #endif - ); + const Timer timer = get_pwm_timer(pin); + if (timer.isPWM) { + if (timer.n == 0) { + _SET_COMnQ(timer, timer.q, COM_CLEAR_SET); // Only allow a TIMER0B select... + _SET_OCRnQ(timer, timer.q, v); // ...and OCR0B duty update. For output pin D4 no frequency changes are permitted. + } + else if (!timer.isProtected) { + const uint16_t top = timer.n == 2 ? TERN(USE_OCR2A_AS_TOP, *timer.OCRnQ[0], 255) : *timer.ICRn; + _SET_COMnQ(timer, SUM_TERN(HAS_TCCR2, timer.q, timer.q == 2), COM_CLEAR_SET + invert); // COM20 is on bit 4 of TCCR2, so +1 for q==2 + _SET_OCRnQ(timer, timer.q, uint16_t(uint32_t(v) * top / v_size)); // Scale 8/16-bit v to top value + } } else - top = *timer.ICRn; // top = ICRn - - _SET_OCRnQ(timer.OCRnQ, timer.q, v * float(top) / float(v_size)); // Scale 8/16-bit v to top value + digitalWrite(pin, v < v_size / 2 ? LOW : HIGH); } } -#endif // NEEDS_HARDWARE_PWM +void MarlinHAL::init_pwm_timers() { + // Init some timer frequencies to a default 1KHz + const pin_t pwm_pin[] = { + #ifdef __AVR_ATmega2560__ + 10, 5, 6, 46 + #elif defined(__AVR_ATmega1280__) + 12, 31 + #elif defined(__AVR_ATmega644__) || defined(__AVR_ATmega1284__) + 15, 6 + #elif defined(__AVR_AT90USB1286__) || defined(__AVR_mega64) || defined(__AVR_mega128) + 16, 24 + #endif + }; + + LOOP_L_N(i, COUNT(pwm_pin)) + set_pwm_frequency(pwm_pin[i], 1000); +} + #endif // __AVR__ diff --git a/Marlin/src/HAL/AVR/fastio.cpp b/Marlin/src/HAL/AVR/fastio.cpp index b51d7f97686a..5c6ef1891512 100644 --- a/Marlin/src/HAL/AVR/fastio.cpp +++ b/Marlin/src/HAL/AVR/fastio.cpp @@ -241,11 +241,11 @@ uint8_t extDigitalRead(const int8_t pin) { * * DC values -1.0 to 1.0. Negative duty cycle inverts the pulse. */ -uint16_t set_pwm_frequency_hz(const float &hz, const float dca, const float dcb, const float dcc) { +uint16_t set_pwm_frequency_hz(const_float_t hz, const float dca, const float dcb, const float dcc) { float count = 0; if (hz > 0 && (dca || dcb || dcc)) { count = float(F_CPU) / hz; // 1x prescaler, TOP for 16MHz base freq. - uint16_t prescaler; // Range of 30.5Hz (65535) 64.5KHz (>31) + uint16_t prescaler; // Range of 30.5Hz (65535) 64.5kHz (>31) if (count >= 255. * 256.) { prescaler = 1024; SET_CS(5, PRESCALER_1024); } else if (count >= 255. * 64.) { prescaler = 256; SET_CS(5, PRESCALER_256); } @@ -257,7 +257,7 @@ uint16_t set_pwm_frequency_hz(const float &hz, const float dca, const float dcb, const float pwm_top = round(count); // Get the rounded count ICR5 = (uint16_t)pwm_top - 1; // Subtract 1 for TOP - OCR5A = pwm_top * ABS(dca); // Update and scale DCs + OCR5A = pwm_top * ABS(dca); // Update and scale DCs OCR5B = pwm_top * ABS(dcb); OCR5C = pwm_top * ABS(dcc); _SET_COM(5, A, dca ? (dca < 0 ? COM_SET_CLEAR : COM_CLEAR_SET) : COM_NORMAL); // Set compare modes @@ -267,17 +267,17 @@ uint16_t set_pwm_frequency_hz(const float &hz, const float dca, const float dcb, SET_WGM(5, FAST_PWM_ICRn); // Fast PWM with ICR5 as TOP //SERIAL_ECHOLNPGM("Timer 5 Settings:"); - //SERIAL_ECHOLNPAIR(" Prescaler=", prescaler); - //SERIAL_ECHOLNPAIR(" TOP=", ICR5); - //SERIAL_ECHOLNPAIR(" OCR5A=", OCR5A); - //SERIAL_ECHOLNPAIR(" OCR5B=", OCR5B); - //SERIAL_ECHOLNPAIR(" OCR5C=", OCR5C); + //SERIAL_ECHOLNPGM(" Prescaler=", prescaler); + //SERIAL_ECHOLNPGM(" TOP=", ICR5); + //SERIAL_ECHOLNPGM(" OCR5A=", OCR5A); + //SERIAL_ECHOLNPGM(" OCR5B=", OCR5B); + //SERIAL_ECHOLNPGM(" OCR5C=", OCR5C); } else { // Restore the default for Timer 5 SET_WGM(5, PWM_PC_8); // PWM 8-bit (Phase Correct) SET_COMS(5, NORMAL, NORMAL, NORMAL); // Do nothing - SET_CS(5, PRESCALER_64); // 16MHz / 64 = 250KHz + SET_CS(5, PRESCALER_64); // 16MHz / 64 = 250kHz OCR5A = OCR5B = OCR5C = 0; } return round(count); diff --git a/Marlin/src/HAL/AVR/fastio.h b/Marlin/src/HAL/AVR/fastio.h index dd0163466110..51d3b311ee9d 100644 --- a/Marlin/src/HAL/AVR/fastio.h +++ b/Marlin/src/HAL/AVR/fastio.h @@ -118,7 +118,7 @@ */ // Waveform Generation Modes -enum WaveGenMode : char { +enum WaveGenMode : uint8_t { WGM_NORMAL, // 0 WGM_PWM_PC_8, // 1 WGM_PWM_PC_9, // 2 @@ -138,19 +138,19 @@ enum WaveGenMode : char { }; // Wavefore Generation Modes (Timer 2 only) -enum WaveGenMode2 : char { - WGM2_NORMAL, // 0 - WGM2_PWM_PC, // 1 - WGM2_CTC_OCR2A, // 2 - WGM2_FAST_PWM, // 3 - WGM2_reserved_1, // 4 - WGM2_PWM_PC_OCR2A, // 5 - WGM2_reserved_2, // 6 - WGM2_FAST_PWM_OCR2A, // 7 +enum WaveGenMode2 : uint8_t { + WGM2_NORMAL, // 0 + WGM2_PWM_PC, // 1 + WGM2_CTC_OCR2A, // 2 + WGM2_FAST_PWM, // 3 + WGM2_reserved_1, // 4 + WGM2_PWM_PC_OCR2A, // 5 + WGM2_reserved_2, // 6 + WGM2_FAST_PWM_OCR2A, // 7 }; // Compare Modes -enum CompareMode : char { +enum CompareMode : uint8_t { COM_NORMAL, // 0 COM_TOGGLE, // 1 Non-PWM: OCnx ... Both PWM (WGM 9,11,14,15): OCnA only ... else NORMAL COM_CLEAR_SET, // 2 Non-PWM: OCnx ... Fast PWM: OCnx/Bottom ... PF-FC: OCnx Up/Down @@ -158,7 +158,7 @@ enum CompareMode : char { }; // Clock Sources -enum ClockSource : char { +enum ClockSource : uint8_t { CS_NONE, // 0 CS_PRESCALER_1, // 1 CS_PRESCALER_8, // 2 @@ -170,7 +170,7 @@ enum ClockSource : char { }; // Clock Sources (Timer 2 only) -enum ClockSource2 : char { +enum ClockSource2 : uint8_t { CS2_NONE, // 0 CS2_PRESCALER_1, // 1 CS2_PRESCALER_8, // 2 @@ -203,40 +203,33 @@ enum ClockSource2 : char { TCCR##T##B = (TCCR##T##B & ~(0x3 << WGM##T##2)) | (((int(V) >> 2) & 0x3) << WGM##T##2); \ }while(0) #define SET_WGM(T,V) _SET_WGM(T,WGM_##V) -// Runtime (see set_pwm_frequency): -#define _SET_WGMnQ(TCCRnQ, V) do{ \ - *(TCCRnQ)[0] = (*(TCCRnQ)[0] & ~(0x3 << 0)) | (( int(V) & 0x3) << 0); \ - *(TCCRnQ)[1] = (*(TCCRnQ)[1] & ~(0x3 << 3)) | (((int(V) >> 2) & 0x3) << 3); \ - }while(0) // Set Clock Select bits // Ex: SET_CS3(PRESCALER_64); +#ifdef TCCR2 + #define HAS_TCCR2 1 +#endif #define _SET_CS(T,V) (TCCR##T##B = (TCCR##T##B & ~(0x7 << CS##T##0)) | ((int(V) & 0x7) << CS##T##0)) #define _SET_CS0(V) _SET_CS(0,V) #define _SET_CS1(V) _SET_CS(1,V) -#ifdef TCCR2 - #define _SET_CS2(V) (TCCR2 = (TCCR2 & ~(0x7 << CS20)) | (int(V) << CS20)) -#else - #define _SET_CS2(V) _SET_CS(2,V) -#endif #define _SET_CS3(V) _SET_CS(3,V) #define _SET_CS4(V) _SET_CS(4,V) #define _SET_CS5(V) _SET_CS(5,V) #define SET_CS0(V) _SET_CS0(CS_##V) #define SET_CS1(V) _SET_CS1(CS_##V) -#ifdef TCCR2 + +#if HAS_TCCR2 + #define _SET_CS2(V) (TCCR2 = (TCCR2 & ~(0x7 << CS20)) | (int(V) << CS20)) #define SET_CS2(V) _SET_CS2(CS2_##V) #else + #define _SET_CS2(V) _SET_CS(2,V) #define SET_CS2(V) _SET_CS2(CS_##V) #endif + #define SET_CS3(V) _SET_CS3(CS_##V) #define SET_CS4(V) _SET_CS4(CS_##V) #define SET_CS5(V) _SET_CS5(CS_##V) #define SET_CS(T,V) SET_CS##T(V) -// Runtime (see set_pwm_frequency) -#define _SET_CSn(TCCRnQ, V) do{ \ - (*(TCCRnQ)[1] = (*(TCCRnQ[1]) & ~(0x7 << 0)) | ((int(V) & 0x7) << 0)); \ - }while(0) // Set Compare Mode bits // Ex: SET_COMS(4,CLEAR_SET,CLEAR_SET,CLEAR_SET); @@ -246,22 +239,6 @@ enum ClockSource2 : char { #define SET_COMB(T,V) SET_COM(T,B,V) #define SET_COMC(T,V) SET_COM(T,C,V) #define SET_COMS(T,V1,V2,V3) do{ SET_COMA(T,V1); SET_COMB(T,V2); SET_COMC(T,V3); }while(0) -// Runtime (see set_pwm_duty) -#define _SET_COMnQ(TCCRnQ, Q, V) do{ \ - (*(TCCRnQ)[0] = (*(TCCRnQ)[0] & ~(0x3 << (6-2*(Q)))) | (int(V) << (6-2*(Q)))); \ - }while(0) - -// Set OCRnQ register -// Runtime (see set_pwm_duty): -#define _SET_OCRnQ(OCRnQ, Q, V) do{ \ - (*(OCRnQ)[(Q)] = (0x0000) | (int(V) & 0xFFFF)); \ - }while(0) - -// Set ICRn register (one per timer) -// Runtime (see set_pwm_frequency) -#define _SET_ICRn(ICRn, V) do{ \ - (*(ICRn) = (0x0000) | (int(V) & 0xFFFF)); \ - }while(0) // Set Noise Canceler bit // Ex: SET_ICNC(2,1) @@ -284,8 +261,8 @@ enum ClockSource2 : char { * PWM availability macros */ -// Determine which harware PWMs are already in use -#define _PWM_CHK_FAN_B(P) (P == E0_AUTO_FAN_PIN || P == E1_AUTO_FAN_PIN || P == E2_AUTO_FAN_PIN || P == E3_AUTO_FAN_PIN || P == E4_AUTO_FAN_PIN || P == E5_AUTO_FAN_PIN || P == E6_AUTO_FAN_PIN || P == E7_AUTO_FAN_PIN || P == CHAMBER_AUTO_FAN_PIN) +// Determine which hardware PWMs are already in use +#define _PWM_CHK_FAN_B(P) (P == E0_AUTO_FAN_PIN || P == E1_AUTO_FAN_PIN || P == E2_AUTO_FAN_PIN || P == E3_AUTO_FAN_PIN || P == E4_AUTO_FAN_PIN || P == E5_AUTO_FAN_PIN || P == E6_AUTO_FAN_PIN || P == E7_AUTO_FAN_PIN || P == CHAMBER_AUTO_FAN_PIN || P == COOLER_AUTO_FAN_PIN) #if PIN_EXISTS(CONTROLLER_FAN) #define PWM_CHK_FAN_B(P) (_PWM_CHK_FAN_B(P) || P == CONTROLLER_FAN_PIN) #else diff --git a/Marlin/src/HAL/AVR/inc/SanityCheck.h b/Marlin/src/HAL/AVR/inc/SanityCheck.h index 51ba247953b4..5c1f01a8f477 100644 --- a/Marlin/src/HAL/AVR/inc/SanityCheck.h +++ b/Marlin/src/HAL/AVR/inc/SanityCheck.h @@ -28,22 +28,30 @@ /** * Checks for FAST PWM */ -#if ENABLED(FAST_PWM_FAN) && (ENABLED(USE_OCR2A_AS_TOP) && defined(TCCR2)) - #error "USE_OCR2A_AS_TOP does not apply to devices with a single output TIMER2" +#if ALL(FAST_PWM_FAN, USE_OCR2A_AS_TOP, HAS_TCCR2) + #error "USE_OCR2A_AS_TOP does not apply to devices with a single output TIMER2." +#endif + +/** + * Checks for SOFT PWM + */ +#if HAS_FAN0 && FAN_PIN == 9 && DISABLED(FAN_SOFT_PWM) && ENABLED(SPEAKER) + #error "FAN_PIN 9 Hardware PWM uses Timer 2 which conflicts with Arduino AVR Tone Timer (for SPEAKER)." + #error "Disable SPEAKER or enable FAN_SOFT_PWM." #endif /** * Sanity checks for Spindle / Laser PWM */ -#if ENABLED(SPINDLE_LASER_PWM) +#if ENABLED(SPINDLE_LASER_USE_PWM) #include "../ServoTimers.h" // Needed to check timer availability (_useTimer3) #if SPINDLE_LASER_PWM_PIN == 4 || WITHIN(SPINDLE_LASER_PWM_PIN, 11, 13) #error "Counter/Timer for SPINDLE_LASER_PWM_PIN is used by a system interrupt." #elif NUM_SERVOS > 0 && defined(_useTimer3) && (WITHIN(SPINDLE_LASER_PWM_PIN, 2, 3) || SPINDLE_LASER_PWM_PIN == 5) #error "Counter/Timer for SPINDLE_LASER_PWM_PIN is used by the servo system." #endif -#elif defined(SPINDLE_LASER_FREQUENCY) - #error "SPINDLE_LASER_FREQUENCY requires SPINDLE_LASER_PWM." +#elif SPINDLE_LASER_FREQUENCY + #error "SPINDLE_LASER_FREQUENCY requires SPINDLE_LASER_USE_PWM." #endif /** diff --git a/Marlin/src/HAL/AVR/math.h b/Marlin/src/HAL/AVR/math.h index 7ede4accc09e..7dd1018ff199 100644 --- a/Marlin/src/HAL/AVR/math.h +++ b/Marlin/src/HAL/AVR/math.h @@ -35,7 +35,7 @@ // C B A is longIn1 // D C B A is longIn2 // -static FORCE_INLINE uint16_t MultiU24X32toH16(uint32_t longIn1, uint32_t longIn2) { +FORCE_INLINE static uint16_t MultiU24X32toH16(uint32_t longIn1, uint32_t longIn2) { uint8_t tmp1; uint8_t tmp2; uint16_t intRes; @@ -89,7 +89,7 @@ static FORCE_INLINE uint16_t MultiU24X32toH16(uint32_t longIn1, uint32_t longIn2 // uses: // r26 to store 0 // r27 to store the byte 1 of the 24 bit result -static FORCE_INLINE uint16_t MultiU16X8toH16(uint8_t charIn1, uint16_t intIn2) { +FORCE_INLINE static uint16_t MultiU16X8toH16(uint8_t charIn1, uint16_t intIn2) { uint8_t tmp; uint16_t intRes; __asm__ __volatile__ ( diff --git a/Marlin/src/HAL/AVR/pinsDebug.h b/Marlin/src/HAL/AVR/pinsDebug.h index 6bf9f33a0ce5..0f564df987f3 100644 --- a/Marlin/src/HAL/AVR/pinsDebug.h +++ b/Marlin/src/HAL/AVR/pinsDebug.h @@ -2,6 +2,9 @@ * Marlin 3D Printer Firmware * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * * This program is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation, either version 3 of the License, or @@ -38,7 +41,7 @@ // portModeRegister takes a different argument #define digitalPinToTimer_DEBUG(p) digitalPinToTimer(p) #define digitalPinToBitMask_DEBUG(p) digitalPinToBitMask(p) - #define digitalPinToPort_DEBUG(p) digitalPinToPort_Teensy(p) + #define digitalPinToPort_DEBUG(p) digitalPinToPort(p) #define GET_PINMODE(pin) (*portModeRegister(pin) & digitalPinToBitMask_DEBUG(pin)) #elif AVR_ATmega2560_FAMILY_PLUS_70 // So we can access/display all the pins on boards using more than 70 @@ -99,7 +102,7 @@ void PRINT_ARRAY_NAME(uint8_t x) { return true; \ } else return false - +#define ABTEST(N) defined(TCCR##N##A) && defined(COM##N##A1) /** * Print a pin's PWM status. @@ -110,7 +113,7 @@ static bool pwm_status(uint8_t pin) { switch (digitalPinToTimer_DEBUG(pin)) { - #if defined(TCCR0A) && defined(COM0A1) + #if ABTEST(0) #ifdef TIMER0A #if !AVR_AT90USB1286_FAMILY // not available in Teensyduino type IDEs PWM_CASE(0, A); @@ -119,20 +122,20 @@ static bool pwm_status(uint8_t pin) { PWM_CASE(0, B); #endif - #if defined(TCCR1A) && defined(COM1A1) + #if ABTEST(1) PWM_CASE(1, A); PWM_CASE(1, B); - #if defined(COM1C1) && defined(TIMER1C) - PWM_CASE(1, C); - #endif + #if defined(COM1C1) && defined(TIMER1C) + PWM_CASE(1, C); + #endif #endif - #if defined(TCCR2A) && defined(COM2A1) + #if ABTEST(2) PWM_CASE(2, A); PWM_CASE(2, B); #endif - #if defined(TCCR3A) && defined(COM3A1) + #if ABTEST(3) PWM_CASE(3, A); PWM_CASE(3, B); #ifdef COM3C1 @@ -146,7 +149,7 @@ static bool pwm_status(uint8_t pin) { PWM_CASE(4, C); #endif - #if defined(TCCR5A) && defined(COM5A1) + #if ABTEST(5) PWM_CASE(5, A); PWM_CASE(5, B); PWM_CASE(5, C); @@ -163,16 +166,16 @@ static bool pwm_status(uint8_t pin) { const volatile uint8_t* const PWM_other[][3] PROGMEM = { { &TCCR0A, &TCCR0B, &TIMSK0 }, { &TCCR1A, &TCCR1B, &TIMSK1 }, - #if defined(TCCR2A) && defined(COM2A1) + #if ABTEST(2) { &TCCR2A, &TCCR2B, &TIMSK2 }, #endif - #if defined(TCCR3A) && defined(COM3A1) + #if ABTEST(3) { &TCCR3A, &TCCR3B, &TIMSK3 }, #endif #ifdef TCCR4A { &TCCR4A, &TCCR4B, &TIMSK4 }, #endif - #if defined(TCCR5A) && defined(COM5A1) + #if ABTEST(5) { &TCCR5A, &TCCR5B, &TIMSK5 }, #endif }; @@ -192,11 +195,11 @@ const volatile uint8_t* const PWM_OCR[][3] PROGMEM = { { (const uint8_t*)&OCR1A, (const uint8_t*)&OCR1B, 0 }, #endif - #if defined(TCCR2A) && defined(COM2A1) + #if ABTEST(2) { &OCR2A, &OCR2B, 0 }, #endif - #if defined(TCCR3A) && defined(COM3A1) + #if ABTEST(3) #ifdef COM3C1 { (const uint8_t*)&OCR3A, (const uint8_t*)&OCR3B, (const uint8_t*)&OCR3C }, #else @@ -208,7 +211,7 @@ const volatile uint8_t* const PWM_OCR[][3] PROGMEM = { { (const uint8_t*)&OCR4A, (const uint8_t*)&OCR4B, (const uint8_t*)&OCR4C }, #endif - #if defined(TCCR5A) && defined(COM5A1) + #if ABTEST(5) { (const uint8_t*)&OCR5A, (const uint8_t*)&OCR5B, (const uint8_t*)&OCR5C }, #endif }; @@ -235,9 +238,9 @@ static void print_is_also_tied() { SERIAL_ECHOPGM(" is also tied to this pin"); inline void com_print(const uint8_t N, const uint8_t Z) { const uint8_t *TCCRA = (uint8_t*)TCCR_A(N); - SERIAL_ECHOPAIR(" COM", AS_CHAR('0' + N)); + SERIAL_ECHOPGM(" COM", AS_DIGIT(N)); SERIAL_CHAR(Z); - SERIAL_ECHOPAIR(": ", int((*TCCRA >> (6 - Z * 2)) & 0x03)); + SERIAL_ECHOPGM(": ", int((*TCCRA >> (6 - Z * 2)) & 0x03)); } void timer_prefix(uint8_t T, char L, uint8_t N) { // T - timer L - pwm N - WGM bit layout @@ -247,7 +250,7 @@ void timer_prefix(uint8_t T, char L, uint8_t N) { // T - timer L - pwm N - uint8_t WGM = (((*TCCRB & _BV(WGM_2)) >> 1) | (*TCCRA & (_BV(WGM_0) | _BV(WGM_1)))); if (N == 4) WGM |= ((*TCCRB & _BV(WGM_3)) >> 1); - SERIAL_ECHOPAIR(" TIMER", AS_CHAR(T + '0')); + SERIAL_ECHOPGM(" TIMER", AS_DIGIT(T)); SERIAL_CHAR(L); SERIAL_ECHO_SP(3); @@ -259,14 +262,14 @@ void timer_prefix(uint8_t T, char L, uint8_t N) { // T - timer L - pwm N - const uint16_t *OCRVAL16 = (uint16_t*)OCR_VAL(T, L - 'A'); PWM_PRINT(*OCRVAL16); } - SERIAL_ECHOPAIR(" WGM: ", WGM); + SERIAL_ECHOPGM(" WGM: ", WGM); com_print(T,L); - SERIAL_ECHOPAIR(" CS: ", (*TCCRB & (_BV(CS_0) | _BV(CS_1) | _BV(CS_2)) )); - SERIAL_ECHOPAIR(" TCCR", AS_CHAR(T + '0'), "A: ", *TCCRA); - SERIAL_ECHOPAIR(" TCCR", AS_CHAR(T + '0'), "B: ", *TCCRB); + SERIAL_ECHOPGM(" CS: ", (*TCCRB & (_BV(CS_0) | _BV(CS_1) | _BV(CS_2)) )); + SERIAL_ECHOPGM(" TCCR", AS_DIGIT(T), "A: ", *TCCRA); + SERIAL_ECHOPGM(" TCCR", AS_DIGIT(T), "B: ", *TCCRB); const uint8_t *TMSK = (uint8_t*)TIMSK(T); - SERIAL_ECHOPAIR(" TIMSK", AS_CHAR(T + '0'), ": ", *TMSK); + SERIAL_ECHOPGM(" TIMSK", AS_DIGIT(T), ": ", *TMSK); const uint8_t OCIE = L - 'A' + 1; if (N == 3) { if (WGM == 0 || WGM == 2 || WGM == 4 || WGM == 6) err_is_counter(); } @@ -278,7 +281,7 @@ void timer_prefix(uint8_t T, char L, uint8_t N) { // T - timer L - pwm N - static void pwm_details(uint8_t pin) { switch (digitalPinToTimer_DEBUG(pin)) { - #if defined(TCCR0A) && defined(COM0A1) + #if ABTEST(0) #ifdef TIMER0A #if !AVR_AT90USB1286_FAMILY // not available in Teensyduino type IDEs case TIMER0A: timer_prefix(0, 'A', 3); break; @@ -287,7 +290,7 @@ static void pwm_details(uint8_t pin) { case TIMER0B: timer_prefix(0, 'B', 3); break; #endif - #if defined(TCCR1A) && defined(COM1A1) + #if ABTEST(1) case TIMER1A: timer_prefix(1, 'A', 4); break; case TIMER1B: timer_prefix(1, 'B', 4); break; #if defined(COM1C1) && defined(TIMER1C) @@ -295,12 +298,12 @@ static void pwm_details(uint8_t pin) { #endif #endif - #if defined(TCCR2A) && defined(COM2A1) + #if ABTEST(2) case TIMER2A: timer_prefix(2, 'A', 3); break; case TIMER2B: timer_prefix(2, 'B', 3); break; #endif - #if defined(TCCR3A) && defined(COM3A1) + #if ABTEST(3) case TIMER3A: timer_prefix(3, 'A', 4); break; case TIMER3B: timer_prefix(3, 'B', 4); break; #ifdef COM3C1 @@ -314,7 +317,7 @@ static void pwm_details(uint8_t pin) { case TIMER4C: timer_prefix(4, 'C', 4); break; #endif - #if defined(TCCR5A) && defined(COM5A1) + #if ABTEST(5) case TIMER5A: timer_prefix(5, 'A', 4); break; case TIMER5B: timer_prefix(5, 'B', 4); break; case TIMER5C: timer_prefix(5, 'C', 4); break; @@ -348,7 +351,6 @@ static void pwm_details(uint8_t pin) { #endif } // pwm_details - #ifndef digitalRead_mod // Use Teensyduino's version of digitalRead - it doesn't disable the PWMs int digitalRead_mod(const int8_t pin) { // same as digitalRead except the PWM stop section has been removed const uint8_t port = digitalPinToPort_DEBUG(pin); @@ -393,3 +395,6 @@ static void pwm_details(uint8_t pin) { #endif #define PRINT_PIN(p) do{ sprintf_P(buffer, PSTR("%3d "), p); SERIAL_ECHO(buffer); }while(0) +#define PRINT_PIN_ANALOG(p) do{ sprintf_P(buffer, PSTR(" (A%2d) "), DIGITAL_PIN_TO_ANALOG_PIN(pin)); SERIAL_ECHO(buffer); }while(0) + +#undef ABTEST diff --git a/Marlin/src/HAL/AVR/pinsDebug_Teensyduino.h b/Marlin/src/HAL/AVR/pinsDebug_Teensyduino.h index 051972a861ad..582ae79ba787 100644 --- a/Marlin/src/HAL/AVR/pinsDebug_Teensyduino.h +++ b/Marlin/src/HAL/AVR/pinsDebug_Teensyduino.h @@ -2,6 +2,9 @@ * Marlin 3D Printer Firmware * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * * This program is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation, either version 3 of the License, or diff --git a/Marlin/src/HAL/AVR/pinsDebug_plus_70.h b/Marlin/src/HAL/AVR/pinsDebug_plus_70.h index db3fdf1f767b..d9aa44c3cb15 100644 --- a/Marlin/src/HAL/AVR/pinsDebug_plus_70.h +++ b/Marlin/src/HAL/AVR/pinsDebug_plus_70.h @@ -2,6 +2,9 @@ * Marlin 3D Printer Firmware * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * * This program is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation, either version 3 of the License, or diff --git a/Marlin/src/HAL/AVR/timers.h b/Marlin/src/HAL/AVR/timers.h index 82eb8b14b161..33c3880b6b99 100644 --- a/Marlin/src/HAL/AVR/timers.h +++ b/Marlin/src/HAL/AVR/timers.h @@ -34,14 +34,14 @@ typedef uint16_t hal_timer_t; #define HAL_TIMER_RATE ((F_CPU) / 8) // i.e., 2MHz or 2.5MHz -#ifndef STEP_TIMER_NUM - #define STEP_TIMER_NUM 1 +#ifndef MF_TIMER_STEP + #define MF_TIMER_STEP 1 #endif -#ifndef PULSE_TIMER_NUM - #define PULSE_TIMER_NUM STEP_TIMER_NUM +#ifndef MF_TIMER_PULSE + #define MF_TIMER_PULSE MF_TIMER_STEP #endif -#ifndef TEMP_TIMER_NUM - #define TEMP_TIMER_NUM 0 +#ifndef MF_TIMER_TEMP + #define MF_TIMER_TEMP 0 #endif #define TEMP_TIMER_FREQUENCY ((F_CPU) / 64.0 / 256.0) @@ -58,13 +58,13 @@ typedef uint16_t hal_timer_t; #define DISABLE_STEPPER_DRIVER_INTERRUPT() CBI(TIMSK1, OCIE1A) #define STEPPER_ISR_ENABLED() TEST(TIMSK1, OCIE1A) -#define ENABLE_TEMPERATURE_INTERRUPT() SBI(TIMSK0, OCIE0B) -#define DISABLE_TEMPERATURE_INTERRUPT() CBI(TIMSK0, OCIE0B) -#define TEMPERATURE_ISR_ENABLED() TEST(TIMSK0, OCIE0B) +#define ENABLE_TEMPERATURE_INTERRUPT() SBI(TIMSK0, OCIE0A) +#define DISABLE_TEMPERATURE_INTERRUPT() CBI(TIMSK0, OCIE0A) +#define TEMPERATURE_ISR_ENABLED() TEST(TIMSK0, OCIE0A) FORCE_INLINE void HAL_timer_start(const uint8_t timer_num, const uint32_t) { switch (timer_num) { - case STEP_TIMER_NUM: + case MF_TIMER_STEP: // waveform generation = 0100 = CTC SET_WGM(1, CTC_OCRnA); @@ -84,10 +84,10 @@ FORCE_INLINE void HAL_timer_start(const uint8_t timer_num, const uint32_t) { TCNT1 = 0; break; - case TEMP_TIMER_NUM: + case MF_TIMER_TEMP: // Use timer0 for temperature measurement // Interleave temperature interrupt with millies interrupt - OCR0B = 128; + OCR0A = 128; break; } } @@ -109,12 +109,12 @@ FORCE_INLINE void HAL_timer_start(const uint8_t timer_num, const uint32_t) { * (otherwise, characters will be lost due to UART overflow). * Then: Stepper, Endstops, Temperature, and -finally- all others. */ -#define HAL_timer_isr_prologue(TIMER_NUM) -#define HAL_timer_isr_epilogue(TIMER_NUM) +#define HAL_timer_isr_prologue(T) NOOP +#define HAL_timer_isr_epilogue(T) NOOP -/* 18 cycles maximum latency */ #ifndef HAL_STEP_TIMER_ISR +/* 18 cycles maximum latency */ #define HAL_STEP_TIMER_ISR() \ extern "C" void TIMER1_COMPA_vect() __attribute__ ((signal, naked, used, externally_visible)); \ extern "C" void TIMER1_COMPA_vect_bottom() asm ("TIMER1_COMPA_vect_bottom") __attribute__ ((used, externally_visible, noinline)); \ @@ -180,7 +180,7 @@ void TIMER1_COMPA_vect() { \ : \ : [timsk0] "i" ((uint16_t)&TIMSK0), \ [timsk1] "i" ((uint16_t)&TIMSK1), \ - [msk0] "M" ((uint8_t)(1< +#include -uint8_t u8g_bitData, u8g_bitNotData, u8g_bitClock, u8g_bitNotClock; -volatile uint8_t *u8g_outData, *u8g_outClock; +static uint8_t u8g_bitData, u8g_bitNotData, u8g_bitClock, u8g_bitNotClock; +static volatile uint8_t *u8g_outData, *u8g_outClock; static void u8g_com_arduino_init_shift_out(uint8_t dataPin, uint8_t clockPin) { u8g_outData = portOutputRegister(digitalPinToPort(dataPin)); diff --git a/Marlin/src/HAL/DUE/HAL.cpp b/Marlin/src/HAL/DUE/HAL.cpp index 034b86ccb07c..bbd13dc05aac 100644 --- a/Marlin/src/HAL/DUE/HAL.cpp +++ b/Marlin/src/HAL/DUE/HAL.cpp @@ -34,7 +34,7 @@ // Public Variables // ------------------------ -uint16_t HAL_adc_result; +uint16_t MarlinHAL::adc_result; // ------------------------ // Public functions @@ -42,8 +42,7 @@ uint16_t HAL_adc_result; TERN_(POSTMORTEM_DEBUGGING, extern void install_min_serial()); -// HAL initialization task -void HAL_init() { +void MarlinHAL::init() { // Initialize the USB stack #if ENABLED(SDSUPPORT) OUT_WRITE(SDSS, HIGH); // Try to set SDSS inactive before any other SPI users start up @@ -52,21 +51,15 @@ void HAL_init() { TERN_(POSTMORTEM_DEBUGGING, install_min_serial()); // Install the min serial handler } -// HAL idle task -void HAL_idletask() { - // Perform USB stack housekeeping - usb_task_idle(); +void MarlinHAL::init_board() { + #ifdef BOARD_INIT + BOARD_INIT(); + #endif } -// Disable interrupts -void cli() { noInterrupts(); } - -// Enable interrupts -void sei() { interrupts(); } - -void HAL_clear_reset_source() { } +void MarlinHAL::idletask() { usb_task_idle(); } // Perform USB stack housekeeping -uint8_t HAL_get_reset_source() { +uint8_t MarlinHAL::get_reset_source() { switch ((RSTC->RSTC_SR >> 8) & 0x07) { case 0: return RST_POWER_ON; case 1: return RST_BACKUP; @@ -77,10 +70,7 @@ uint8_t HAL_get_reset_source() { } } -void _delay_ms(const int delay_ms) { - // Todo: port for Due? - delay(delay_ms); -} +void MarlinHAL::reboot() { rstc_start_software_reset(RSTC); } extern "C" { extern unsigned int _ebss; // end of bss section @@ -92,31 +82,18 @@ int freeMemory() { return (int)&free_memory - (heap_end ?: (int)&_ebss); } -// ------------------------ -// ADC -// ------------------------ - -void HAL_adc_start_conversion(const uint8_t ch) { - HAL_adc_result = analogRead(ch); -} - -uint16_t HAL_adc_get_result() { - // nop - return HAL_adc_result; -} - // Forward the default serial ports -#if ANY_SERIAL_IS(0) - DefaultSerial MSerial(false, Serial); +#if USING_HW_SERIAL0 + DefaultSerial1 MSerial0(false, Serial); #endif -#if ANY_SERIAL_IS(1) - DefaultSerial1 MSerial1(false, Serial1); +#if USING_HW_SERIAL1 + DefaultSerial2 MSerial1(false, Serial1); #endif -#if ANY_SERIAL_IS(2) - DefaultSerial2 MSerial2(false, Serial2); +#if USING_HW_SERIAL2 + DefaultSerial3 MSerial2(false, Serial2); #endif -#if ANY_SERIAL_IS(3) - DefaultSerial3 MSerial3(false, Serial3); +#if USING_HW_SERIAL3 + DefaultSerial4 MSerial3(false, Serial3); #endif #endif // ARDUINO_ARCH_SAM diff --git a/Marlin/src/HAL/DUE/HAL.h b/Marlin/src/HAL/DUE/HAL.h index b1c6a38c0fe5..9a02c9a0dcf5 100644 --- a/Marlin/src/HAL/DUE/HAL.h +++ b/Marlin/src/HAL/DUE/HAL.h @@ -38,35 +38,47 @@ #include "../../core/serial_hook.h" -typedef ForwardSerial0Type< decltype(Serial) > DefaultSerial; -typedef ForwardSerial0Type< decltype(Serial1) > DefaultSerial1; -typedef ForwardSerial0Type< decltype(Serial2) > DefaultSerial2; -typedef ForwardSerial0Type< decltype(Serial3) > DefaultSerial3; -extern DefaultSerial MSerial; -extern DefaultSerial1 MSerial1; -extern DefaultSerial2 MSerial2; -extern DefaultSerial3 MSerial3; +// ------------------------ +// Serial ports +// ------------------------ + +typedef ForwardSerial1Class< decltype(Serial) > DefaultSerial1; +typedef ForwardSerial1Class< decltype(Serial1) > DefaultSerial2; +typedef ForwardSerial1Class< decltype(Serial2) > DefaultSerial3; +typedef ForwardSerial1Class< decltype(Serial3) > DefaultSerial4; +extern DefaultSerial1 MSerial0; +extern DefaultSerial2 MSerial1; +extern DefaultSerial3 MSerial2; +extern DefaultSerial4 MSerial3; #define _MSERIAL(X) MSerial##X #define MSERIAL(X) _MSERIAL(X) -#define MSerial0 MSerial -// Define MYSERIAL0/1 before MarlinSerial includes! #if SERIAL_PORT == -1 || ENABLED(EMERGENCY_PARSER) - #define MYSERIAL0 customizedSerial1 + #define MYSERIAL1 customizedSerial1 #elif WITHIN(SERIAL_PORT, 0, 3) - #define MYSERIAL0 MSERIAL(SERIAL_PORT) + #define MYSERIAL1 MSERIAL(SERIAL_PORT) #else - #error "The required SERIAL_PORT must be from -1 to 3. Please update your configuration." + #error "The required SERIAL_PORT must be from 0 to 3, or -1 for USB Serial." #endif #ifdef SERIAL_PORT_2 #if SERIAL_PORT_2 == -1 || ENABLED(EMERGENCY_PARSER) - #define MYSERIAL1 customizedSerial2 + #define MYSERIAL2 customizedSerial2 #elif WITHIN(SERIAL_PORT_2, 0, 3) - #define MYSERIAL1 MSERIAL(SERIAL_PORT_2) + #define MYSERIAL2 MSERIAL(SERIAL_PORT_2) #else - #error "SERIAL_PORT_2 must be from -1 to 3. Please update your configuration." + #error "SERIAL_PORT_2 must be from 0 to 3, or -1 for USB Serial." + #endif +#endif + +#ifdef SERIAL_PORT_3 + #if SERIAL_PORT_3 == -1 || ENABLED(EMERGENCY_PARSER) + #define MYSERIAL3 customizedSerial3 + #elif WITHIN(SERIAL_PORT_3, 0, 3) + #define MYSERIAL3 MSERIAL(SERIAL_PORT_3) + #else + #error "SERIAL_PORT_3 must be from 0 to 3, or -1 for USB Serial." #endif #endif @@ -74,72 +86,53 @@ extern DefaultSerial3 MSerial3; #if WITHIN(MMU2_SERIAL_PORT, 0, 3) #define MMU2_SERIAL MSERIAL(MMU2_SERIAL_PORT) #else - #error "MMU2_SERIAL_PORT must be from 0 to 3. Please update your configuration." + #error "MMU2_SERIAL_PORT must be from 0 to 3." #endif #endif #ifdef LCD_SERIAL_PORT - #if LCD_SERIAL_PORT == -1 - #define LCD_SERIAL lcdSerial - #elif WITHIN(LCD_SERIAL_PORT, 0, 3) + #if WITHIN(LCD_SERIAL_PORT, 0, 3) #define LCD_SERIAL MSERIAL(LCD_SERIAL_PORT) #else - #error "LCD_SERIAL_PORT must be from -1 to 3. Please update your configuration." + #error "LCD_SERIAL_PORT must be from 0 to 3." #endif #endif #include "MarlinSerial.h" #include "MarlinSerialUSB.h" -// On AVR this is in math.h? -#define square(x) ((x)*(x)) +// ------------------------ +// Types +// ------------------------ typedef int8_t pin_t; -#define SHARED_SERVOS HAS_SERVOS -#define HAL_SERVO_LIB Servo +#define SHARED_SERVOS HAS_SERVOS // Use shared/servos.cpp + +class Servo; +typedef Servo hal_servo_t; // // Interrupts // -#define CRITICAL_SECTION_START() uint32_t primask = __get_PRIMASK(); __disable_irq() -#define CRITICAL_SECTION_END() if (!primask) __enable_irq() -#define ISRS_ENABLED() (!__get_PRIMASK()) -#define ENABLE_ISRS() __enable_irq() -#define DISABLE_ISRS() __disable_irq() - -void cli(); // Disable interrupts -void sei(); // Enable interrupts +#define sei() noInterrupts() +#define cli() interrupts() -void HAL_clear_reset_source(); // clear reset reason -uint8_t HAL_get_reset_source(); // get reset reason - -inline void HAL_reboot() {} // reboot the board or restart the bootloader +#define CRITICAL_SECTION_START() const bool _irqon = hal.isr_state(); hal.isr_off() +#define CRITICAL_SECTION_END() if (_irqon) hal.isr_on() // // ADC // -extern uint16_t HAL_adc_result; // result of last ADC conversion +#define HAL_ADC_VREF 3.3 +#define HAL_ADC_RESOLUTION 10 #ifndef analogInputToDigitalPin #define analogInputToDigitalPin(p) ((p < 12U) ? (p) + 54U : -1) #endif -#define HAL_ANALOG_SELECT(ch) - -inline void HAL_adc_init() {}//todo - -#define HAL_ADC_VREF 3.3 -#define HAL_ADC_RESOLUTION 10 -#define HAL_START_ADC(ch) HAL_adc_start_conversion(ch) -#define HAL_READ_ADC() HAL_adc_result -#define HAL_ADC_READY() true - -void HAL_adc_start_conversion(const uint8_t ch); -uint16_t HAL_adc_get_result(); - // -// Pin Map +// Pin Mapping for M42, M43, M226 // #define GET_PIN_MAP_PIN(index) index #define GET_PIN_MAP_INDEX(pin) pin @@ -148,30 +141,19 @@ uint16_t HAL_adc_get_result(); // // Tone // -void toneInit(); void tone(const pin_t _pin, const unsigned int frequency, const unsigned long duration=0); void noTone(const pin_t _pin); -// Enable hooks into idle and setup for HAL -#define HAL_IDLETASK 1 -void HAL_idletask(); -void HAL_init(); - -// -// Utility functions -// -void _delay_ms(const int delay); +// ------------------------ +// Class Utilities +// ------------------------ +#pragma GCC diagnostic push #if GCC_VERSION <= 50000 - #pragma GCC diagnostic push #pragma GCC diagnostic ignored "-Wunused-function" #endif -int freeMemory(); - -#if GCC_VERSION <= 50000 - #pragma GCC diagnostic pop -#endif +#pragma GCC diagnostic pop #ifdef __cplusplus extern "C" { @@ -180,3 +162,69 @@ char *dtostrf(double __val, signed char __width, unsigned char __prec, char *__s #ifdef __cplusplus } #endif + +// Return free RAM between end of heap (or end bss) and whatever is current +int freeMemory(); + +// ------------------------ +// MarlinHAL Class +// ------------------------ + +class MarlinHAL { +public: + + // Earliest possible init, before setup() + MarlinHAL() {} + + static void init(); // Called early in setup() + static void init_board(); // Called less early in setup() + static void reboot(); // Software reset + + // Interrupts + static bool isr_state() { return !__get_PRIMASK(); } + static void isr_on() { __enable_irq(); } + static void isr_off() { __disable_irq(); } + + static void delay_ms(const int ms) { delay(ms); } + + // Tasks, called from idle() + static void idletask(); + + // Reset + static uint8_t get_reset_source(); + static void clear_reset_source() {} + + // Free SRAM + static int freeMemory() { return ::freeMemory(); } + + // + // ADC Methods + // + + static uint16_t adc_result; + + // Called by Temperature::init once at startup + static void adc_init() {} + + // Called by Temperature::init for each sensor at startup + static void adc_enable(const uint8_t ch) {} + + // Begin ADC sampling on the given channel + static void adc_start(const uint8_t ch) { adc_result = analogRead(ch); } + + // Is the ADC ready for reading? + static bool adc_ready() { return true; } + + // The current value of the ADC register + static uint16_t adc_value() { return adc_result; } + + /** + * Set the PWM duty cycle for the pin to the given value. + * No inverting the duty cycle in this HAL. + * No changing the maximum size of the provided value to enable finer PWM duty control in this HAL. + */ + static void set_pwm_duty(const pin_t pin, const uint16_t v, const uint16_t=255, const bool=false) { + analogWrite(pin, v); + } + +}; diff --git a/Marlin/src/HAL/DUE/HAL_SPI.cpp b/Marlin/src/HAL/DUE/HAL_SPI.cpp index 6cb2912c12a4..c5e8f2433d7d 100644 --- a/Marlin/src/HAL/DUE/HAL_SPI.cpp +++ b/Marlin/src/HAL/DUE/HAL_SPI.cpp @@ -56,8 +56,8 @@ #pragma GCC optimize (3) typedef uint8_t (*pfnSpiTransfer)(uint8_t b); - typedef void (*pfnSpiRxBlock)(uint8_t* buf, uint32_t nbyte); - typedef void (*pfnSpiTxBlock)(const uint8_t* buf, uint32_t nbyte); + typedef void (*pfnSpiRxBlock)(uint8_t *buf, uint32_t nbyte); + typedef void (*pfnSpiTxBlock)(const uint8_t *buf, uint32_t nbyte); /* ---------------- Macros to be able to access definitions from asm */ #define _PORT(IO) DIO ## IO ## _WPORT @@ -240,7 +240,7 @@ } // all the others - static uint32_t spiDelayCyclesX4 = 4 * (F_CPU) / 1000000; // 4µs => 125khz + static uint16_t spiDelayNS = 4000; // 4000ns => 125khz static uint8_t spiTransferX(uint8_t b) { // using Mode 0 int bits = 8; @@ -249,12 +249,12 @@ b <<= 1; // little setup time WRITE(SD_SCK_PIN, HIGH); - DELAY_CYCLES(spiDelayCyclesX4); + DELAY_NS(spiDelayNS); b |= (READ(SD_MISO_PIN) != 0); WRITE(SD_SCK_PIN, LOW); - DELAY_CYCLES(spiDelayCyclesX4); + DELAY_NS(spiDelayNS); } while (--bits); return b; } @@ -270,7 +270,7 @@ static pfnSpiTransfer spiTransferTx = (pfnSpiTransfer)spiTransferX; // Block transfers run at ~8 .. ~10Mhz - Tx version (Rx data discarded) - static void spiTxBlock0(const uint8_t* ptr, uint32_t todo) { + static void spiTxBlock0(const uint8_t *ptr, uint32_t todo) { uint32_t MOSI_PORT_PLUS30 = ((uint32_t) PORT(SD_MOSI_PIN)) + 0x30; /* SODR of port */ uint32_t MOSI_MASK = PIN_MASK(SD_MOSI_PIN); uint32_t SCK_PORT_PLUS30 = ((uint32_t) PORT(SD_SCK_PIN)) + 0x30; /* SODR of port */ @@ -349,7 +349,7 @@ ); } - static void spiRxBlock0(uint8_t* ptr, uint32_t todo) { + static void spiRxBlock0(uint8_t *ptr, uint32_t todo) { uint32_t bin = 0; uint32_t work = 0; uint32_t BITBAND_MISO_PORT = BITBAND_ADDRESS( ((uint32_t)PORT(SD_MISO_PIN))+0x3C, PIN_SHIFT(SD_MISO_PIN)); /* PDSR of port in bitband area */ @@ -425,19 +425,19 @@ ); } - static void spiTxBlockX(const uint8_t* buf, uint32_t todo) { + static void spiTxBlockX(const uint8_t *buf, uint32_t todo) { do { (void)spiTransferTx(*buf++); } while (--todo); } - static void spiRxBlockX(uint8_t* buf, uint32_t todo) { + static void spiRxBlockX(uint8_t *buf, uint32_t todo) { do { *buf++ = spiTransferRx(0xFF); } while (--todo); } - // Pointers to generic functions for block tranfers + // Pointers to generic functions for block transfers static pfnSpiTxBlock spiTxBlock = (pfnSpiTxBlock)spiTxBlockX; static pfnSpiRxBlock spiRxBlock = (pfnSpiRxBlock)spiRxBlockX; @@ -463,7 +463,7 @@ return b; } - void spiRead(uint8_t* buf, uint16_t nbyte) { + void spiRead(uint8_t *buf, uint16_t nbyte) { if (nbyte) { _SS_WRITE(LOW); WRITE(SD_MOSI_PIN, HIGH); // Output 1s 1 @@ -478,7 +478,7 @@ _SS_WRITE(HIGH); } - void spiSendBlock(uint8_t token, const uint8_t* buf) { + void spiSendBlock(uint8_t token, const uint8_t *buf) { _SS_WRITE(LOW); (void)spiTransferTx(token); spiTxBlock(buf, 512); @@ -510,7 +510,7 @@ spiRxBlock = (pfnSpiRxBlock)spiRxBlockX; break; default: - spiDelayCyclesX4 = ((F_CPU) / 1000000) >> (6 - spiRate) << 2; // spiRate of 2 gives the maximum error with current CPU + spiDelayNS = 4000 >> (6 - spiRate); // spiRate of 2 gives the maximum error with current CPU spiTransferTx = (pfnSpiTransfer)spiTransferX; spiTransferRx = (pfnSpiTransfer)spiTransferX; spiTxBlock = (pfnSpiTxBlock)spiTxBlockX; @@ -594,18 +594,14 @@ SPI_Configure(SPI0, ID_SPI0, SPI_MR_MSTR | SPI_MR_MODFDIS | SPI_MR_PS); SPI_Enable(SPI0); - SET_OUTPUT(DAC0_SYNC); + SET_OUTPUT(DAC0_SYNC_PIN); #if HAS_MULTI_EXTRUDER - SET_OUTPUT(DAC1_SYNC); - WRITE(DAC1_SYNC, HIGH); + OUT_WRITE(DAC1_SYNC_PIN, HIGH); #endif - SET_OUTPUT(SPI_EEPROM1_CS); - SET_OUTPUT(SPI_EEPROM2_CS); - SET_OUTPUT(SPI_FLASH_CS); - WRITE(DAC0_SYNC, HIGH); - WRITE(SPI_EEPROM1_CS, HIGH); - WRITE(SPI_EEPROM2_CS, HIGH); - WRITE(SPI_FLASH_CS, HIGH); + WRITE(DAC0_SYNC_PIN, HIGH); + OUT_WRITE(SPI_EEPROM1_CS_PIN, HIGH); + OUT_WRITE(SPI_EEPROM2_CS_PIN, HIGH); + OUT_WRITE(SPI_FLASH_CS_PIN, HIGH); WRITE(SD_SS_PIN, HIGH); OUT_WRITE(SDSS, LOW); @@ -645,7 +641,7 @@ } // Read from SPI into buffer - void spiRead(uint8_t* buf, uint16_t nbyte) { + void spiRead(uint8_t *buf, uint16_t nbyte) { if (!nbyte) return; --nbyte; for (int i = 0; i < nbyte; i++) { @@ -668,7 +664,7 @@ //DELAY_US(1U); } - void spiSend(const uint8_t* buf, size_t nbyte) { + void spiSend(const uint8_t *buf, size_t nbyte) { if (!nbyte) return; --nbyte; for (size_t i = 0; i < nbyte; i++) { @@ -689,7 +685,7 @@ FLUSH_RX(); } - void spiSend(uint32_t chan, const uint8_t* buf, size_t nbyte) { + void spiSend(uint32_t chan, const uint8_t *buf, size_t nbyte) { if (!nbyte) return; --nbyte; for (size_t i = 0; i < nbyte; i++) { @@ -702,7 +698,7 @@ } // Write from buffer to SPI - void spiSendBlock(uint8_t token, const uint8_t* buf) { + void spiSendBlock(uint8_t token, const uint8_t *buf) { SPI0->SPI_TDR = (uint32_t)token | SPI_PCS(SPI_CHAN); WHILE_TX(0); //WHILE_RX(0); @@ -801,19 +797,19 @@ uint8_t spiRec() { return (uint8_t)spiTransfer(0xFF); } - void spiRead(uint8_t* buf, uint16_t nbyte) { + void spiRead(uint8_t *buf, uint16_t nbyte) { for (int i = 0; i < nbyte; i++) buf[i] = spiTransfer(0xFF); } void spiSend(uint8_t data) { spiTransfer(data); } - void spiSend(const uint8_t* buf, size_t nbyte) { + void spiSend(const uint8_t *buf, size_t nbyte) { for (uint16_t i = 0; i < nbyte; i++) spiTransfer(buf[i]); } - void spiSendBlock(uint8_t token, const uint8_t* buf) { + void spiSendBlock(uint8_t token, const uint8_t *buf) { spiTransfer(token); for (uint16_t i = 0; i < 512; i++) spiTransfer(buf[i]); diff --git a/Marlin/src/HAL/DUE/MarlinSPI.h b/Marlin/src/HAL/DUE/MarlinSPI.h new file mode 100644 index 000000000000..0c447ba4cb3d --- /dev/null +++ b/Marlin/src/HAL/DUE/MarlinSPI.h @@ -0,0 +1,26 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2021 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + * + */ +#pragma once + +#include + +using MarlinSPI = SPIClass; diff --git a/Marlin/src/HAL/DUE/MarlinSerial.cpp b/Marlin/src/HAL/DUE/MarlinSerial.cpp index 50b84c0b1d40..638f7a100722 100644 --- a/Marlin/src/HAL/DUE/MarlinSerial.cpp +++ b/Marlin/src/HAL/DUE/MarlinSerial.cpp @@ -406,7 +406,7 @@ size_t MarlinSerial::write(const uint8_t c) { const uint8_t i = (tx_buffer.head + 1) & (Cfg::TX_SIZE - 1); // If global interrupts are disabled (as the result of being called from an ISR)... - if (!ISRS_ENABLED()) { + if (!hal.isr_state()) { // Make room by polling if it is possible to transmit, and do so! while (i == tx_buffer.tail) { @@ -454,7 +454,7 @@ void MarlinSerial::flushTX() { if (!_written) return; // If global interrupts are disabled (as the result of being called from an ISR)... - if (!ISRS_ENABLED()) { + if (!hal.isr_state()) { // Wait until everything was transmitted - We must do polling, as interrupts are disabled while (tx_buffer.head != tx_buffer.tail || !(HWUART->UART_SR & UART_SR_TXEMPTY)) { @@ -476,9 +476,9 @@ void MarlinSerial::flushTX() { // If not using the USB port as serial port -#if SERIAL_PORT >= 0 +#if defined(SERIAL_PORT) && SERIAL_PORT >= 0 template class MarlinSerial< MarlinSerialCfg >; - MSerialT customizedSerial1(MarlinSerialCfg::EMERGENCYPARSER); + MSerialT1 customizedSerial1(MarlinSerialCfg::EMERGENCYPARSER); #endif #if defined(SERIAL_PORT_2) && SERIAL_PORT_2 >= 0 @@ -486,4 +486,9 @@ void MarlinSerial::flushTX() { MSerialT2 customizedSerial2(MarlinSerialCfg::EMERGENCYPARSER); #endif +#if defined(SERIAL_PORT_3) && SERIAL_PORT_3 >= 0 + template class MarlinSerial< MarlinSerialCfg >; + MSerialT3 customizedSerial3(MarlinSerialCfg::EMERGENCYPARSER); +#endif + #endif // ARDUINO_ARCH_SAM diff --git a/Marlin/src/HAL/DUE/MarlinSerial.h b/Marlin/src/HAL/DUE/MarlinSerial.h index 7fc21264bba9..5a61bffee0da 100644 --- a/Marlin/src/HAL/DUE/MarlinSerial.h +++ b/Marlin/src/HAL/DUE/MarlinSerial.h @@ -118,7 +118,7 @@ class MarlinSerial { static size_t write(const uint8_t c); static void flushTX(); - static inline bool emergency_parser_enabled() { return Cfg::EMERGENCYPARSER; } + static bool emergency_parser_enabled() { return Cfg::EMERGENCYPARSER; } FORCE_INLINE static uint8_t dropped() { return Cfg::DROPPED_RX ? rx_dropped_bytes : 0; } FORCE_INLINE static uint8_t buffer_overruns() { return Cfg::RX_OVERRUNS ? rx_buffer_overruns : 0; } @@ -140,12 +140,17 @@ struct MarlinSerialCfg { static constexpr bool MAX_RX_QUEUED = ENABLED(SERIAL_STATS_MAX_RX_QUEUED); }; -#if SERIAL_PORT >= 0 - typedef Serial0Type< MarlinSerial< MarlinSerialCfg > > MSerialT; - extern MSerialT customizedSerial1; +#if defined(SERIAL_PORT) && SERIAL_PORT >= 0 + typedef Serial1Class< MarlinSerial< MarlinSerialCfg > > MSerialT1; + extern MSerialT1 customizedSerial1; #endif #if defined(SERIAL_PORT_2) && SERIAL_PORT_2 >= 0 - typedef Serial0Type< MarlinSerial< MarlinSerialCfg > > MSerialT2; + typedef Serial1Class< MarlinSerial< MarlinSerialCfg > > MSerialT2; extern MSerialT2 customizedSerial2; #endif + +#if defined(SERIAL_PORT_3) && SERIAL_PORT_3 >= 0 + typedef Serial1Class< MarlinSerial< MarlinSerialCfg > > MSerialT3; + extern MSerialT3 customizedSerial3; +#endif diff --git a/Marlin/src/HAL/DUE/MarlinSerialUSB.cpp b/Marlin/src/HAL/DUE/MarlinSerialUSB.cpp index fca677c7981e..8de2dc7924a0 100644 --- a/Marlin/src/HAL/DUE/MarlinSerialUSB.cpp +++ b/Marlin/src/HAL/DUE/MarlinSerialUSB.cpp @@ -19,13 +19,13 @@ * along with this program. If not, see . * */ +#ifdef ARDUINO_ARCH_SAM /** * MarlinSerial_Due.cpp - Hardware serial library for Arduino DUE * Copyright (c) 2017 Eduardo José Tagle. All right reserved * Based on MarlinSerial for AVR, copyright (c) 2006 Nicholas Zambetti. All right reserved. */ -#ifdef ARDUINO_ARCH_SAM #include "../../inc/MarlinConfig.h" @@ -41,7 +41,7 @@ extern "C" { int udi_cdc_getc(); bool udi_cdc_is_tx_ready(); int udi_cdc_putc(int value); -}; +} // Pending character static int pending_char = -1; @@ -65,7 +65,7 @@ int MarlinSerialUSB::peek() { pending_char = udi_cdc_getc(); - TERN_(EMERGENCY_PARSER, emergency_parser.update(static_cast(this)->emergency_state, (char)pending_char)); + TERN_(EMERGENCY_PARSER, emergency_parser.update(static_cast(this)->emergency_state, (char)pending_char)); return pending_char; } @@ -87,7 +87,7 @@ int MarlinSerialUSB::read() { int c = udi_cdc_getc(); - TERN_(EMERGENCY_PARSER, emergency_parser.update(static_cast(this)->emergency_state, (char)c)); + TERN_(EMERGENCY_PARSER, emergency_parser.update(static_cast(this)->emergency_state, (char)c)); return c; } @@ -129,10 +129,13 @@ size_t MarlinSerialUSB::write(const uint8_t c) { // Preinstantiate #if SERIAL_PORT == -1 - MSerialT customizedSerial1(TERN0(EMERGENCY_PARSER, true)); + MSerialT1 customizedSerial1(TERN0(EMERGENCY_PARSER, true)); #endif #if SERIAL_PORT_2 == -1 - MSerialT customizedSerial2(TERN0(EMERGENCY_PARSER, true)); + MSerialT2 customizedSerial2(TERN0(EMERGENCY_PARSER, true)); +#endif +#if SERIAL_PORT_3 == -1 + MSerialT3 customizedSerial3(TERN0(EMERGENCY_PARSER, true)); #endif #endif // HAS_USB_SERIAL diff --git a/Marlin/src/HAL/DUE/MarlinSerialUSB.h b/Marlin/src/HAL/DUE/MarlinSerialUSB.h index f9cea2986960..6da1ef8c08f6 100644 --- a/Marlin/src/HAL/DUE/MarlinSerialUSB.h +++ b/Marlin/src/HAL/DUE/MarlinSerialUSB.h @@ -27,11 +27,9 @@ */ #include "../../inc/MarlinConfig.h" -#if HAS_USB_SERIAL - -#include #include "../../core/serial_hook.h" +#include struct MarlinSerialUSB { void begin(const long); @@ -50,14 +48,18 @@ struct MarlinSerialUSB { FORCE_INLINE int rxMaxEnqueued() { return 0; } #endif }; -typedef Serial0Type MSerialT; #if SERIAL_PORT == -1 - extern MSerialT customizedSerial1; + typedef Serial1Class MSerialT1; + extern MSerialT1 customizedSerial1; #endif #if SERIAL_PORT_2 == -1 - extern MSerialT customizedSerial2; + typedef Serial1Class MSerialT2; + extern MSerialT2 customizedSerial2; #endif -#endif // HAS_USB_SERIAL +#if SERIAL_PORT_3 == -1 + typedef Serial1Class MSerialT3; + extern MSerialT3 customizedSerial3; +#endif diff --git a/Marlin/src/HAL/DUE/Tone.cpp b/Marlin/src/HAL/DUE/Tone.cpp index 9beb6022237f..4bc8142aba27 100644 --- a/Marlin/src/HAL/DUE/Tone.cpp +++ b/Marlin/src/HAL/DUE/Tone.cpp @@ -35,20 +35,20 @@ static pin_t tone_pin; volatile static int32_t toggles; -void tone(const pin_t _pin, const unsigned int frequency, const unsigned long duration) { +void tone(const pin_t _pin, const unsigned int frequency, const unsigned long duration/*=0*/) { tone_pin = _pin; toggles = 2 * frequency * duration / 1000; - HAL_timer_start(TONE_TIMER_NUM, 2 * frequency); + HAL_timer_start(MF_TIMER_TONE, 2 * frequency); } void noTone(const pin_t _pin) { - HAL_timer_disable_interrupt(TONE_TIMER_NUM); + HAL_timer_disable_interrupt(MF_TIMER_TONE); extDigitalWrite(_pin, LOW); } HAL_TONE_TIMER_ISR() { static uint8_t pin_state = 0; - HAL_timer_isr_prologue(TONE_TIMER_NUM); + HAL_timer_isr_prologue(MF_TIMER_TONE); if (toggles) { toggles--; diff --git a/Marlin/src/HAL/DUE/dogm/u8g_com_HAL_DUE_shared_hw_spi.cpp b/Marlin/src/HAL/DUE/dogm/u8g_com_HAL_DUE_shared_hw_spi.cpp index d07da15ad80e..68f6a5c1a7cb 100644 --- a/Marlin/src/HAL/DUE/dogm/u8g_com_HAL_DUE_shared_hw_spi.cpp +++ b/Marlin/src/HAL/DUE/dogm/u8g_com_HAL_DUE_shared_hw_spi.cpp @@ -20,7 +20,6 @@ * */ - /** * Based on u8g_com_msp430_hw_spi.c * @@ -60,7 +59,7 @@ #if HAS_MARLINUI_U8GLIB -#include +#include #include "../../../MarlinCore.h" diff --git a/Marlin/src/HAL/DUE/dogm/u8g_com_HAL_DUE_st7920_sw_spi.cpp b/Marlin/src/HAL/DUE/dogm/u8g_com_HAL_DUE_st7920_sw_spi.cpp index d01cd4dd6b27..8268cf307e08 100644 --- a/Marlin/src/HAL/DUE/dogm/u8g_com_HAL_DUE_st7920_sw_spi.cpp +++ b/Marlin/src/HAL/DUE/dogm/u8g_com_HAL_DUE_st7920_sw_spi.cpp @@ -57,12 +57,12 @@ #include "../../../inc/MarlinConfigPre.h" -#if ENABLED(U8GLIB_ST7920) +#if IS_U8GLIB_ST7920 #include "../../../inc/MarlinConfig.h" #include "../../shared/Delay.h" -#include +#include #include "u8g_com_HAL_DUE_sw_spi_shared.h" @@ -182,5 +182,5 @@ uint8_t u8g_com_HAL_DUE_ST7920_sw_spi_fn(u8g_t *u8g, uint8_t msg, uint8_t arg_va } #endif // LIGHTWEIGHT_UI -#endif // U8GLIB_ST7920 +#endif // IS_U8GLIB_ST7920 #endif // ARDUINO_ARCH_SAM diff --git a/Marlin/src/HAL/DUE/dogm/u8g_com_HAL_DUE_sw_spi.cpp b/Marlin/src/HAL/DUE/dogm/u8g_com_HAL_DUE_sw_spi.cpp index 890546af58bb..68e3e74a45a0 100644 --- a/Marlin/src/HAL/DUE/dogm/u8g_com_HAL_DUE_sw_spi.cpp +++ b/Marlin/src/HAL/DUE/dogm/u8g_com_HAL_DUE_sw_spi.cpp @@ -57,14 +57,14 @@ #include "../../../inc/MarlinConfigPre.h" -#if HAS_MARLINUI_U8GLIB && DISABLED(U8GLIB_ST7920) +#if HAS_MARLINUI_U8GLIB && !IS_U8GLIB_ST7920 #include "u8g_com_HAL_DUE_sw_spi_shared.h" #include "../../shared/Marduino.h" #include "../../shared/Delay.h" -#include +#include #if ENABLED(FYSETC_MINI_12864) #define SPISEND_SW_DUE u8g_spiSend_sw_DUE_mode_3 @@ -141,5 +141,5 @@ uint8_t u8g_com_HAL_DUE_sw_spi_fn(u8g_t *u8g, uint8_t msg, uint8_t arg_val, void return 1; } -#endif // HAS_MARLINUI_U8GLIB && !U8GLIB_ST7920 +#endif // HAS_MARLINUI_U8GLIB && !IS_U8GLIB_ST7920 #endif // ARDUINO_ARCH_SAM diff --git a/Marlin/src/HAL/DUE/dogm/u8g_com_HAL_DUE_sw_spi_shared.cpp b/Marlin/src/HAL/DUE/dogm/u8g_com_HAL_DUE_sw_spi_shared.cpp index 4fb7a6e2c315..904924793b4c 100644 --- a/Marlin/src/HAL/DUE/dogm/u8g_com_HAL_DUE_sw_spi_shared.cpp +++ b/Marlin/src/HAL/DUE/dogm/u8g_com_HAL_DUE_sw_spi_shared.cpp @@ -62,7 +62,7 @@ #include "../../../inc/MarlinConfig.h" #include "../../shared/Delay.h" -#include +#include #include "u8g_com_HAL_DUE_sw_spi_shared.h" diff --git a/Marlin/src/HAL/DUE/dogm/u8g_com_HAL_DUE_sw_spi_shared.h b/Marlin/src/HAL/DUE/dogm/u8g_com_HAL_DUE_sw_spi_shared.h index f076c503cac2..45231fd091eb 100644 --- a/Marlin/src/HAL/DUE/dogm/u8g_com_HAL_DUE_sw_spi_shared.h +++ b/Marlin/src/HAL/DUE/dogm/u8g_com_HAL_DUE_sw_spi_shared.h @@ -23,7 +23,7 @@ #include "../../../inc/MarlinConfigPre.h" #include "../../shared/Marduino.h" -#include +#include void u8g_SetPIOutput_DUE(u8g_t *u8g, uint8_t pin_index); void u8g_SetPILevel_DUE(u8g_t *u8g, uint8_t pin_index, uint8_t level); diff --git a/Marlin/src/HAL/DUE/eeprom_flash.cpp b/Marlin/src/HAL/DUE/eeprom_flash.cpp index 209a5161ae02..7ce4a84df531 100644 --- a/Marlin/src/HAL/DUE/eeprom_flash.cpp +++ b/Marlin/src/HAL/DUE/eeprom_flash.cpp @@ -135,11 +135,11 @@ static uint8_t buffer[256] = {0}, // The RAM buffer to accumulate writes #define DEBUG_OUT ENABLED(EE_EMU_DEBUG) #include "../../core/debug_out.h" -static void ee_Dump(const int page, const void* data) { +static void ee_Dump(const int page, const void *data) { #ifdef EE_EMU_DEBUG - const uint8_t* c = (const uint8_t*) data; + const uint8_t *c = (const uint8_t*) data; char buffer[80]; sprintf_P(buffer, PSTR("Page: %d (0x%04x)\n"), page, page); @@ -181,7 +181,7 @@ static void ee_Dump(const int page, const void* data) { * @param data (pointer to the data buffer) */ __attribute__ ((long_call, section (".ramfunc"))) -static bool ee_PageWrite(uint16_t page, const void* data) { +static bool ee_PageWrite(uint16_t page, const void *data) { uint16_t i; uint32_t addrflash = uint32_t(getFlashStorage(page)); @@ -200,9 +200,9 @@ static bool ee_PageWrite(uint16_t page, const void* data) { pageContents[i] = (((uint32_t*)data)[i]) | (~(pageContents[i] ^ ((uint32_t*)data)[i])); DEBUG_ECHO_START(); - DEBUG_ECHOLNPAIR("EEPROM PageWrite ", page); - DEBUG_ECHOLNPAIR(" in FLASH address ", (uint32_t)addrflash); - DEBUG_ECHOLNPAIR(" base address ", (uint32_t)getFlashStorage(0)); + DEBUG_ECHOLNPGM("EEPROM PageWrite ", page); + DEBUG_ECHOLNPGM(" in FLASH address ", (uint32_t)addrflash); + DEBUG_ECHOLNPGM(" base address ", (uint32_t)getFlashStorage(0)); DEBUG_FLUSH(); // Get the page relative to the start of the EFC controller, and the EFC controller to use @@ -246,7 +246,7 @@ static bool ee_PageWrite(uint16_t page, const void* data) { __enable_irq(); DEBUG_ECHO_START(); - DEBUG_ECHOLNPAIR("EEPROM Unlock failure for page ", page); + DEBUG_ECHOLNPGM("EEPROM Unlock failure for page ", page); return false; } @@ -271,7 +271,7 @@ static bool ee_PageWrite(uint16_t page, const void* data) { __enable_irq(); DEBUG_ECHO_START(); - DEBUG_ECHOLNPAIR("EEPROM Write failure for page ", page); + DEBUG_ECHOLNPGM("EEPROM Write failure for page ", page); return false; } @@ -287,14 +287,14 @@ static bool ee_PageWrite(uint16_t page, const void* data) { #ifdef EE_EMU_DEBUG DEBUG_ECHO_START(); - DEBUG_ECHOLNPAIR("EEPROM Verify Write failure for page ", page); + DEBUG_ECHOLNPGM("EEPROM Verify Write failure for page ", page); ee_Dump( page, (uint32_t *)addrflash); ee_Dump(-page, data); // Calculate count of changed bits - uint32_t* p1 = (uint32_t*)addrflash; - uint32_t* p2 = (uint32_t*)data; + uint32_t *p1 = (uint32_t*)addrflash; + uint32_t *p2 = (uint32_t*)data; int count = 0; for (i =0; i> 2; i++) { if (p1[i] != p2[i]) { @@ -306,7 +306,7 @@ static bool ee_PageWrite(uint16_t page, const void* data) { } } } - DEBUG_ECHOLNPAIR("--> Differing bits: ", count); + DEBUG_ECHOLNPGM("--> Differing bits: ", count); #endif return false; @@ -326,9 +326,9 @@ static bool ee_PageErase(uint16_t page) { uint32_t addrflash = uint32_t(getFlashStorage(page)); DEBUG_ECHO_START(); - DEBUG_ECHOLNPAIR("EEPROM PageErase ", page); - DEBUG_ECHOLNPAIR(" in FLASH address ", (uint32_t)addrflash); - DEBUG_ECHOLNPAIR(" base address ", (uint32_t)getFlashStorage(0)); + DEBUG_ECHOLNPGM("EEPROM PageErase ", page); + DEBUG_ECHOLNPGM(" in FLASH address ", (uint32_t)addrflash); + DEBUG_ECHOLNPGM(" base address ", (uint32_t)getFlashStorage(0)); DEBUG_FLUSH(); // Get the page relative to the start of the EFC controller, and the EFC controller to use @@ -371,7 +371,7 @@ static bool ee_PageErase(uint16_t page) { __enable_irq(); DEBUG_ECHO_START(); - DEBUG_ECHOLNPAIR("EEPROM Unlock failure for page ",page); + DEBUG_ECHOLNPGM("EEPROM Unlock failure for page ",page); return false; } @@ -395,7 +395,7 @@ static bool ee_PageErase(uint16_t page) { __enable_irq(); DEBUG_ECHO_START(); - DEBUG_ECHOLNPAIR("EEPROM Erase failure for page ",page); + DEBUG_ECHOLNPGM("EEPROM Erase failure for page ",page); return false; } @@ -411,7 +411,7 @@ static bool ee_PageErase(uint16_t page) { for (i = 0; i < PageSize >> 2; i++) { if (*aligned_src++ != 0xFFFFFFFF) { DEBUG_ECHO_START(); - DEBUG_ECHOLNPAIR("EEPROM Verify Erase failure for page ",page); + DEBUG_ECHOLNPGM("EEPROM Verify Erase failure for page ",page); ee_Dump(page, (uint32_t *)addrflash); return false; } @@ -470,7 +470,7 @@ static uint8_t ee_Read(uint32_t address, bool excludeRAMBuffer=false) { for (int page = curPage - 1; page >= 0; --page) { // Get a pointer to the flash page - uint8_t* pflash = (uint8_t*)getFlashStorage(page + curGroup * PagesPerGroup); + uint8_t *pflash = (uint8_t*)getFlashStorage(page + curGroup * PagesPerGroup); uint16_t i = 0; while (i <= (PageSize - 4)) { /* (PageSize - 4) because otherwise, there is not enough room for data and headers */ @@ -550,7 +550,7 @@ static uint32_t ee_GetAddrRange(uint32_t address, bool excludeRAMBuffer=false) { for (int page = curPage - 1; page >= 0; --page) { // Get a pointer to the flash page - uint8_t* pflash = (uint8_t*)getFlashStorage(page + curGroup * PagesPerGroup); + uint8_t *pflash = (uint8_t*)getFlashStorage(page + curGroup * PagesPerGroup); uint16_t i = 0; while (i <= (PageSize - 4)) { /* (PageSize - 4) because otherwise, there is not enough room for data and headers */ @@ -589,7 +589,7 @@ static uint32_t ee_GetAddrRange(uint32_t address, bool excludeRAMBuffer=false) { } static bool ee_IsPageClean(int page) { - uint32_t* pflash = (uint32_t*) getFlashStorage(page); + uint32_t *pflash = (uint32_t*) getFlashStorage(page); for (uint16_t i = 0; i < (PageSize >> 2); ++i) if (*pflash++ != 0xFFFFFFFF) return false; return true; @@ -599,7 +599,7 @@ static bool ee_Flush(uint32_t overrideAddress = 0xFFFFFFFF, uint8_t overrideData // Check if RAM buffer has something to be written bool isEmpty = true; - uint32_t* p = (uint32_t*) &buffer[0]; + uint32_t *p = (uint32_t*) &buffer[0]; for (uint16_t j = 0; j < (PageSize >> 2); j++) { if (*p++ != 0xFFFFFFFF) { isEmpty = false; @@ -922,7 +922,7 @@ static void ee_Init() { if (curGroup >= GroupCount) curGroup = 0; DEBUG_ECHO_START(); - DEBUG_ECHOLNPAIR("EEPROM Current Group: ",curGroup); + DEBUG_ECHOLNPGM("EEPROM Current Group: ",curGroup); DEBUG_FLUSH(); // Now, validate that all the other group pages are empty @@ -932,7 +932,7 @@ static void ee_Init() { for (int page = 0; page < PagesPerGroup; page++) { if (!ee_IsPageClean(grp * PagesPerGroup + page)) { DEBUG_ECHO_START(); - DEBUG_ECHOLNPAIR("EEPROM Page ", page, " not clean on group ", grp); + DEBUG_ECHOLNPGM("EEPROM Page ", page, " not clean on group ", grp); DEBUG_FLUSH(); ee_PageErase(grp * PagesPerGroup + page); } @@ -949,14 +949,14 @@ static void ee_Init() { } DEBUG_ECHO_START(); - DEBUG_ECHOLNPAIR("EEPROM Active page: ", curPage); + DEBUG_ECHOLNPGM("EEPROM Active page: ", curPage); DEBUG_FLUSH(); // Make sure the pages following the first clean one are also clean for (int page = curPage + 1; page < PagesPerGroup; page++) { if (!ee_IsPageClean(curGroup * PagesPerGroup + page)) { DEBUG_ECHO_START(); - DEBUG_ECHOLNPAIR("EEPROM Page ", page, " not clean on active group ", curGroup); + DEBUG_ECHOLNPGM("EEPROM Page ", page, " not clean on active group ", curGroup); DEBUG_FLUSH(); ee_Dump(curGroup * PagesPerGroup + page, getFlashStorage(curGroup * PagesPerGroup + page)); ee_PageErase(curGroup * PagesPerGroup + page); @@ -976,14 +976,13 @@ bool PersistentStore::access_start() { ee_Init(); return true; } bool PersistentStore::access_finish() { ee_Flush(); return true; } bool PersistentStore::write_data(int &pos, const uint8_t *value, size_t size, uint16_t *crc) { + uint16_t written = 0; while (size--) { uint8_t * const p = (uint8_t * const)pos; uint8_t v = *value; - // EEPROM has only ~100,000 write cycles, - // so only write bytes that have changed! - if (v != ee_Read(uint32_t(p))) { + if (v != ee_Read(uint32_t(p))) { // EEPROM has only ~100,000 write cycles, so only write bytes that have changed! ee_Write(uint32_t(p), v); - delay(2); + if (++written & 0x7F) delay(2); else safe_delay(2); // Avoid triggering watchdog during long EEPROM writes if (ee_Read(uint32_t(p)) != v) { SERIAL_ECHO_MSG(STR_ERR_EEPROM_WRITE); return true; diff --git a/Marlin/src/HAL/DUE/eeprom_wired.cpp b/Marlin/src/HAL/DUE/eeprom_wired.cpp index b488c36f1666..557a2f2cffa5 100644 --- a/Marlin/src/HAL/DUE/eeprom_wired.cpp +++ b/Marlin/src/HAL/DUE/eeprom_wired.cpp @@ -42,14 +42,13 @@ bool PersistentStore::access_start() { eeprom_init(); return true; } bool PersistentStore::access_finish() { return true; } bool PersistentStore::write_data(int &pos, const uint8_t *value, size_t size, uint16_t *crc) { + uint16_t written = 0; while (size--) { uint8_t * const p = (uint8_t * const)pos; uint8_t v = *value; - // EEPROM has only ~100,000 write cycles, - // so only write bytes that have changed! - if (v != eeprom_read_byte(p)) { + if (v != eeprom_read_byte(p)) { // EEPROM has only ~100,000 write cycles, so only write bytes that have changed! eeprom_write_byte(p, v); - delay(2); + if (++written & 0x7F) delay(2); else safe_delay(2); // Avoid triggering watchdog during long EEPROM writes if (eeprom_read_byte(p) != v) { SERIAL_ECHO_MSG(STR_ERR_EEPROM_WRITE); return true; diff --git a/Marlin/src/HAL/DUE/endstop_interrupts.h b/Marlin/src/HAL/DUE/endstop_interrupts.h index 999ada512761..9c7e2104882e 100644 --- a/Marlin/src/HAL/DUE/endstop_interrupts.h +++ b/Marlin/src/HAL/DUE/endstop_interrupts.h @@ -64,4 +64,10 @@ void setup_endstop_interrupts() { TERN_(HAS_Z4_MAX, _ATTACH(Z4_MAX_PIN)); TERN_(HAS_Z4_MIN, _ATTACH(Z4_MIN_PIN)); TERN_(HAS_Z_MIN_PROBE_PIN, _ATTACH(Z_MIN_PROBE_PIN)); + TERN_(HAS_I_MAX, _ATTACH(I_MAX_PIN)); + TERN_(HAS_I_MIN, _ATTACH(I_MIN_PIN)); + TERN_(HAS_J_MAX, _ATTACH(J_MAX_PIN)); + TERN_(HAS_J_MIN, _ATTACH(J_MIN_PIN)); + TERN_(HAS_K_MAX, _ATTACH(K_MAX_PIN)); + TERN_(HAS_K_MIN, _ATTACH(K_MIN_PIN)); } diff --git a/Marlin/src/HAL/DUE/fastio.h b/Marlin/src/HAL/DUE/fastio.h index f375cb6b2977..a609210d8130 100644 --- a/Marlin/src/HAL/DUE/fastio.h +++ b/Marlin/src/HAL/DUE/fastio.h @@ -33,7 +33,7 @@ * For ARDUINO_ARCH_SAM * Note the code here was specifically crafted by disassembling what GCC produces * out of it, so GCC is able to optimize it out as much as possible to the least - * amount of instructions. Be very carefull if you modify them, as "clean code" + * amount of instructions. Be very careful if you modify them, as "clean code" * leads to less efficient compiled code!! */ diff --git a/Marlin/src/HAL/DUE/fastio/G2_PWM.cpp b/Marlin/src/HAL/DUE/fastio/G2_PWM.cpp index d9fbabce2148..800915ff692b 100644 --- a/Marlin/src/HAL/DUE/fastio/G2_PWM.cpp +++ b/Marlin/src/HAL/DUE/fastio/G2_PWM.cpp @@ -25,7 +25,7 @@ * is NOT used to directly toggle pins. The ISR writes to the pin assigned to * that interrupt. * - * All PWMs use the same repetition rate. The G2 needs about 10KHz min in order to + * All PWMs use the same repetition rate. The G2 needs about 10kHz min in order to * not have obvious ripple on the Vref signals. * * The data structures are setup to minimize the computation done by the ISR which diff --git a/Marlin/src/HAL/DUE/pinsDebug.h b/Marlin/src/HAL/DUE/pinsDebug.h index a99ca8ecce90..6c08585974fd 100644 --- a/Marlin/src/HAL/DUE/pinsDebug.h +++ b/Marlin/src/HAL/DUE/pinsDebug.h @@ -2,6 +2,9 @@ * Marlin 3D Printer Firmware * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * * This program is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation, either version 3 of the License, or @@ -50,7 +53,7 @@ * The net result is that both the g_pinStatus[pin] array and the PIO_OSR register * needs to be looked at when determining if a pin is an input or an output. * - * b) Due has only pins 6, 7, 8 & 9 enabled for PWMs. FYI - they run at 1KHz + * b) Due has only pins 6, 7, 8 & 9 enabled for PWMs. FYI - they run at 1kHz * * c) NUM_DIGITAL_PINS does not include the analog pins * @@ -64,6 +67,7 @@ #define PRINT_PORT(p) #define PRINT_ARRAY_NAME(x) do{ sprintf_P(buffer, PSTR("%-" STRINGIFY(MAX_NAME_LENGTH) "s"), pin_array[x].name); SERIAL_ECHO(buffer); }while(0) #define PRINT_PIN(p) do{ sprintf_P(buffer, PSTR("%02d"), p); SERIAL_ECHO(buffer); }while(0) +#define PRINT_PIN_ANALOG(p) do{ sprintf_P(buffer, PSTR(" (A%2d) "), DIGITAL_PIN_TO_ANALOG_PIN(pin)); SERIAL_ECHO(buffer); }while(0) #define GET_ARRAY_PIN(p) pin_array[p].pin #define GET_ARRAY_IS_DIGITAL(p) pin_array[p].is_digital #define VALID_PIN(pin) (pin >= 0 && pin < (int8_t)NUMBER_PINS_TOTAL ? 1 : 0) @@ -86,7 +90,7 @@ bool GET_PINMODE(int8_t pin) { // 1: output, 0: input void pwm_details(int32_t pin) { if (pwm_status(pin)) { uint32_t chan = g_APinDescription[pin].ulPWMChannel; - SERIAL_ECHOPAIR("PWM = ", PWM_INTERFACE->PWM_CH_NUM[chan].PWM_CDTY); + SERIAL_ECHOPGM("PWM = ", PWM_INTERFACE->PWM_CH_NUM[chan].PWM_CDTY); } } diff --git a/Marlin/src/HAL/DUE/timers.cpp b/Marlin/src/HAL/DUE/timers.cpp index 65073c510d7c..a7bf7fbd6d85 100644 --- a/Marlin/src/HAL/DUE/timers.cpp +++ b/Marlin/src/HAL/DUE/timers.cpp @@ -42,7 +42,7 @@ // Private Variables // ------------------------ -const tTimerConfig TimerConfig [NUM_HARDWARE_TIMERS] = { +const tTimerConfig timer_config[NUM_HARDWARE_TIMERS] = { { TC0, 0, TC0_IRQn, 3}, // 0 - [servo timer5] { TC0, 1, TC1_IRQn, 0}, // 1 { TC0, 2, TC2_IRQn, 2}, // 2 - stepper @@ -66,9 +66,9 @@ const tTimerConfig TimerConfig [NUM_HARDWARE_TIMERS] = { */ void HAL_timer_start(const uint8_t timer_num, const uint32_t frequency) { - Tc *tc = TimerConfig[timer_num].pTimerRegs; - IRQn_Type irq = TimerConfig[timer_num].IRQ_Id; - uint32_t channel = TimerConfig[timer_num].channel; + Tc *tc = timer_config[timer_num].pTimerRegs; + IRQn_Type irq = timer_config[timer_num].IRQ_Id; + uint32_t channel = timer_config[timer_num].channel; // Disable interrupt, just in case it was already enabled NVIC_DisableIRQ(irq); @@ -86,7 +86,7 @@ void HAL_timer_start(const uint8_t timer_num, const uint32_t frequency) { pmc_set_writeprotect(false); pmc_enable_periph_clk((uint32_t)irq); - NVIC_SetPriority(irq, TimerConfig [timer_num].priority); + NVIC_SetPriority(irq, timer_config[timer_num].priority); // wave mode, reset counter on match with RC, TC_Configure(tc, channel, TC_CMR_WAVE | TC_CMR_WAVSEL_UP_RC | TC_CMR_TCCLKS_TIMER_CLOCK1); @@ -105,12 +105,12 @@ void HAL_timer_start(const uint8_t timer_num, const uint32_t frequency) { } void HAL_timer_enable_interrupt(const uint8_t timer_num) { - IRQn_Type irq = TimerConfig[timer_num].IRQ_Id; + IRQn_Type irq = timer_config[timer_num].IRQ_Id; NVIC_EnableIRQ(irq); } void HAL_timer_disable_interrupt(const uint8_t timer_num) { - IRQn_Type irq = TimerConfig[timer_num].IRQ_Id; + IRQn_Type irq = timer_config[timer_num].IRQ_Id; NVIC_DisableIRQ(irq); // We NEED memory barriers to ensure Interrupts are actually disabled! @@ -125,7 +125,7 @@ static bool NVIC_GetEnabledIRQ(IRQn_Type IRQn) { } bool HAL_timer_interrupt_enabled(const uint8_t timer_num) { - IRQn_Type irq = TimerConfig[timer_num].IRQ_Id; + IRQn_Type irq = timer_config[timer_num].IRQ_Id; return NVIC_GetEnabledIRQ(irq); } diff --git a/Marlin/src/HAL/DUE/timers.h b/Marlin/src/HAL/DUE/timers.h index 0e1ea07cc2e4..bcfd07e268c5 100644 --- a/Marlin/src/HAL/DUE/timers.h +++ b/Marlin/src/HAL/DUE/timers.h @@ -37,35 +37,35 @@ typedef uint32_t hal_timer_t; #define HAL_TIMER_RATE ((F_CPU) / 2) // frequency of timers peripherals -#ifndef STEP_TIMER_NUM - #define STEP_TIMER_NUM 2 // Timer Index for Stepper +#ifndef MF_TIMER_STEP + #define MF_TIMER_STEP 2 // Timer Index for Stepper #endif -#ifndef PULSE_TIMER_NUM - #define PULSE_TIMER_NUM STEP_TIMER_NUM +#ifndef MF_TIMER_PULSE + #define MF_TIMER_PULSE MF_TIMER_STEP #endif -#ifndef TEMP_TIMER_NUM - #define TEMP_TIMER_NUM 4 // Timer Index for Temperature +#ifndef MF_TIMER_TEMP + #define MF_TIMER_TEMP 4 // Timer Index for Temperature #endif -#ifndef TONE_TIMER_NUM - #define TONE_TIMER_NUM 6 // index of timer to use for beeper tones +#ifndef MF_TIMER_TONE + #define MF_TIMER_TONE 6 // index of timer to use for beeper tones #endif #define TEMP_TIMER_FREQUENCY 1000 // temperature interrupt frequency -#define STEPPER_TIMER_RATE HAL_TIMER_RATE // frequency of stepper timer (HAL_TIMER_RATE / STEPPER_TIMER_PRESCALE) -#define STEPPER_TIMER_TICKS_PER_US ((STEPPER_TIMER_RATE) / 1000000) // stepper timer ticks per µs -#define STEPPER_TIMER_PRESCALE (CYCLES_PER_MICROSECOND / STEPPER_TIMER_TICKS_PER_US) +#define STEPPER_TIMER_RATE HAL_TIMER_RATE // frequency of stepper timer (HAL_TIMER_RATE / STEPPER_TIMER_PRESCALE) +#define STEPPER_TIMER_TICKS_PER_US ((STEPPER_TIMER_RATE) / 1000000) // stepper timer ticks per µs +#define STEPPER_TIMER_PRESCALE (CYCLES_PER_MICROSECOND / STEPPER_TIMER_TICKS_PER_US) -#define PULSE_TIMER_RATE STEPPER_TIMER_RATE // frequency of pulse timer -#define PULSE_TIMER_PRESCALE STEPPER_TIMER_PRESCALE -#define PULSE_TIMER_TICKS_PER_US STEPPER_TIMER_TICKS_PER_US +#define PULSE_TIMER_RATE STEPPER_TIMER_RATE // frequency of pulse timer +#define PULSE_TIMER_PRESCALE STEPPER_TIMER_PRESCALE +#define PULSE_TIMER_TICKS_PER_US STEPPER_TIMER_TICKS_PER_US -#define ENABLE_STEPPER_DRIVER_INTERRUPT() HAL_timer_enable_interrupt(STEP_TIMER_NUM) -#define DISABLE_STEPPER_DRIVER_INTERRUPT() HAL_timer_disable_interrupt(STEP_TIMER_NUM) -#define STEPPER_ISR_ENABLED() HAL_timer_interrupt_enabled(STEP_TIMER_NUM) +#define ENABLE_STEPPER_DRIVER_INTERRUPT() HAL_timer_enable_interrupt(MF_TIMER_STEP) +#define DISABLE_STEPPER_DRIVER_INTERRUPT() HAL_timer_disable_interrupt(MF_TIMER_STEP) +#define STEPPER_ISR_ENABLED() HAL_timer_interrupt_enabled(MF_TIMER_STEP) -#define ENABLE_TEMPERATURE_INTERRUPT() HAL_timer_enable_interrupt(TEMP_TIMER_NUM) -#define DISABLE_TEMPERATURE_INTERRUPT() HAL_timer_disable_interrupt(TEMP_TIMER_NUM) +#define ENABLE_TEMPERATURE_INTERRUPT() HAL_timer_enable_interrupt(MF_TIMER_TEMP) +#define DISABLE_TEMPERATURE_INTERRUPT() HAL_timer_disable_interrupt(MF_TIMER_TEMP) #ifndef HAL_STEP_TIMER_ISR #define HAL_STEP_TIMER_ISR() void TC2_Handler() @@ -92,7 +92,7 @@ typedef struct { // Public Variables // ------------------------ -extern const tTimerConfig TimerConfig[]; +extern const tTimerConfig timer_config[]; // ------------------------ // Public functions @@ -101,17 +101,17 @@ extern const tTimerConfig TimerConfig[]; void HAL_timer_start(const uint8_t timer_num, const uint32_t frequency); FORCE_INLINE static void HAL_timer_set_compare(const uint8_t timer_num, const hal_timer_t compare) { - const tTimerConfig * const pConfig = &TimerConfig[timer_num]; + const tTimerConfig * const pConfig = &timer_config[timer_num]; pConfig->pTimerRegs->TC_CHANNEL[pConfig->channel].TC_RC = compare; } FORCE_INLINE static hal_timer_t HAL_timer_get_compare(const uint8_t timer_num) { - const tTimerConfig * const pConfig = &TimerConfig[timer_num]; + const tTimerConfig * const pConfig = &timer_config[timer_num]; return pConfig->pTimerRegs->TC_CHANNEL[pConfig->channel].TC_RC; } FORCE_INLINE static hal_timer_t HAL_timer_get_count(const uint8_t timer_num) { - const tTimerConfig * const pConfig = &TimerConfig[timer_num]; + const tTimerConfig * const pConfig = &timer_config[timer_num]; return pConfig->pTimerRegs->TC_CHANNEL[pConfig->channel].TC_CV; } @@ -120,9 +120,9 @@ void HAL_timer_disable_interrupt(const uint8_t timer_num); bool HAL_timer_interrupt_enabled(const uint8_t timer_num); FORCE_INLINE static void HAL_timer_isr_prologue(const uint8_t timer_num) { - const tTimerConfig * const pConfig = &TimerConfig[timer_num]; + const tTimerConfig * const pConfig = &timer_config[timer_num]; // Reading the status register clears the interrupt flag pConfig->pTimerRegs->TC_CHANNEL[pConfig->channel].TC_SR; } -#define HAL_timer_isr_epilogue(TIMER_NUM) +#define HAL_timer_isr_epilogue(T) NOOP diff --git a/Marlin/src/HAL/DUE/upload_extra_script.py b/Marlin/src/HAL/DUE/upload_extra_script.py index d52a0a3642b4..4f7a494512b2 100644 --- a/Marlin/src/HAL/DUE/upload_extra_script.py +++ b/Marlin/src/HAL/DUE/upload_extra_script.py @@ -4,15 +4,16 @@ # Windows: bossac.exe # Other: leave unchanged # +import pioutil +if pioutil.is_pio_build(): + import platform + current_OS = platform.system() -import platform -current_OS = platform.system() + if current_OS == 'Windows': -if current_OS == 'Windows': + Import("env") - Import("env") - - # Use bossac.exe on Windows - env.Replace( - UPLOADCMD="bossac --info --unlock --write --verify --reset --erase -U false --boot $SOURCE" - ) + # Use bossac.exe on Windows + env.Replace( + UPLOADCMD="bossac --info --unlock --write --verify --reset --erase -U false --boot $SOURCE" + ) diff --git a/Marlin/src/HAL/DUE/usb/arduino_due_x.h b/Marlin/src/HAL/DUE/usb/arduino_due_x.h index d3b333fb349a..e7b6f3dcb303 100644 --- a/Marlin/src/HAL/DUE/usb/arduino_due_x.h +++ b/Marlin/src/HAL/DUE/usb/arduino_due_x.h @@ -71,7 +71,7 @@ /* ------------------------------------------------------------------------ */ /** - * \page arduino_due_x_board_info "Arduino Due/X - Board informations" + * \page arduino_due_x_board_info "Arduino Due/X - Board information" * This page lists several definition related to the board description. * */ diff --git a/Marlin/src/HAL/DUE/usb/sd_mmc_spi_mem.cpp b/Marlin/src/HAL/DUE/usb/sd_mmc_spi_mem.cpp index d92d332c1efd..34cc256b30ff 100644 --- a/Marlin/src/HAL/DUE/usb/sd_mmc_spi_mem.cpp +++ b/Marlin/src/HAL/DUE/usb/sd_mmc_spi_mem.cpp @@ -10,7 +10,7 @@ #include "../../../sd/cardreader.h" extern "C" { -#include "sd_mmc_spi_mem.h" + #include "sd_mmc_spi_mem.h" } #define SD_MMC_BLOCK_SIZE 512 @@ -32,7 +32,7 @@ Ctrl_status sd_mmc_spi_test_unit_ready() { Ctrl_status sd_mmc_spi_read_capacity(uint32_t *nb_sector) { if (!IS_SD_INSERTED() || IS_SD_PRINTING() || IS_SD_FILE_OPEN() || !card.isMounted()) return CTRL_NO_PRESENT; - *nb_sector = card.getSd2Card().cardSize() - 1; + *nb_sector = card.diskIODriver()->cardSize() - 1; return CTRL_GOOD; } @@ -74,24 +74,24 @@ Ctrl_status sd_mmc_spi_usb_read_10(uint32_t addr, uint16_t nb_sector) { #endif // Start reading - if (!card.getSd2Card().readStart(addr)) + if (!card.diskIODriver()->readStart(addr)) return CTRL_FAIL; // For each specified sector while (nb_sector--) { // Read a sector - card.getSd2Card().readData(sector_buf); + card.diskIODriver()->readData(sector_buf); // RAM -> USB if (!udi_msc_trans_block(true, sector_buf, SD_MMC_BLOCK_SIZE, nullptr)) { - card.getSd2Card().readStop(); + card.diskIODriver()->readStop(); return CTRL_FAIL; } } // Stop reading - card.getSd2Card().readStop(); + card.diskIODriver()->readStop(); // Done return CTRL_GOOD; @@ -113,7 +113,7 @@ Ctrl_status sd_mmc_spi_usb_write_10(uint32_t addr, uint16_t nb_sector) { } #endif - if (!card.getSd2Card().writeStart(addr, nb_sector)) + if (!card.diskIODriver()->writeStart(addr, nb_sector)) return CTRL_FAIL; // For each specified sector @@ -121,16 +121,16 @@ Ctrl_status sd_mmc_spi_usb_write_10(uint32_t addr, uint16_t nb_sector) { // USB -> RAM if (!udi_msc_trans_block(false, sector_buf, SD_MMC_BLOCK_SIZE, nullptr)) { - card.getSd2Card().writeStop(); + card.diskIODriver()->writeStop(); return CTRL_FAIL; } // Write a sector - card.getSd2Card().writeData(sector_buf); + card.diskIODriver()->writeData(sector_buf); } // Stop writing - card.getSd2Card().writeStop(); + card.diskIODriver()->writeStop(); // Done return CTRL_GOOD; diff --git a/Marlin/src/HAL/DUE/usb/sd_mmc_spi_mem.h b/Marlin/src/HAL/DUE/usb/sd_mmc_spi_mem.h index d77e4f95232b..553fd3c29a88 100644 --- a/Marlin/src/HAL/DUE/usb/sd_mmc_spi_mem.h +++ b/Marlin/src/HAL/DUE/usb/sd_mmc_spi_mem.h @@ -74,7 +74,7 @@ #define SD_MMC_REMOVING 2 -//---- CONTROL FONCTIONS ---- +//---- CONTROL FUNCTIONS ---- //! //! @brief This function initializes the hw/sw resources required to drive the SD_MMC_SPI. //!/ @@ -134,7 +134,7 @@ extern bool sd_mmc_spi_wr_protect(void); extern bool sd_mmc_spi_removal(void); -//---- ACCESS DATA FONCTIONS ---- +//---- ACCESS DATA FUNCTIONS ---- #if ACCESS_USB == true // Standard functions for open in read/write mode the device diff --git a/Marlin/src/HAL/DUE/usb/udd.h b/Marlin/src/HAL/DUE/usb/udd.h index 7ec8c03dee63..319d8842f744 100644 --- a/Marlin/src/HAL/DUE/usb/udd.h +++ b/Marlin/src/HAL/DUE/usb/udd.h @@ -90,7 +90,7 @@ typedef struct { //! This buffer must be word align for DATA IN phase (use prefix COMPILER_WORD_ALIGNED for buffer) uint8_t *payload; - //! Size of buffer to send or fill, and content the number of byte transfered + //! Size of buffer to send or fill, and content the number of byte transferred uint16_t payload_size; //! Callback called after reception of ZLP from setup request @@ -132,10 +132,10 @@ typedef void (*udd_callback_halt_cleared_t)(void); * * \param status UDD_EP_TRANSFER_OK, if transfer is complete * \param status UDD_EP_TRANSFER_ABORT, if transfer is aborted - * \param n number of data transfered + * \param n number of data transferred */ typedef void (*udd_callback_trans_t) (udd_ep_status_t status, - iram_size_t nb_transfered, udd_ep_id_t ep); + iram_size_t nb_transferred, udd_ep_id_t ep); /** * \brief Authorizes the VBUS event @@ -303,7 +303,7 @@ bool udd_ep_wait_stall_clear(udd_ep_id_t ep, * The driver uses a specific DMA USB to transfer data * from internal RAM to endpoint, if this one is available. * When the transfer is finished or aborted (stall, reset, ...), the \a callback is called. - * The \a callback returns the transfer status and eventually the number of byte transfered. + * The \a callback returns the transfer status and eventually the number of byte transferred. * Note: The control endpoint is not authorized. * * \param ep The ID of the endpoint to use diff --git a/Marlin/src/HAL/DUE/usb/udi_cdc.c b/Marlin/src/HAL/DUE/usb/udi_cdc.c index cbe23dbb68fb..89debe57f130 100644 --- a/Marlin/src/HAL/DUE/usb/udi_cdc.c +++ b/Marlin/src/HAL/DUE/usb/udi_cdc.c @@ -162,7 +162,7 @@ static void udi_cdc_ctrl_state_notify(uint8_t port, udd_ep_id_t ep); * * \param status UDD_EP_TRANSFER_OK, if transfer finished * \param status UDD_EP_TRANSFER_ABORT, if transfer aborted - * \param n number of data transfered + * \param n number of data transferred */ static void udi_cdc_serial_state_msg_sent(udd_ep_status_t status, iram_size_t n, udd_ep_id_t ep); @@ -200,7 +200,7 @@ static void udi_cdc_data_received(udd_ep_status_t status, iram_size_t n, udd_ep_ * * \param status UDD_EP_TRANSFER_OK, if transfer finished * \param status UDD_EP_TRANSFER_ABORT, if transfer aborted - * \param n number of data transfered + * \param n number of data transferred */ static void udi_cdc_data_sent(udd_ep_status_t status, iram_size_t n, udd_ep_id_t ep); diff --git a/Marlin/src/HAL/DUE/usb/udi_cdc.h b/Marlin/src/HAL/DUE/usb/udi_cdc.h index 0ecf7bb00e5e..b61845011aa2 100644 --- a/Marlin/src/HAL/DUE/usb/udi_cdc.h +++ b/Marlin/src/HAL/DUE/usb/udi_cdc.h @@ -675,11 +675,11 @@ iram_size_t udi_cdc_multi_write_buf(uint8_t port, const void* buf, iram_size_t s * - \code // Waits and gets a value on CDC line int udi_cdc_getc(void); // Reads a RAM buffer on CDC line - iram_size_t udi_cdc_read_buf(int* buf, iram_size_t size); + iram_size_t udi_cdc_read_buf(int *buf, iram_size_t size); // Puts a byte on CDC line int udi_cdc_putc(int value); // Writes a RAM buffer on CDC line - iram_size_t udi_cdc_write_buf(const int* buf, iram_size_t size); \endcode + iram_size_t udi_cdc_write_buf(const int *buf, iram_size_t size); \endcode * * \section udi_cdc_use_cases Advanced use cases * For more advanced use of the UDI CDC module, see the following use cases: diff --git a/Marlin/src/HAL/DUE/usb/udi_cdc_conf.h b/Marlin/src/HAL/DUE/usb/udi_cdc_conf.h index d406a87743f3..e61b8cbaadf4 100644 --- a/Marlin/src/HAL/DUE/usb/udi_cdc_conf.h +++ b/Marlin/src/HAL/DUE/usb/udi_cdc_conf.h @@ -106,7 +106,7 @@ extern "C" { */ //@{ # if UDI_CDC_PORT_NB > 2 -# error USBB, UDP, UDPHS and UOTGHS interfaces have not enought endpoints. +# error USBB, UDP, UDPHS and UOTGHS interfaces have not enough endpoints. # endif #define UDI_CDC_DATA_EP_IN_0 (1 | USB_EP_DIR_IN) // TX #define UDI_CDC_DATA_EP_OUT_0 (2 | USB_EP_DIR_OUT) // RX diff --git a/Marlin/src/HAL/DUE/usb/udi_msc.c b/Marlin/src/HAL/DUE/usb/udi_msc.c index b7c3bb5ea016..dd3404877210 100644 --- a/Marlin/src/HAL/DUE/usb/udi_msc.c +++ b/Marlin/src/HAL/DUE/usb/udi_msc.c @@ -173,7 +173,7 @@ static void udi_msc_cbw_wait(void); * * \param status UDD_EP_TRANSFER_OK, if transfer is finished * \param status UDD_EP_TRANSFER_ABORT, if transfer is aborted - * \param nb_received number of data transfered + * \param nb_received number of data transferred */ static void udi_msc_cbw_received(udd_ep_status_t status, iram_size_t nb_received, udd_ep_id_t ep); @@ -211,7 +211,7 @@ static void udi_msc_data_send(uint8_t * buffer, uint8_t buf_size); * * \param status UDD_EP_TRANSFER_OK, if transfer finish * \param status UDD_EP_TRANSFER_ABORT, if transfer aborted - * \param nb_sent number of data transfered + * \param nb_sent number of data transferred */ static void udi_msc_data_sent(udd_ep_status_t status, iram_size_t nb_sent, udd_ep_id_t ep); @@ -244,7 +244,7 @@ void udi_msc_csw_send(void); * * \param status UDD_EP_TRANSFER_OK, if transfer is finished * \param status UDD_EP_TRANSFER_ABORT, if transfer is aborted - * \param nb_sent number of data transfered + * \param nb_sent number of data transferred */ static void udi_msc_csw_sent(udd_ep_status_t status, iram_size_t nb_sent, udd_ep_id_t ep); @@ -463,7 +463,7 @@ uint8_t udi_msc_getsetting(void) static void udi_msc_cbw_invalid(void) { if (!udi_msc_b_cbw_invalid) - return; // Don't re-stall endpoint if error reseted by setup + return; // Don't re-stall endpoint if error reset by setup udd_ep_set_halt(UDI_MSC_EP_OUT); // If stall cleared then re-stall it. Only Setup MSC Reset can clear it udd_ep_wait_stall_clear(UDI_MSC_EP_OUT, udi_msc_cbw_invalid); @@ -472,7 +472,7 @@ static void udi_msc_cbw_invalid(void) static void udi_msc_csw_invalid(void) { if (!udi_msc_b_cbw_invalid) - return; // Don't re-stall endpoint if error reseted by setup + return; // Don't re-stall endpoint if error reset by setup udd_ep_set_halt(UDI_MSC_EP_IN); // If stall cleared then re-stall it. Only Setup MSC Reset can clear it udd_ep_wait_stall_clear(UDI_MSC_EP_IN, udi_msc_csw_invalid); diff --git a/Marlin/src/HAL/DUE/usb/uotghs_device_due.c b/Marlin/src/HAL/DUE/usb/uotghs_device_due.c index e13232a39c7d..c7e8f8d99135 100644 --- a/Marlin/src/HAL/DUE/usb/uotghs_device_due.c +++ b/Marlin/src/HAL/DUE/usb/uotghs_device_due.c @@ -325,7 +325,7 @@ static void udd_sleep_mode(bool b_idle) /** * \name Control endpoint low level management routine. * - * This function performs control endpoint mangement. + * This function performs control endpoint management. * It handle the SETUP/DATA/HANDSHAKE phases of a control transaction. */ //@{ @@ -397,9 +397,9 @@ static void udd_ctrl_endofrequest(void); /** * \brief Main interrupt routine for control endpoint * - * This switchs control endpoint events to correct sub function. + * This switches control endpoint events to correct sub function. * - * \return \c 1 if an event about control endpoint is occured, otherwise \c 0. + * \return \c 1 if an event about control endpoint is occurred, otherwise \c 0. */ static bool udd_ctrl_interrupt(void); @@ -410,7 +410,7 @@ static bool udd_ctrl_interrupt(void); * \name Management of bulk/interrupt/isochronous endpoints * * The UDD manages the data transfer on endpoints: - * - Start data tranfer on endpoint with USB Device DMA + * - Start data transfer on endpoint with USB Device DMA * - Send a ZLP packet if requested * - Call callback registered to signal end of transfer * The transfer abort and stall feature are supported. @@ -431,7 +431,7 @@ typedef struct { uint8_t *buf; //! Size of buffer to send or fill iram_size_t buf_size; - //!< Size of data transfered + //!< Size of data transferred iram_size_t buf_cnt; //!< Size of data loaded (or prepared for DMA) last time iram_size_t buf_load; @@ -486,7 +486,7 @@ static void udd_ep_finish_job(udd_ep_job_t * ptr_job, bool b_abort, uint8_t ep_n #ifdef UDD_EP_DMA_SUPPORTED /** - * \brief Start the next transfer if necessary or complet the job associated. + * \brief Start the next transfer if necessary or complete the job associated. * * \param ep endpoint number without direction flag */ @@ -496,9 +496,9 @@ static void udd_ep_finish_job(udd_ep_job_t * ptr_job, bool b_abort, uint8_t ep_n /** * \brief Main interrupt routine for bulk/interrupt/isochronous endpoints * - * This switchs endpoint events to correct sub function. + * This switches endpoint events to correct sub function. * - * \return \c 1 if an event about bulk/interrupt/isochronous endpoints has occured, otherwise \c 0. + * \return \c 1 if an event about bulk/interrupt/isochronous endpoints has occurred, otherwise \c 0. */ static bool udd_ep_interrupt(void); @@ -520,7 +520,7 @@ static bool udd_ep_interrupt(void); * * Note: * Here, the global interrupt mask is not clear when an USB interrupt is enabled - * because this one can not be occured during the USB ISR (=during INTX is masked). + * because this one can not be occurred during the USB ISR (=during INTX is masked). * See Technical reference $3.8.3 Masking interrupt requests in peripheral modules. */ #ifdef UHD_ENABLE @@ -787,7 +787,7 @@ void udd_attach(void) udd_sleep_mode(true); otg_unfreeze_clock(); - // This section of clock check can be improved with a chek of + // This section of clock check can be improved with a check of // USB clock source via sysclk() // Check USB clock because the source can be a PLL while (!Is_otg_clock_usable()); @@ -803,7 +803,7 @@ void udd_attach(void) #ifdef USB_DEVICE_HS_SUPPORT udd_enable_msof_interrupt(); #endif - // Reset following interupts flag + // Reset following interrupts flag udd_ack_reset(); udd_ack_sof(); udd_ack_msof(); @@ -902,7 +902,7 @@ bool udd_ep_alloc(udd_ep_id_t ep, uint8_t bmAttributes, } dbg_print("alloc(%x, %d) ", ep, MaxEndpointSize); - // Bank choise + // Bank choice switch (bmAttributes & USB_EP_TYPE_MASK) { case USB_EP_TYPE_ISOCHRONOUS: nb_bank = UDD_ISOCHRONOUS_NB_BANK(ep); @@ -1228,7 +1228,7 @@ bool udd_ep_wait_stall_clear(udd_ep_id_t ep, if (Is_udd_endpoint_stall_requested(ep) || ptr_job->stall_requested) { - // Endpoint halted then registes the callback + // Endpoint halted then registers the callback ptr_job->busy = true; ptr_job->call_nohalt = callback; } else { @@ -1386,7 +1386,7 @@ static void udd_ctrl_setup_received(void) // Decode setup request if (udc_process_setup() == false) { - // Setup request unknow then stall it + // Setup request unknown then stall it udd_ctrl_stall_data(); udd_ack_setup_received(0); return; @@ -1447,7 +1447,7 @@ static void udd_ctrl_in_sent(void) udd_ctrl_prev_payload_buf_cnt += udd_ctrl_payload_buf_cnt; if ((udd_g_ctrlreq.req.wLength == udd_ctrl_prev_payload_buf_cnt) || b_shortpacket) { - // All data requested are transfered or a short packet has been sent + // All data requested are transferred or a short packet has been sent // then it is the end of data phase. // Generate an OUT ZLP for handshake phase. udd_ctrl_send_zlp_out(); @@ -1516,7 +1516,7 @@ static void udd_ctrl_out_received(void) // End of SETUP request: // - Data IN Phase aborted, // - or last Data IN Phase hidden by ZLP OUT sending quiclky, - // - or ZLP OUT received normaly. + // - or ZLP OUT received normally. udd_ctrl_endofrequest(); } else { // Protocol error during SETUP request @@ -1544,7 +1544,7 @@ static void udd_ctrl_out_received(void) (udd_ctrl_prev_payload_buf_cnt + udd_ctrl_payload_buf_cnt))) { // End of reception because it is a short packet - // Before send ZLP, call intermediat calback + // Before send ZLP, call intermediate callback // in case of data receiv generate a stall udd_g_ctrlreq.payload_size = udd_ctrl_payload_buf_cnt; if (NULL != udd_g_ctrlreq.over_under_run) { @@ -1565,7 +1565,7 @@ static void udd_ctrl_out_received(void) if (udd_g_ctrlreq.payload_size == udd_ctrl_payload_buf_cnt) { // Overrun then request a new payload buffer if (!udd_g_ctrlreq.over_under_run) { - // No callback availabled to request a new payload buffer + // No callback available to request a new payload buffer udd_ctrl_stall_data(); // Ack reception of OUT to replace NAK by a STALL udd_ack_out_received(0); @@ -1805,7 +1805,7 @@ static void udd_ep_trans_done(udd_ep_id_t ep) // transfer size of UDD_ENDPOINT_MAX_TRANS Bytes next_trans = UDD_ENDPOINT_MAX_TRANS; - // Set 0 to tranfer the maximum + // Set 0 to transfer the maximum udd_dma_ctrl = UOTGHS_DEVDMACONTROL_BUFF_LENGTH(0); } else { udd_dma_ctrl = UOTGHS_DEVDMACONTROL_BUFF_LENGTH(next_trans); @@ -1850,7 +1850,7 @@ static void udd_ep_trans_done(udd_ep_id_t ep) } cpu_irq_restore(flags); - // Here a ZLP has been recieved + // Here a ZLP has been received // and the DMA transfer must be not started. // It is the end of transfer ptr_job->buf_size = ptr_job->buf_cnt; @@ -1991,13 +1991,13 @@ static bool udd_ep_interrupt(void) } dbg_print("dma%x: ", ep); udd_disable_endpoint_dma_interrupt(ep); - // Save number of data no transfered + // Save number of data no transferred nb_remaining = (udd_endpoint_dma_get_status(ep) & UOTGHS_DEVDMASTATUS_BUFF_COUNT_Msk) >> UOTGHS_DEVDMASTATUS_BUFF_COUNT_Pos; if (nb_remaining) { // Transfer no complete (short packet or ZLP) then: - // Update number of data transfered + // Update number of data transferred ptr_job->buf_cnt -= nb_remaining; // Set transfer complete to stop the transfer ptr_job->buf_size = ptr_job->buf_cnt; @@ -2056,7 +2056,7 @@ static bool udd_ep_interrupt(void) udd_disable_endpoint_interrupt(ep); Assert(ptr_job->stall_requested); - // A stall has been requested during backgound transfer + // A stall has been requested during background transfer ptr_job->stall_requested = false; udd_disable_endpoint_bank_autoswitch(ep); udd_enable_stall_handshake(ep); diff --git a/Marlin/src/HAL/DUE/usb/usb_protocol_msc.h b/Marlin/src/HAL/DUE/usb/usb_protocol_msc.h index 0fef30804662..e1e59237d823 100644 --- a/Marlin/src/HAL/DUE/usb/usb_protocol_msc.h +++ b/Marlin/src/HAL/DUE/usb/usb_protocol_msc.h @@ -130,7 +130,7 @@ struct usb_msc_cbw { struct usb_msc_csw { le32_t dCSWSignature; //!< Must contain 'USBS' le32_t dCSWTag; //!< Same as dCBWTag - le32_t dCSWDataResidue; //!< Number of bytes not transfered + le32_t dCSWDataResidue; //!< Number of bytes not transferred uint8_t bCSWStatus; //!< Status code }; diff --git a/Marlin/src/HAL/DUE/usb/usb_task.c b/Marlin/src/HAL/DUE/usb/usb_task.c index 66bdb265d881..54a808d7f4f1 100644 --- a/Marlin/src/HAL/DUE/usb/usb_task.c +++ b/Marlin/src/HAL/DUE/usb/usb_task.c @@ -264,7 +264,7 @@ bool usb_task_extra_string(void) { ** Handle device requests that the ASF stack doesn't */ bool usb_task_other_requests(void) { - uint8_t* ptr = 0; + uint8_t *ptr = 0; uint16_t size = 0; if (Udd_setup_type() == USB_REQ_TYPE_VENDOR) { @@ -322,7 +322,7 @@ void usb_task_init(void) { char *sptr; // Patch in the filament diameter - sprintf_P(diam, PSTR("%d"), (int)((DEFAULT_NOMINAL_FILAMENT_DIA) * 1000.0)); + itoa((int)((DEFAULT_NOMINAL_FILAMENT_DIA) * 1000), diam, 10); // And copy it to the proper place, expanding it to unicode sptr = &diam[0]; diff --git a/Marlin/src/HAL/ESP32/FlushableHardwareSerial.cpp b/Marlin/src/HAL/ESP32/FlushableHardwareSerial.cpp index cc5a4fc476c4..145662215a9b 100644 --- a/Marlin/src/HAL/ESP32/FlushableHardwareSerial.cpp +++ b/Marlin/src/HAL/ESP32/FlushableHardwareSerial.cpp @@ -20,11 +20,10 @@ * */ -#include "FlushableHardwareSerial.h" - #ifdef ARDUINO_ARCH_ESP32 +#include "FlushableHardwareSerial.h" -Serial0Type flushableSerial(false, 0); +Serial1Class flushableSerial(false, 0); -#endif // ARDUINO_ARCH_ESP32 +#endif diff --git a/Marlin/src/HAL/ESP32/FlushableHardwareSerial.h b/Marlin/src/HAL/ESP32/FlushableHardwareSerial.h index 27df0be4b63b..012dda8626b0 100644 --- a/Marlin/src/HAL/ESP32/FlushableHardwareSerial.h +++ b/Marlin/src/HAL/ESP32/FlushableHardwareSerial.h @@ -21,9 +21,9 @@ */ #pragma once -#ifdef ARDUINO_ARCH_ESP32 - #include + +#include "../shared/Marduino.h" #include "../../core/serial_hook.h" class FlushableHardwareSerial : public HardwareSerial { @@ -31,6 +31,4 @@ class FlushableHardwareSerial : public HardwareSerial { FlushableHardwareSerial(int uart_nr) : HardwareSerial(uart_nr) {} }; -extern Serial0Type flushableSerial; - -#endif // ARDUINO_ARCH_ESP32 +extern Serial1Class flushableSerial; diff --git a/Marlin/src/HAL/ESP32/HAL.cpp b/Marlin/src/HAL/ESP32/HAL.cpp index fb5f531b22aa..adf5cecabe41 100644 --- a/Marlin/src/HAL/ESP32/HAL.cpp +++ b/Marlin/src/HAL/ESP32/HAL.cpp @@ -28,6 +28,10 @@ #include #include +#if ENABLED(USE_ESP32_TASK_WDT) + #include +#endif + #if ENABLED(WIFISUPPORT) #include #include "wifi.h" @@ -41,14 +45,14 @@ #endif #if ENABLED(ESP3D_WIFISUPPORT) - DefaultSerial MSerial(false, Serial2Socket); + DefaultSerial1 MSerial0(false, Serial2Socket); #endif // ------------------------ // Externs // ------------------------ -portMUX_TYPE spinlock = portMUX_INITIALIZER_UNLOCKED; +portMUX_TYPE MarlinHAL::spinlock = portMUX_INITIALIZER_UNLOCKED; // ------------------------ // Local defines @@ -60,7 +64,7 @@ portMUX_TYPE spinlock = portMUX_INITIALIZER_UNLOCKED; // Public Variables // ------------------------ -uint16_t HAL_adc_result; +uint16_t MarlinHAL::adc_result; // ------------------------ // Private Variables @@ -90,8 +94,26 @@ volatile int numPWMUsed = 0, #endif -void HAL_init_board() { +#if ENABLED(USE_ESP32_EXIO) + + HardwareSerial YSerial2(2); + + void Write_EXIO(uint8_t IO, uint8_t v) { + if (hal.isr_state()) { + hal.isr_off(); + YSerial2.write(0x80 | (((char)v) << 5) | (IO - 100)); + hal.isr_on(); + } + else + YSerial2.write(0x80 | (((char)v) << 5) | (IO - 100)); + } + +#endif +void MarlinHAL::init_board() { + #if ENABLED(USE_ESP32_TASK_WDT) + esp_task_wdt_init(10, true); + #endif #if ENABLED(ESP3D_WIFISUPPORT) esp3dlib.init(); #elif ENABLED(WIFISUPPORT) @@ -127,28 +149,31 @@ void HAL_init_board() { // Initialize the i2s peripheral only if the I2S stepper stream is enabled. // The following initialization is performed after Serial1 and Serial2 are defined as // their native pins might conflict with the i2s stream even when they are remapped. - TERN_(I2S_STEPPER_STREAM, i2s_init()); + #if ENABLED(USE_ESP32_EXIO) + YSerial2.begin(460800 * 3, SERIAL_8N1, 16, 17); + #elif ENABLED(I2S_STEPPER_STREAM) + i2s_init(); + #endif } -void HAL_idletask() { +void MarlinHAL::idletask() { #if BOTH(WIFISUPPORT, OTASUPPORT) OTA_handle(); #endif TERN_(ESP3D_WIFISUPPORT, esp3dlib.idletask()); } -void HAL_clear_reset_source() { } - -uint8_t HAL_get_reset_source() { return rtc_get_reset_reason(1); } +uint8_t MarlinHAL::get_reset_source() { return rtc_get_reset_reason(1); } -void _delay_ms(int delay_ms) { delay(delay_ms); } +void MarlinHAL::reboot() { ESP.restart(); } // return free memory between end of heap (or end bss) and whatever is current -int freeMemory() { return ESP.getFreeHeap(); } +int MarlinHAL::freeMemory() { return ESP.getFreeHeap(); } // ------------------------ // ADC // ------------------------ + #define ADC1_CHANNEL(pin) ADC1_GPIO ## pin ## _CHANNEL adc1_channel_t get_channel(int pin) { @@ -170,7 +195,7 @@ void adc1_set_attenuation(adc1_channel_t chan, adc_atten_t atten) { } } -void HAL_adc_init() { +void MarlinHAL::adc_init() { // Configure ADC adc1_config_width(ADC_WIDTH_12Bit); @@ -185,6 +210,9 @@ void HAL_adc_init() { TERN_(HAS_TEMP_ADC_7, adc3_set_attenuation(get_channel(TEMP_7_PIN), ADC_ATTEN_11db)); TERN_(HAS_HEATED_BED, adc1_set_attenuation(get_channel(TEMP_BED_PIN), ADC_ATTEN_11db)); TERN_(HAS_TEMP_CHAMBER, adc1_set_attenuation(get_channel(TEMP_CHAMBER_PIN), ADC_ATTEN_11db)); + TERN_(HAS_TEMP_PROBE, adc1_set_attenuation(get_channel(TEMP_PROBE_PIN), ADC_ATTEN_11db)); + TERN_(HAS_TEMP_COOLER, adc1_set_attenuation(get_channel(TEMP_COOLER_PIN), ADC_ATTEN_11db)); + TERN_(HAS_TEMP_BOARD, adc1_set_attenuation(get_channel(TEMP_BOARD_PIN), ADC_ATTEN_11db)); TERN_(FILAMENT_WIDTH_SENSOR, adc1_set_attenuation(get_channel(FILWIDTH_PIN), ADC_ATTEN_11db)); // Note that adc2 is shared with the WiFi module, which has higher priority, so the conversion may fail. @@ -199,11 +227,11 @@ void HAL_adc_init() { } } -void HAL_adc_start_conversion(const uint8_t adc_pin) { - const adc1_channel_t chan = get_channel(adc_pin); +void MarlinHAL::adc_start(const pin_t pin) { + const adc1_channel_t chan = get_channel(pin); uint32_t mv; esp_adc_cal_get_voltage((adc_channel_t)chan, &characteristics[attenuations[chan]], &mv); - HAL_adc_result = mv * 1023.0 / 3300.0; + adc_result = mv * 1023.0 / 3300.0; // Change the attenuation level based on the new reading adc_atten_t atten; @@ -249,7 +277,7 @@ void analogWrite(pin_t pin, int value) { idx = numPWMUsed; pwmPins[idx] = pin; // Start timer on first use - if (idx == 0) HAL_timer_start(PWM_TIMER_NUM, PWM_TIMER_FREQUENCY); + if (idx == 0) HAL_timer_start(MF_TIMER_PWM, PWM_TIMER_FREQUENCY); ++numPWMUsed; } @@ -260,7 +288,7 @@ void analogWrite(pin_t pin, int value) { // Handle PWM timer interrupt HAL_PWM_TIMER_ISR() { - HAL_timer_isr_prologue(PWM_TIMER_NUM); + HAL_timer_isr_prologue(MF_TIMER_PWM); static uint8_t count = 0; @@ -274,7 +302,7 @@ HAL_PWM_TIMER_ISR() { // 128 for 7 Bit resolution count = (count + 1) & 0x7F; - HAL_timer_isr_epilogue(PWM_TIMER_NUM); + HAL_timer_isr_epilogue(MF_TIMER_PWM); } #endif // ARDUINO_ARCH_ESP32 diff --git a/Marlin/src/HAL/ESP32/HAL.h b/Marlin/src/HAL/ESP32/HAL.h index 4d1db571d04a..df52e2186cbd 100644 --- a/Marlin/src/HAL/ESP32/HAL.h +++ b/Marlin/src/HAL/ESP32/HAL.h @@ -49,25 +49,20 @@ // Defines // ------------------------ -extern portMUX_TYPE spinlock; - -#define MYSERIAL0 flushableSerial +#define MYSERIAL1 flushableSerial #if EITHER(WIFISUPPORT, ESP3D_WIFISUPPORT) #if ENABLED(ESP3D_WIFISUPPORT) - typedef ForwardSerial0Type< decltype(Serial2Socket) > DefaultSerial; - extern DefaultSerial MSerial; - #define MYSERIAL1 MSerial + typedef ForwardSerial1Class< decltype(Serial2Socket) > DefaultSerial1; + extern DefaultSerial1 MSerial0; + #define MYSERIAL2 MSerial0 #else - #define MYSERIAL1 webSocketSerial + #define MYSERIAL2 webSocketSerial #endif #endif #define CRITICAL_SECTION_START() portENTER_CRITICAL(&spinlock) #define CRITICAL_SECTION_END() portEXIT_CRITICAL(&spinlock) -#define ISRS_ENABLED() (spinlock.owner == portMUX_FREE_VAL) -#define ENABLE_ISRS() if (spinlock.owner != portMUX_FREE_VAL) portEXIT_CRITICAL(&spinlock) -#define DISABLE_ISRS() portENTER_CRITICAL(&spinlock) // ------------------------ // Types @@ -75,14 +70,8 @@ extern portMUX_TYPE spinlock; typedef int16_t pin_t; -#define HAL_SERVO_LIB Servo - -// ------------------------ -// Public Variables -// ------------------------ - -/** result of last ADC conversion */ -extern uint16_t HAL_adc_result; +class Servo; +typedef Servo hal_servo_t; // ------------------------ // Public functions @@ -91,56 +80,21 @@ extern uint16_t HAL_adc_result; // // Tone // -void toneInit(); void tone(const pin_t _pin, const unsigned int frequency, const unsigned long duration=0); void noTone(const pin_t _pin); -// clear reset reason -void HAL_clear_reset_source(); - -// reset reason -uint8_t HAL_get_reset_source(); - -inline void HAL_reboot() {} // reboot the board or restart the bootloader - -void _delay_ms(int delay); - -#if GCC_VERSION <= 50000 - #pragma GCC diagnostic push - #pragma GCC diagnostic ignored "-Wunused-function" -#endif - -int freeMemory(); - -#if GCC_VERSION <= 50000 - #pragma GCC diagnostic pop -#endif - void analogWrite(pin_t pin, int value); -// ADC -#define HAL_ANALOG_SELECT(pin) - -void HAL_adc_init(); - -#define HAL_ADC_VREF 3.3 -#define HAL_ADC_RESOLUTION 10 -#define HAL_START_ADC(pin) HAL_adc_start_conversion(pin) -#define HAL_READ_ADC() HAL_adc_result -#define HAL_ADC_READY() true - -void HAL_adc_start_conversion(const uint8_t adc_pin); - +// +// Pin Mapping for M42, M43, M226 +// #define GET_PIN_MAP_PIN(index) index #define GET_PIN_MAP_INDEX(pin) pin #define PARSED_PIN_INDEX(code, dval) parser.intval(code, dval) -// Enable hooks into idle and setup for HAL -#define HAL_IDLETASK 1 -#define BOARD_INIT() HAL_init_board(); -void HAL_idletask(); -inline void HAL_init() {} -void HAL_init_board(); +#if ENABLED(USE_ESP32_EXIO) + void Write_EXIO(uint8_t IO, uint8_t v); +#endif // // Delay in cycles (used by DELAY_NS / DELAY_US) @@ -182,3 +136,85 @@ FORCE_INLINE static void DELAY_CYCLES(uint32_t x) { } } + +// ------------------------ +// Class Utilities +// ------------------------ + +#pragma GCC diagnostic push +#if GCC_VERSION <= 50000 + #pragma GCC diagnostic ignored "-Wunused-function" +#endif + +int freeMemory(); + +#pragma GCC diagnostic pop + +void _delay_ms(const int ms); + +// ------------------------ +// MarlinHAL Class +// ------------------------ + +#define HAL_ADC_VREF 3.3 +#define HAL_ADC_RESOLUTION 10 + +class MarlinHAL { +public: + + // Earliest possible init, before setup() + MarlinHAL() {} + + static void init() {} // Called early in setup() + static void init_board(); // Called less early in setup() + static void reboot(); // Restart the firmware + + // Interrupts + static portMUX_TYPE spinlock; + static bool isr_state() { return spinlock.owner == portMUX_FREE_VAL; } + static void isr_on() { if (spinlock.owner != portMUX_FREE_VAL) portEXIT_CRITICAL(&spinlock); } + static void isr_off() { portENTER_CRITICAL(&spinlock); } + + static void delay_ms(const int ms) { _delay_ms(ms); } + + // Tasks, called from idle() + static void idletask(); + + // Reset + static uint8_t get_reset_source(); + static void clear_reset_source() {} + + // Free SRAM + static int freeMemory(); + + // + // ADC Methods + // + + static uint16_t adc_result; + + // Called by Temperature::init once at startup + static void adc_init(); + + // Called by Temperature::init for each sensor at startup + static void adc_enable(const pin_t pin) {} + + // Begin ADC sampling on the given channel + static void adc_start(const pin_t pin); + + // Is the ADC ready for reading? + static bool adc_ready() { return true; } + + // The current value of the ADC register + static uint16_t adc_value() { return adc_result; } + + /** + * Set the PWM duty cycle for the pin to the given value. + * No inverting the duty cycle in this HAL. + * No changing the maximum size of the provided value to enable finer PWM duty control in this HAL. + */ + static void set_pwm_duty(const pin_t pin, const uint16_t v, const uint16_t=255, const bool=false) { + analogWrite(pin, v); + } + +}; diff --git a/Marlin/src/HAL/ESP32/HAL_SPI.cpp b/Marlin/src/HAL/ESP32/HAL_SPI.cpp index 8ee837ba1567..868ab1b6712d 100644 --- a/Marlin/src/HAL/ESP32/HAL_SPI.cpp +++ b/Marlin/src/HAL/ESP32/HAL_SPI.cpp @@ -53,11 +53,9 @@ static SPISettings spiConfig; // ------------------------ void spiBegin() { - #if !PIN_EXISTS(SD_SS) - #error "SD_SS_PIN not defined!" + #if ENABLED(SDSUPPORT) && PIN_EXISTS(SD_SS) + OUT_WRITE(SD_SS_PIN, HIGH); #endif - - OUT_WRITE(SD_SS_PIN, HIGH); } void spiInit(uint8_t spiRate) { @@ -85,7 +83,7 @@ uint8_t spiRec() { return returnByte; } -void spiRead(uint8_t* buf, uint16_t nbyte) { +void spiRead(uint8_t *buf, uint16_t nbyte) { SPI.beginTransaction(spiConfig); SPI.transferBytes(0, buf, nbyte); SPI.endTransaction(); @@ -97,7 +95,7 @@ void spiSend(uint8_t b) { SPI.endTransaction(); } -void spiSendBlock(uint8_t token, const uint8_t* buf) { +void spiSendBlock(uint8_t token, const uint8_t *buf) { SPI.beginTransaction(spiConfig); SPI.transfer(token); SPI.writeBytes(const_cast(buf), 512); diff --git a/Marlin/src/HAL/ESP32/MarlinSPI.h b/Marlin/src/HAL/ESP32/MarlinSPI.h new file mode 100644 index 000000000000..0c447ba4cb3d --- /dev/null +++ b/Marlin/src/HAL/ESP32/MarlinSPI.h @@ -0,0 +1,26 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2021 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + * + */ +#pragma once + +#include + +using MarlinSPI = SPIClass; diff --git a/Marlin/src/HAL/ESP32/Tone.cpp b/Marlin/src/HAL/ESP32/Tone.cpp index 376c0f32e129..839c612b6a87 100644 --- a/Marlin/src/HAL/ESP32/Tone.cpp +++ b/Marlin/src/HAL/ESP32/Tone.cpp @@ -35,19 +35,19 @@ static pin_t tone_pin; volatile static int32_t toggles; -void tone(const pin_t _pin, const unsigned int frequency, const unsigned long duration) { +void tone(const pin_t _pin, const unsigned int frequency, const unsigned long duration/*=0*/) { tone_pin = _pin; toggles = 2 * frequency * duration / 1000; - HAL_timer_start(TONE_TIMER_NUM, 2 * frequency); + HAL_timer_start(MF_TIMER_TONE, 2 * frequency); } void noTone(const pin_t _pin) { - HAL_timer_disable_interrupt(TONE_TIMER_NUM); + HAL_timer_disable_interrupt(MF_TIMER_TONE); WRITE(_pin, LOW); } HAL_TONE_TIMER_ISR() { - HAL_timer_isr_prologue(TONE_TIMER_NUM); + HAL_timer_isr_prologue(MF_TIMER_TONE); if (toggles) { toggles--; diff --git a/Marlin/src/HAL/ESP32/WebSocketSerial.cpp b/Marlin/src/HAL/ESP32/WebSocketSerial.cpp index 8825742d38fa..eb5b9d60395a 100644 --- a/Marlin/src/HAL/ESP32/WebSocketSerial.cpp +++ b/Marlin/src/HAL/ESP32/WebSocketSerial.cpp @@ -29,7 +29,7 @@ #include "wifi.h" #include -MSerialT webSocketSerial(false); +MSerialWebSocketT webSocketSerial(false); AsyncWebSocket ws("/ws"); // TODO Move inside the class. // RingBuffer impl @@ -137,7 +137,7 @@ size_t WebSocketSerial::write(const uint8_t c) { return ret; } -size_t WebSocketSerial::write(const uint8_t* buffer, size_t size) { +size_t WebSocketSerial::write(const uint8_t *buffer, size_t size) { size_t written = 0; for (size_t i = 0; i < size; i++) written += write(buffer[i]); diff --git a/Marlin/src/HAL/ESP32/WebSocketSerial.h b/Marlin/src/HAL/ESP32/WebSocketSerial.h index c68792c8c15d..6b3e419d10c5 100644 --- a/Marlin/src/HAL/ESP32/WebSocketSerial.h +++ b/Marlin/src/HAL/ESP32/WebSocketSerial.h @@ -54,7 +54,7 @@ class RingBuffer { ring_buffer_pos_t read(uint8_t *buffer); void flush(); ring_buffer_pos_t write(const uint8_t c); - ring_buffer_pos_t write(const uint8_t* buffer, ring_buffer_pos_t size); + ring_buffer_pos_t write(const uint8_t *buffer, ring_buffer_pos_t size); }; class WebSocketSerial: public Stream { @@ -70,7 +70,7 @@ class WebSocketSerial: public Stream { int read(); void flush(); size_t write(const uint8_t c); - size_t write(const uint8_t* buffer, size_t size); + size_t write(const uint8_t *buffer, size_t size); #if ENABLED(SERIAL_STATS_DROPPED_RX) FORCE_INLINE uint32_t dropped() { return 0; } @@ -81,5 +81,5 @@ class WebSocketSerial: public Stream { #endif }; -typedef Serial0Type MSerialT; -extern MSerialT webSocketSerial; +typedef Serial1Class MSerialWebSocketT; +extern MSerialWebSocketT webSocketSerial; diff --git a/Marlin/src/HAL/ESP32/endstop_interrupts.h b/Marlin/src/HAL/ESP32/endstop_interrupts.h index 743ccd99c904..4725df921b1a 100644 --- a/Marlin/src/HAL/ESP32/endstop_interrupts.h +++ b/Marlin/src/HAL/ESP32/endstop_interrupts.h @@ -59,4 +59,10 @@ void setup_endstop_interrupts() { TERN_(HAS_Z4_MAX, _ATTACH(Z4_MAX_PIN)); TERN_(HAS_Z4_MIN, _ATTACH(Z4_MIN_PIN)); TERN_(HAS_Z_MIN_PROBE_PIN, _ATTACH(Z_MIN_PROBE_PIN)); + TERN_(HAS_I_MAX, _ATTACH(I_MAX_PIN)); + TERN_(HAS_I_MIN, _ATTACH(I_MIN_PIN)); + TERN_(HAS_J_MAX, _ATTACH(J_MAX_PIN)); + TERN_(HAS_J_MIN, _ATTACH(J_MIN_PIN)); + TERN_(HAS_K_MAX, _ATTACH(K_MAX_PIN)); + TERN_(HAS_K_MIN, _ATTACH(K_MIN_PIN)); } diff --git a/Marlin/src/HAL/ESP32/esp32.csv b/Marlin/src/HAL/ESP32/esp32.csv new file mode 100644 index 000000000000..8f6e101f02ea --- /dev/null +++ b/Marlin/src/HAL/ESP32/esp32.csv @@ -0,0 +1,6 @@ +# Name, Type, SubType, Offset, Size, Flags +nvs, data, nvs, 0x9000, 0x5000, +otadata, data, ota, 0xe000, 0x2000, +app0, app, ota_0, 0x10000, 0x180000, +app1, app, ota_1, 0x190000, 0x180000, +spiffs, data, spiffs, 0x310000, 0xF0000, diff --git a/Marlin/src/HAL/ESP32/fastio.h b/Marlin/src/HAL/ESP32/fastio.h index 8db89dca12a1..c8e3f7e343e9 100644 --- a/Marlin/src/HAL/ESP32/fastio.h +++ b/Marlin/src/HAL/ESP32/fastio.h @@ -40,13 +40,19 @@ // Set pin as input with pullup mode #define _PULLUP(IO, v) pinMode(IO, v ? INPUT_PULLUP : INPUT) -// Read a pin wrapper -#define READ(IO) (IS_I2S_EXPANDER_PIN(IO) ? i2s_state(I2S_EXPANDER_PIN_INDEX(IO)) : digitalRead(IO)) - -// Write to a pin wrapper -#define WRITE(IO, v) (IS_I2S_EXPANDER_PIN(IO) ? i2s_write(I2S_EXPANDER_PIN_INDEX(IO), v) : digitalWrite(IO, v)) - -// Set pin as input wrapper +#if ENABLED(USE_ESP32_EXIO) + // Read a pin wrapper + #define READ(IO) digitalRead(IO) + // Write to a pin wrapper + #define WRITE(IO, v) (IO >= 100 ? Write_EXIO(IO, v) : digitalWrite(IO, v)) +#else + // Read a pin wrapper + #define READ(IO) (IS_I2S_EXPANDER_PIN(IO) ? i2s_state(I2S_EXPANDER_PIN_INDEX(IO)) : digitalRead(IO)) + // Write to a pin wrapper + #define WRITE(IO, v) (IS_I2S_EXPANDER_PIN(IO) ? i2s_write(I2S_EXPANDER_PIN_INDEX(IO), v) : digitalWrite(IO, v)) +#endif + +// Set pin as input wrapper (0x80 | (v << 5) | (IO - 100)) #define SET_INPUT(IO) _SET_INPUT(IO) // Set pin as input with pullup wrapper diff --git a/Marlin/src/HAL/ESP32/i2s.cpp b/Marlin/src/HAL/ESP32/i2s.cpp index e8f380654314..3e77b658360b 100644 --- a/Marlin/src/HAL/ESP32/i2s.cpp +++ b/Marlin/src/HAL/ESP32/i2s.cpp @@ -23,6 +23,8 @@ #include "../../inc/MarlinConfigPre.h" +#if DISABLED(USE_ESP32_EXIO) + #include "i2s.h" #include "../shared/Marduino.h" @@ -62,12 +64,9 @@ uint32_t i2s_port_data = 0; #define I2S_EXIT_CRITICAL() portEXIT_CRITICAL(&i2s_spinlock[i2s_num]) static inline void gpio_matrix_out_check(uint32_t gpio, uint32_t signal_idx, bool out_inv, bool oen_inv) { - //if pin = -1, do not need to configure - if (gpio != -1) { - PIN_FUNC_SELECT(GPIO_PIN_MUX_REG[gpio], PIN_FUNC_GPIO); - gpio_set_direction((gpio_num_t)gpio, (gpio_mode_t)GPIO_MODE_DEF_OUTPUT); - gpio_matrix_out(gpio, signal_idx, out_inv, oen_inv); - } + PIN_FUNC_SELECT(GPIO_PIN_MUX_REG[gpio], PIN_FUNC_GPIO); + gpio_set_direction((gpio_num_t)gpio, (gpio_mode_t)GPIO_MODE_DEF_OUTPUT); + gpio_matrix_out(gpio, signal_idx, out_inv, oen_inv); } static esp_err_t i2s_reset_fifo(i2s_port_t i2s_num) { @@ -139,7 +138,7 @@ static void IRAM_ATTR i2s_intr_handler_default(void *arg) { I2S0.int_clr.val = I2S0.int_st.val; //clear pending interrupt } -void stepperTask(void* parameter) { +void stepperTask(void *parameter) { uint32_t remaining = 0; while (1) { @@ -254,13 +253,7 @@ int i2s_init() { I2S0.fifo_conf.dscr_en = 0; - I2S0.conf_chan.tx_chan_mod = ( - #if ENABLED(I2S_STEPPER_SPLIT_STREAM) - 4 - #else - 0 - #endif - ); + I2S0.conf_chan.tx_chan_mod = TERN(I2S_STEPPER_SPLIT_STREAM, 4, 0); I2S0.fifo_conf.tx_fifo_mod = 0; I2S0.conf.tx_mono = 0; @@ -311,9 +304,16 @@ int i2s_init() { xTaskCreatePinnedToCore(stepperTask, "StepperTask", 10000, nullptr, 1, nullptr, CONFIG_ARDUINO_RUNNING_CORE); // run I2S stepper task on same core as rest of Marlin // Route the i2s pins to the appropriate GPIO - gpio_matrix_out_check(I2S_DATA, I2S0O_DATA_OUT23_IDX, 0, 0); - gpio_matrix_out_check(I2S_BCK, I2S0O_BCK_OUT_IDX, 0, 0); - gpio_matrix_out_check(I2S_WS, I2S0O_WS_OUT_IDX, 0, 0); + // If a pin is not defined, no need to configure + #if defined(I2S_DATA) && I2S_DATA >= 0 + gpio_matrix_out_check(I2S_DATA, I2S0O_DATA_OUT23_IDX, 0, 0); + #endif + #if defined(I2S_BCK) && I2S_BCK >= 0 + gpio_matrix_out_check(I2S_BCK, I2S0O_BCK_OUT_IDX, 0, 0); + #endif + #if defined(I2S_WS) && I2S_WS >= 0 + gpio_matrix_out_check(I2S_WS, I2S0O_WS_OUT_IDX, 0, 0); + #endif // Start the I2S peripheral return i2s_start(I2S_NUM_0); @@ -340,4 +340,5 @@ void i2s_push_sample() { dma.current[dma.rw_pos++] = i2s_port_data; } +#endif // !USE_ESP32_EXIO #endif // ARDUINO_ARCH_ESP32 diff --git a/Marlin/src/HAL/ESP32/spi_pins.h b/Marlin/src/HAL/ESP32/spi_pins.h index cfe71eee4a75..58881f0ea7ec 100644 --- a/Marlin/src/HAL/ESP32/spi_pins.h +++ b/Marlin/src/HAL/ESP32/spi_pins.h @@ -2,6 +2,9 @@ * Marlin 3D Printer Firmware * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * * This program is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation, either version 3 of the License, or diff --git a/Marlin/src/HAL/ESP32/timers.cpp b/Marlin/src/HAL/ESP32/timers.cpp index 57662a665882..c37ad2430cb2 100644 --- a/Marlin/src/HAL/ESP32/timers.cpp +++ b/Marlin/src/HAL/ESP32/timers.cpp @@ -41,7 +41,7 @@ static timg_dev_t *TG[2] = {&TIMERG0, &TIMERG1}; -const tTimerConfig TimerConfig [NUM_HARDWARE_TIMERS] = { +const tTimerConfig timer_config[NUM_HARDWARE_TIMERS] = { { TIMER_GROUP_0, TIMER_0, STEPPER_TIMER_PRESCALE, stepTC_Handler }, // 0 - Stepper { TIMER_GROUP_0, TIMER_1, TEMP_TIMER_PRESCALE, tempTC_Handler }, // 1 - Temperature { TIMER_GROUP_1, TIMER_0, PWM_TIMER_PRESCALE, pwmTC_Handler }, // 2 - PWM @@ -53,7 +53,7 @@ const tTimerConfig TimerConfig [NUM_HARDWARE_TIMERS] = { // ------------------------ void IRAM_ATTR timer_isr(void *para) { - const tTimerConfig& timer = TimerConfig[(int)para]; + const tTimerConfig& timer = timer_config[(int)para]; // Retrieve the interrupt status and the counter value // from the timer that reported the interrupt @@ -81,8 +81,8 @@ void IRAM_ATTR timer_isr(void *para) { * @param timer_num timer number to initialize * @param frequency frequency of the timer */ -void HAL_timer_start(const uint8_t timer_num, uint32_t frequency) { - const tTimerConfig timer = TimerConfig[timer_num]; +void HAL_timer_start(const uint8_t timer_num, const uint32_t frequency) { + const tTimerConfig timer = timer_config[timer_num]; timer_config_t config; config.divider = timer.divider; @@ -115,7 +115,7 @@ void HAL_timer_start(const uint8_t timer_num, uint32_t frequency) { * @param count threshold at which the interrupt is triggered */ void HAL_timer_set_compare(const uint8_t timer_num, hal_timer_t count) { - const tTimerConfig timer = TimerConfig[timer_num]; + const tTimerConfig timer = timer_config[timer_num]; timer_set_alarm_value(timer.group, timer.idx, count); } @@ -125,7 +125,7 @@ void HAL_timer_set_compare(const uint8_t timer_num, hal_timer_t count) { * @return the timer current threshold for the alarm to be triggered */ hal_timer_t HAL_timer_get_compare(const uint8_t timer_num) { - const tTimerConfig timer = TimerConfig[timer_num]; + const tTimerConfig timer = timer_config[timer_num]; uint64_t alarm_value; timer_get_alarm_value(timer.group, timer.idx, &alarm_value); @@ -139,7 +139,7 @@ hal_timer_t HAL_timer_get_compare(const uint8_t timer_num) { * @return the current counter of the alarm */ hal_timer_t HAL_timer_get_count(const uint8_t timer_num) { - const tTimerConfig timer = TimerConfig[timer_num]; + const tTimerConfig timer = timer_config[timer_num]; uint64_t counter_value; timer_get_counter_value(timer.group, timer.idx, &counter_value); return counter_value; @@ -150,7 +150,7 @@ hal_timer_t HAL_timer_get_count(const uint8_t timer_num) { * @param timer_num timer number to enable interrupts on */ void HAL_timer_enable_interrupt(const uint8_t timer_num) { - //const tTimerConfig timer = TimerConfig[timer_num]; + //const tTimerConfig timer = timer_config[timer_num]; //timer_enable_intr(timer.group, timer.idx); } @@ -159,12 +159,12 @@ void HAL_timer_enable_interrupt(const uint8_t timer_num) { * @param timer_num timer number to disable interrupts on */ void HAL_timer_disable_interrupt(const uint8_t timer_num) { - //const tTimerConfig timer = TimerConfig[timer_num]; + //const tTimerConfig timer = timer_config[timer_num]; //timer_disable_intr(timer.group, timer.idx); } bool HAL_timer_interrupt_enabled(const uint8_t timer_num) { - const tTimerConfig timer = TimerConfig[timer_num]; + const tTimerConfig timer = timer_config[timer_num]; return TG[timer.group]->int_ena.val | BIT(timer_num); } diff --git a/Marlin/src/HAL/ESP32/timers.h b/Marlin/src/HAL/ESP32/timers.h index a47697113d5d..aa4e1551f066 100644 --- a/Marlin/src/HAL/ESP32/timers.h +++ b/Marlin/src/HAL/ESP32/timers.h @@ -32,20 +32,20 @@ typedef uint64_t hal_timer_t; #define HAL_TIMER_TYPE_MAX 0xFFFFFFFFFFFFFFFFULL -#ifndef STEP_TIMER_NUM - #define STEP_TIMER_NUM 0 // Timer Index for Stepper +#ifndef MF_TIMER_STEP + #define MF_TIMER_STEP 0 // Timer Index for Stepper #endif -#ifndef PULSE_TIMER_NUM - #define PULSE_TIMER_NUM STEP_TIMER_NUM +#ifndef MF_TIMER_PULSE + #define MF_TIMER_PULSE MF_TIMER_STEP #endif -#ifndef TEMP_TIMER_NUM - #define TEMP_TIMER_NUM 1 // Timer Index for Temperature +#ifndef MF_TIMER_TEMP + #define MF_TIMER_TEMP 1 // Timer Index for Temperature #endif -#ifndef PWM_TIMER_NUM - #define PWM_TIMER_NUM 2 // index of timer to use for PWM outputs +#ifndef MF_TIMER_PWM + #define MF_TIMER_PWM 2 // index of timer to use for PWM outputs #endif -#ifndef TONE_TIMER_NUM - #define TONE_TIMER_NUM 3 // index of timer for beeper tones +#ifndef MF_TIMER_TONE + #define MF_TIMER_TONE 3 // index of timer for beeper tones #endif #define HAL_TIMER_RATE APB_CLK_FREQ // frequency of timer peripherals @@ -79,12 +79,12 @@ typedef uint64_t hal_timer_t; #define PULSE_TIMER_PRESCALE STEPPER_TIMER_PRESCALE #define PULSE_TIMER_TICKS_PER_US STEPPER_TIMER_TICKS_PER_US -#define ENABLE_STEPPER_DRIVER_INTERRUPT() HAL_timer_enable_interrupt(STEP_TIMER_NUM) -#define DISABLE_STEPPER_DRIVER_INTERRUPT() HAL_timer_disable_interrupt(STEP_TIMER_NUM) -#define STEPPER_ISR_ENABLED() HAL_timer_interrupt_enabled(STEP_TIMER_NUM) +#define ENABLE_STEPPER_DRIVER_INTERRUPT() HAL_timer_enable_interrupt(MF_TIMER_STEP) +#define DISABLE_STEPPER_DRIVER_INTERRUPT() HAL_timer_disable_interrupt(MF_TIMER_STEP) +#define STEPPER_ISR_ENABLED() HAL_timer_interrupt_enabled(MF_TIMER_STEP) -#define ENABLE_TEMPERATURE_INTERRUPT() HAL_timer_enable_interrupt(TEMP_TIMER_NUM) -#define DISABLE_TEMPERATURE_INTERRUPT() HAL_timer_disable_interrupt(TEMP_TIMER_NUM) +#define ENABLE_TEMPERATURE_INTERRUPT() HAL_timer_enable_interrupt(MF_TIMER_TEMP) +#define DISABLE_TEMPERATURE_INTERRUPT() HAL_timer_disable_interrupt(MF_TIMER_TEMP) #ifndef HAL_TEMP_TIMER_ISR #define HAL_TEMP_TIMER_ISR() extern "C" void tempTC_Handler() @@ -121,13 +121,13 @@ typedef struct { // Public Variables // ------------------------ -extern const tTimerConfig TimerConfig[]; +extern const tTimerConfig timer_config[]; // ------------------------ // Public functions // ------------------------ -void HAL_timer_start (const uint8_t timer_num, uint32_t frequency); +void HAL_timer_start(const uint8_t timer_num, const uint32_t frequency); void HAL_timer_set_compare(const uint8_t timer_num, const hal_timer_t count); hal_timer_t HAL_timer_get_compare(const uint8_t timer_num); hal_timer_t HAL_timer_get_count(const uint8_t timer_num); @@ -136,5 +136,5 @@ void HAL_timer_enable_interrupt(const uint8_t timer_num); void HAL_timer_disable_interrupt(const uint8_t timer_num); bool HAL_timer_interrupt_enabled(const uint8_t timer_num); -#define HAL_timer_isr_prologue(TIMER_NUM) -#define HAL_timer_isr_epilogue(TIMER_NUM) +#define HAL_timer_isr_prologue(T) NOOP +#define HAL_timer_isr_epilogue(T) NOOP diff --git a/Marlin/src/HAL/ESP32/u8g_esp32_spi.cpp b/Marlin/src/HAL/ESP32/u8g_esp32_spi.cpp new file mode 100644 index 000000000000..e454130d43f3 --- /dev/null +++ b/Marlin/src/HAL/ESP32/u8g_esp32_spi.cpp @@ -0,0 +1,99 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2022 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * Copypaste of SAMD51 HAL developed by Giuliano Zaro (AKA GMagician) + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + * + */ +#ifdef ARDUINO_ARCH_ESP32 + +#include "../../inc/MarlinConfigPre.h" + +#if ENABLED(FYSETC_MINI_12864_2_1) + +#include +#include "Arduino.h" +#include "../shared/HAL_SPI.h" +#include "SPI.h" + +static SPISettings spiConfig; + +#define MDOGLCD_MOSI 23 +#define MDOGLCD_SCK 18 +#define MLCD_RESET_PIN 0 +#define MLCD_PINS_DC 4 +#define MDOGLCD_CS 21 +#define MDOGLCD_A0 4 + +#ifndef LCD_SPI_SPEED + #ifdef SD_SPI_SPEED + #define LCD_SPI_SPEED SD_SPI_SPEED // Assume SPI speed shared with SD + #else + #define LCD_SPI_SPEED SPI_FULL_SPEED // Use full speed if SD speed is not supplied + #endif +#endif + +uint8_t u8g_eps_hw_spi_fn(u8g_t *u8g, uint8_t msg, uint8_t arg_val, void *arg_ptr) { + static uint8_t msgInitCount = 2; // Ignore all messages until 2nd U8G_COM_MSG_INIT + if (msgInitCount) { + if (msg == U8G_COM_MSG_INIT) msgInitCount--; + if (msgInitCount) return -1; + } + + switch (msg) { + case U8G_COM_MSG_STOP: break; + + case U8G_COM_MSG_INIT: + OUT_WRITE(MDOGLCD_CS, HIGH); + OUT_WRITE(MDOGLCD_A0, HIGH); + OUT_WRITE(MLCD_RESET_PIN, HIGH); + u8g_Delay(5); + spiBegin(); + spiInit(LCD_SPI_SPEED); + break; + + case U8G_COM_MSG_ADDRESS: /* define cmd (arg_val = 0) or data mode (arg_val = 1) */ + WRITE(MDOGLCD_A0, arg_val ? HIGH : LOW); + break; + + case U8G_COM_MSG_CHIP_SELECT: /* arg_val == 0 means HIGH level of U8G_PI_CS */ + WRITE(MDOGLCD_CS, arg_val ? LOW : HIGH); + break; + + case U8G_COM_MSG_RESET: + WRITE(MLCD_RESET_PIN, arg_val); + break; + + case U8G_COM_MSG_WRITE_BYTE: + spiSend((uint8_t)arg_val); + break; + + case U8G_COM_MSG_WRITE_SEQ: + uint8_t *ptr = (uint8_t*) arg_ptr; + while (arg_val > 0) { + spiSend(*ptr++); + arg_val--; + } + break; + } + return 1; +} + +#endif // FYSETC_MINI_12864_2_1 +#endif // ARDUINO_ARCH_ESP32 diff --git a/Marlin/src/HAL/ESP32/watchdog.h b/Marlin/src/HAL/ESP32/watchdog.h index b6c169e34742..43db8130763f 100644 --- a/Marlin/src/HAL/ESP32/watchdog.h +++ b/Marlin/src/HAL/ESP32/watchdog.h @@ -25,7 +25,7 @@ extern "C" { #endif - esp_err_t esp_task_wdt_reset(); + esp_err_t esp_task_wdt_reset(); #ifdef __cplusplus } diff --git a/Marlin/src/HAL/ESP32/wifi.cpp b/Marlin/src/HAL/ESP32/wifi.cpp index f4cf5a606a03..060f3bdb4874 100644 --- a/Marlin/src/HAL/ESP32/wifi.cpp +++ b/Marlin/src/HAL/ESP32/wifi.cpp @@ -59,7 +59,7 @@ void wifi_init() { MDNS.addService("http", "tcp", 80); - SERIAL_ECHOLNPAIR("Successfully connected to WiFi with SSID '" WIFI_SSID "', hostname: '" WIFI_HOSTNAME "', IP address: ", WiFi.localIP().toString().c_str()); + SERIAL_ECHOLNPGM("Successfully connected to WiFi with SSID '" WIFI_SSID "', hostname: '" WIFI_HOSTNAME "', IP address: ", WiFi.localIP().toString().c_str()); } #endif // WIFISUPPORT diff --git a/Marlin/src/HAL/HAL.h b/Marlin/src/HAL/HAL.h index 9eefda8fb1e2..0cd836af2b68 100644 --- a/Marlin/src/HAL/HAL.h +++ b/Marlin/src/HAL/HAL.h @@ -29,12 +29,6 @@ #include HAL_PATH(.,HAL.h) -#ifdef SERIAL_PORT_2 - #define NUM_SERIAL 2 -#else - #define NUM_SERIAL 1 -#endif - #define HAL_ADC_RANGE _BV(HAL_ADC_RESOLUTION) #ifndef I2C_ADDRESS diff --git a/Marlin/src/HAL/LINUX/HAL.cpp b/Marlin/src/HAL/LINUX/HAL.cpp index 771f1d2a087f..db43f42eaafd 100644 --- a/Marlin/src/HAL/LINUX/HAL.cpp +++ b/Marlin/src/HAL/LINUX/HAL.cpp @@ -24,6 +24,10 @@ #include "../../inc/MarlinConfig.h" #include "../shared/Delay.h" +// ------------------------ +// Serial ports +// ------------------------ + MSerialT usb_serial(TERN0(EMERGENCY_PARSER, true)); // U8glib required functions @@ -37,40 +41,21 @@ extern "C" { //************************// // return free heap space -int freeMemory() { - return 0; -} +int freeMemory() { return 0; } // ------------------------ // ADC // ------------------------ -void HAL_adc_init() { +uint8_t MarlinHAL::active_ch = 0; -} - -void HAL_adc_enable_channel(const uint8_t ch) { - -} - -uint8_t active_ch = 0; -void HAL_adc_start_conversion(const uint8_t ch) { - active_ch = ch; -} - -bool HAL_adc_finished() { - return true; -} - -uint16_t HAL_adc_get_result() { - pin_t pin = analogInputToDigitalPin(active_ch); +uint16_t MarlinHAL::adc_value() { + const pin_t pin = analogInputToDigitalPin(active_ch); if (!VALID_PIN(pin)) return 0; - uint16_t data = ((Gpio::get(pin) >> 2) & 0x3FF); + const uint16_t data = ((Gpio::get(pin) >> 2) & 0x3FF); return data; // return 10bit value as Marlin expects } -void HAL_pwm_init() { - -} +void MarlinHAL::reboot() { /* Reset the application state and GPIO */ } #endif // __PLAT_LINUX__ diff --git a/Marlin/src/HAL/LINUX/HAL.h b/Marlin/src/HAL/LINUX/HAL.h index e4f4dd3fc36a..43899c632de5 100644 --- a/Marlin/src/HAL/LINUX/HAL.h +++ b/Marlin/src/HAL/LINUX/HAL.h @@ -21,25 +21,42 @@ */ #pragma once -#define CPU_32_BIT - -#define F_CPU 100000000UL -#define SystemCoreClock F_CPU #include #include #include - #undef min #undef max - #include -void _printf (const char *format, ...); +#include "hardware/Clock.h" + +#include "../shared/Marduino.h" +#include "../shared/math_32bit.h" +#include "../shared/HAL_SPI.h" +#include "fastio.h" +#include "watchdog.h" +#include "serial.h" + +// ------------------------ +// Defines +// ------------------------ + +#define CPU_32_BIT +#define SHARED_SERVOS HAS_SERVOS // Use shared/servos.cpp + +#define F_CPU 100000000UL +#define SystemCoreClock F_CPU + +#define DELAY_CYCLES(x) Clock::delayCycles(x) + +#define CPU_ST7920_DELAY_1 600 +#define CPU_ST7920_DELAY_2 750 +#define CPU_ST7920_DELAY_3 750 + +void _printf(const char *format, ...); void _putc(uint8_t c); uint8_t _getc(); -//extern "C" volatile uint32_t _millis; - //arduino: Print.h #define DEC 10 #define HEX 16 @@ -49,67 +66,96 @@ uint8_t _getc(); #define B01 1 #define B10 2 -#include "hardware/Clock.h" - -#include "../shared/Marduino.h" -#include "../shared/math_32bit.h" -#include "../shared/HAL_SPI.h" -#include "fastio.h" -#include "watchdog.h" -#include "serial.h" - -#define SHARED_SERVOS HAS_SERVOS +// ------------------------ +// Serial ports +// ------------------------ extern MSerialT usb_serial; -#define MYSERIAL0 usb_serial - -#define ST7920_DELAY_1 DELAY_NS(600) -#define ST7920_DELAY_2 DELAY_NS(750) -#define ST7920_DELAY_3 DELAY_NS(750) +#define MYSERIAL1 usb_serial // // Interrupts // #define CRITICAL_SECTION_START() #define CRITICAL_SECTION_END() -#define ISRS_ENABLED() -#define ENABLE_ISRS() -#define DISABLE_ISRS() -inline void HAL_init() {} +// ADC +#define HAL_ADC_VREF 5.0 +#define HAL_ADC_RESOLUTION 10 + +// ------------------------ +// Class Utilities +// ------------------------ -// Utility functions +#pragma GCC diagnostic push #if GCC_VERSION <= 50000 - #pragma GCC diagnostic push #pragma GCC diagnostic ignored "-Wunused-function" #endif int freeMemory(); -#if GCC_VERSION <= 50000 - #pragma GCC diagnostic pop -#endif +#pragma GCC diagnostic pop -// ADC -#define HAL_ADC_VREF 5.0 -#define HAL_ADC_RESOLUTION 10 -#define HAL_ANALOG_SELECT(ch) HAL_adc_enable_channel(ch) -#define HAL_START_ADC(ch) HAL_adc_start_conversion(ch) -#define HAL_READ_ADC() HAL_adc_get_result() -#define HAL_ADC_READY() true - -void HAL_adc_init(); -void HAL_adc_enable_channel(const uint8_t ch); -void HAL_adc_start_conversion(const uint8_t ch); -uint16_t HAL_adc_get_result(); - -// Reset source -inline void HAL_clear_reset_source(void) {} -inline uint8_t HAL_get_reset_source(void) { return RST_POWER_ON; } - -inline void HAL_reboot() {} // reboot the board or restart the bootloader - -/* ---------------- Delay in cycles */ -FORCE_INLINE static void DELAY_CYCLES(uint64_t x) { - Clock::delayCycles(x); -} +// ------------------------ +// MarlinHAL Class +// ------------------------ + +class MarlinHAL { +public: + + // Earliest possible init, before setup() + MarlinHAL() {} + + static void init() {} // Called early in setup() + static void init_board() {} // Called less early in setup() + static void reboot(); // Reset the application state and GPIO + + // Interrupts + static bool isr_state() { return true; } + static void isr_on() {} + static void isr_off() {} + + static void delay_ms(const int ms) { _delay_ms(ms); } + + // Tasks, called from idle() + static void idletask() {} + + // Reset + static constexpr uint8_t reset_reason = RST_POWER_ON; + static uint8_t get_reset_source() { return reset_reason; } + static void clear_reset_source() {} + + // Free SRAM + static int freeMemory() { return ::freeMemory(); } + + // + // ADC Methods + // + + static uint8_t active_ch; + + // Called by Temperature::init once at startup + static void adc_init() {} + + // Called by Temperature::init for each sensor at startup + static void adc_enable(const uint8_t) {} + + // Begin ADC sampling on the given channel + static void adc_start(const uint8_t ch) { active_ch = ch; } + + // Is the ADC ready for reading? + static bool adc_ready() { return true; } + + // The current value of the ADC register + static uint16_t adc_value(); + + /** + * Set the PWM duty cycle for the pin to the given value. + * No option to change the resolution or invert the duty cycle. + */ + static void set_pwm_duty(const pin_t pin, const uint16_t v, const uint16_t=255, const bool=false) { + analogWrite(pin, v); + } + + static void set_pwm_frequency(const pin_t, int) {} +}; diff --git a/Marlin/src/HAL/LINUX/MarlinSPI.h b/Marlin/src/HAL/LINUX/MarlinSPI.h new file mode 100644 index 000000000000..0c447ba4cb3d --- /dev/null +++ b/Marlin/src/HAL/LINUX/MarlinSPI.h @@ -0,0 +1,26 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2021 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + * + */ +#pragma once + +#include + +using MarlinSPI = SPIClass; diff --git a/Marlin/src/HAL/LINUX/arduino.cpp b/Marlin/src/HAL/LINUX/arduino.cpp index 4b56d02a389c..075b4ccde2f4 100644 --- a/Marlin/src/HAL/LINUX/arduino.cpp +++ b/Marlin/src/HAL/LINUX/arduino.cpp @@ -31,9 +31,7 @@ void cli() { } // Disable void sei() { } // Enable // Time functions -void _delay_ms(const int delay_ms) { - delay(delay_ms); -} +void _delay_ms(const int ms) { delay(ms); } uint32_t millis() { return (uint32_t)Clock::millis(); diff --git a/Marlin/src/HAL/LINUX/hardware/Gpio.h b/Marlin/src/HAL/LINUX/hardware/Gpio.h index 2d9b1f29ebad..f946be648466 100644 --- a/Marlin/src/HAL/LINUX/hardware/Gpio.h +++ b/Marlin/src/HAL/LINUX/hardware/Gpio.h @@ -40,7 +40,7 @@ struct GpioEvent { pin_type pin_id; GpioEvent::Type event; - GpioEvent(uint64_t timestamp, pin_type pin_id, GpioEvent::Type event){ + GpioEvent(uint64_t timestamp, pin_type pin_id, GpioEvent::Type event) { this->timestamp = timestamp; this->pin_id = pin_id; this->event = event; diff --git a/Marlin/src/HAL/LINUX/hardware/Heater.cpp b/Marlin/src/HAL/LINUX/hardware/Heater.cpp index 70df8161829a..44f11986c98b 100644 --- a/Marlin/src/HAL/LINUX/hardware/Heater.cpp +++ b/Marlin/src/HAL/LINUX/hardware/Heater.cpp @@ -54,7 +54,7 @@ void Heater::update() { } void Heater::interrupt(GpioEvent ev) { - // ununsed + // unused } #endif // __PLAT_LINUX__ diff --git a/Marlin/src/HAL/LINUX/hardware/LinearAxis.cpp b/Marlin/src/HAL/LINUX/hardware/LinearAxis.cpp index c5b3ccc98656..e122ef3666c5 100644 --- a/Marlin/src/HAL/LINUX/hardware/LinearAxis.cpp +++ b/Marlin/src/HAL/LINUX/hardware/LinearAxis.cpp @@ -51,7 +51,7 @@ void LinearAxis::update() { } void LinearAxis::interrupt(GpioEvent ev) { - if (ev.pin_id == step_pin && !Gpio::pin_map[enable_pin].value){ + if (ev.pin_id == step_pin && !Gpio::pin_map[enable_pin].value) { if (ev.event == GpioEvent::RISE) { last_update = ev.timestamp; position += -1 + 2 * Gpio::pin_map[dir_pin].value; diff --git a/Marlin/src/HAL/LINUX/hardware/Timer.h b/Marlin/src/HAL/LINUX/hardware/Timer.h index 757efdcdbd7e..1b3b800dca3d 100644 --- a/Marlin/src/HAL/LINUX/hardware/Timer.h +++ b/Marlin/src/HAL/LINUX/hardware/Timer.h @@ -52,7 +52,7 @@ class Timer { return (*(intptr_t*)timerid); } - static void handler(int sig, siginfo_t *si, void *uc){ + static void handler(int sig, siginfo_t *si, void *uc) { Timer* _this = (Timer*)si->si_value.sival_ptr; _this->avg_error += (Clock::nanos() - _this->start_time) - _this->period; //high_resolution_clock is also limited in precision, but best we have _this->avg_error /= 2; //very crude precision analysis (actually within +-500ns usually) diff --git a/Marlin/src/HAL/LINUX/inc/SanityCheck.h b/Marlin/src/HAL/LINUX/inc/SanityCheck.h index 45bb2662ace5..36d3190a3e08 100644 --- a/Marlin/src/HAL/LINUX/inc/SanityCheck.h +++ b/Marlin/src/HAL/LINUX/inc/SanityCheck.h @@ -26,7 +26,7 @@ */ // Emulating RAMPS -#if ENABLED(SPINDLE_LASER_PWM) && !(SPINDLE_LASER_PWM_PIN == 4 || SPINDLE_LASER_PWM_PIN == 6 || SPINDLE_LASER_PWM_PIN == 11) +#if ENABLED(SPINDLE_LASER_USE_PWM) && !(SPINDLE_LASER_PWM_PIN == 4 || SPINDLE_LASER_PWM_PIN == 6 || SPINDLE_LASER_PWM_PIN == 11) #error "SPINDLE_LASER_PWM_PIN must use SERVO0, SERVO1 or SERVO3 connector" #endif diff --git a/Marlin/src/HAL/LINUX/include/Arduino.h b/Marlin/src/HAL/LINUX/include/Arduino.h index d4086e259a2f..f05aaed88083 100644 --- a/Marlin/src/HAL/LINUX/include/Arduino.h +++ b/Marlin/src/HAL/LINUX/include/Arduino.h @@ -59,10 +59,9 @@ typedef uint8_t byte; #endif #define sq(v) ((v) * (v)) -#define square(v) sq(v) #define constrain(value, arg_min, arg_max) ((value) < (arg_min) ? (arg_min) :((value) > (arg_max) ? (arg_max) : (value))) -//Interrupts +// Interrupts void cli(); // Disable void sei(); // Enable void attachInterrupt(uint32_t pin, void (*callback)(), uint32_t mode); @@ -74,8 +73,8 @@ extern "C" { } // Time functions -extern "C" void delay(const int milis); -void _delay_ms(const int delay); +extern "C" void delay(const int ms); +void _delay_ms(const int ms); void delayMicroseconds(unsigned long); uint32_t millis(); diff --git a/Marlin/src/HAL/LINUX/include/pinmapping.cpp b/Marlin/src/HAL/LINUX/include/pinmapping.cpp index 870ab3a96e5f..5823668cd50d 100644 --- a/Marlin/src/HAL/LINUX/include/pinmapping.cpp +++ b/Marlin/src/HAL/LINUX/include/pinmapping.cpp @@ -25,43 +25,6 @@ #include "../../../gcode/parser.h" -uint8_t analog_offset = NUM_DIGITAL_PINS - NUM_ANALOG_INPUTS; - -// Get the digital pin for an analog index -pin_t analogInputToDigitalPin(const int8_t p) { - return (WITHIN(p, 0, NUM_ANALOG_INPUTS) ? analog_offset + p : P_NC); -} - -// Return the index of a pin number -int16_t GET_PIN_MAP_INDEX(const pin_t pin) { - return pin; -} - -// Test whether the pin is valid -bool VALID_PIN(const pin_t p) { - return WITHIN(p, 0, NUM_DIGITAL_PINS); -} - -// Get the analog index for a digital pin -int8_t DIGITAL_PIN_TO_ANALOG_PIN(const pin_t p) { - return (WITHIN(p, analog_offset, NUM_DIGITAL_PINS) ? p - analog_offset : P_NC); -} - -// Test whether the pin is PWM -bool PWM_PIN(const pin_t p) { - return false; -} - -// Test whether the pin is interruptable -bool INTERRUPT_PIN(const pin_t p) { - return false; -} - -// Get the pin number at the given index -pin_t GET_PIN_MAP_PIN(const int16_t ind) { - return ind; -} - int16_t PARSED_PIN_INDEX(const char code, const int16_t dval) { return parser.intval(code, dval); } diff --git a/Marlin/src/HAL/LINUX/include/pinmapping.h b/Marlin/src/HAL/LINUX/include/pinmapping.h index 98f4b812e873..cfac5e3b48e4 100644 --- a/Marlin/src/HAL/LINUX/include/pinmapping.h +++ b/Marlin/src/HAL/LINUX/include/pinmapping.h @@ -34,26 +34,32 @@ constexpr uint8_t NUM_ANALOG_INPUTS = 16; #define HAL_SENSITIVE_PINS +constexpr uint8_t analog_offset = NUM_DIGITAL_PINS - NUM_ANALOG_INPUTS; + // Get the digital pin for an analog index -pin_t analogInputToDigitalPin(const int8_t p); +constexpr pin_t analogInputToDigitalPin(const int8_t p) { + return (WITHIN(p, 0, NUM_ANALOG_INPUTS) ? analog_offset + p : P_NC); +} + +// Get the analog index for a digital pin +constexpr int8_t DIGITAL_PIN_TO_ANALOG_PIN(const pin_t p) { + return (WITHIN(p, analog_offset, NUM_DIGITAL_PINS) ? p - analog_offset : P_NC); +} // Return the index of a pin number -int16_t GET_PIN_MAP_INDEX(const pin_t pin); +constexpr int16_t GET_PIN_MAP_INDEX(const pin_t pin) { return pin; } // Test whether the pin is valid -bool VALID_PIN(const pin_t p); - -// Get the analog index for a digital pin -int8_t DIGITAL_PIN_TO_ANALOG_PIN(const pin_t p); +constexpr bool VALID_PIN(const pin_t p) { return WITHIN(p, 0, NUM_DIGITAL_PINS); } // Test whether the pin is PWM -bool PWM_PIN(const pin_t p); +constexpr bool PWM_PIN(const pin_t p) { return false; } -// Test whether the pin is interruptable -bool INTERRUPT_PIN(const pin_t p); +// Test whether the pin is interruptible +constexpr bool INTERRUPT_PIN(const pin_t p) { return false; } // Get the pin number at the given index -pin_t GET_PIN_MAP_PIN(const int16_t ind); +constexpr pin_t GET_PIN_MAP_PIN(const int16_t ind) { return ind; } // Parse a G-code word into a pin index int16_t PARSED_PIN_INDEX(const char code, const int16_t dval); diff --git a/Marlin/src/HAL/LINUX/include/serial.h b/Marlin/src/HAL/LINUX/include/serial.h index 2585be25bf01..ebae066c3a57 100644 --- a/Marlin/src/HAL/LINUX/include/serial.h +++ b/Marlin/src/HAL/LINUX/include/serial.h @@ -115,4 +115,4 @@ struct HalSerial { volatile bool host_connected; }; -typedef Serial0Type MSerialT; +typedef Serial1Class MSerialT; diff --git a/Marlin/src/HAL/LINUX/main.cpp b/Marlin/src/HAL/LINUX/main.cpp index c409a83e5d55..f2af2ff33f52 100644 --- a/Marlin/src/HAL/LINUX/main.cpp +++ b/Marlin/src/HAL/LINUX/main.cpp @@ -2,6 +2,9 @@ * Marlin 3D Printer Firmware * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * * This program is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation, either version 3 of the License, or @@ -16,6 +19,7 @@ * along with this program. If not, see . * */ + #ifdef __PLAT_LINUX__ //#define GPIO_LOGGING // Full GPIO and Positional Logging @@ -105,8 +109,8 @@ int main() { std::thread write_serial (write_serial_thread); std::thread read_serial (read_serial_thread); - #ifdef MYSERIAL0 - MYSERIAL0.begin(BAUDRATE); + #ifdef MYSERIAL1 + MYSERIAL1.begin(BAUDRATE); SERIAL_ECHOLNPGM("x86_64 Initialized"); SERIAL_FLUSHTX(); #endif diff --git a/Marlin/src/HAL/LINUX/pinsDebug.h b/Marlin/src/HAL/LINUX/pinsDebug.h index 8f8543ef5984..7bfd97d024f7 100644 --- a/Marlin/src/HAL/LINUX/pinsDebug.h +++ b/Marlin/src/HAL/LINUX/pinsDebug.h @@ -2,6 +2,9 @@ * Marlin 3D Printer Firmware * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * * This program is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation, either version 3 of the License, or @@ -34,6 +37,7 @@ #define GET_ARRAY_PIN(p) pin_array[p].pin #define PRINT_ARRAY_NAME(x) do{ sprintf_P(buffer, PSTR("%-" STRINGIFY(MAX_NAME_LENGTH) "s"), pin_array[x].name); SERIAL_ECHO(buffer); }while(0) #define PRINT_PIN(p) do{ sprintf_P(buffer, PSTR("%3d "), p); SERIAL_ECHO(buffer); }while(0) +#define PRINT_PIN_ANALOG(p) do{ sprintf_P(buffer, PSTR(" (A%2d) "), DIGITAL_PIN_TO_ANALOG_PIN(pin)); SERIAL_ECHO(buffer); }while(0) #define MULTI_NAME_PAD 16 // space needed to be pretty if not first name assigned to a pin // active ADC function/mode/code values for PINSEL registers diff --git a/Marlin/src/HAL/LINUX/timers.h b/Marlin/src/HAL/LINUX/timers.h index 1beaea95abc6..2d2a95774c1b 100644 --- a/Marlin/src/HAL/LINUX/timers.h +++ b/Marlin/src/HAL/LINUX/timers.h @@ -37,14 +37,14 @@ typedef uint32_t hal_timer_t; #define HAL_TIMER_RATE ((SystemCoreClock) / 4) // frequency of timers peripherals -#ifndef STEP_TIMER_NUM - #define STEP_TIMER_NUM 0 // Timer Index for Stepper +#ifndef MF_TIMER_STEP + #define MF_TIMER_STEP 0 // Timer Index for Stepper #endif -#ifndef PULSE_TIMER_NUM - #define PULSE_TIMER_NUM STEP_TIMER_NUM +#ifndef MF_TIMER_PULSE + #define MF_TIMER_PULSE MF_TIMER_STEP #endif -#ifndef TEMP_TIMER_NUM - #define TEMP_TIMER_NUM 1 // Timer Index for Temperature +#ifndef MF_TIMER_TEMP + #define MF_TIMER_TEMP 1 // Timer Index for Temperature #endif #define TEMP_TIMER_RATE 1000000 @@ -58,12 +58,12 @@ typedef uint32_t hal_timer_t; #define PULSE_TIMER_PRESCALE STEPPER_TIMER_PRESCALE #define PULSE_TIMER_TICKS_PER_US STEPPER_TIMER_TICKS_PER_US -#define ENABLE_STEPPER_DRIVER_INTERRUPT() HAL_timer_enable_interrupt(STEP_TIMER_NUM) -#define DISABLE_STEPPER_DRIVER_INTERRUPT() HAL_timer_disable_interrupt(STEP_TIMER_NUM) -#define STEPPER_ISR_ENABLED() HAL_timer_interrupt_enabled(STEP_TIMER_NUM) +#define ENABLE_STEPPER_DRIVER_INTERRUPT() HAL_timer_enable_interrupt(MF_TIMER_STEP) +#define DISABLE_STEPPER_DRIVER_INTERRUPT() HAL_timer_disable_interrupt(MF_TIMER_STEP) +#define STEPPER_ISR_ENABLED() HAL_timer_interrupt_enabled(MF_TIMER_STEP) -#define ENABLE_TEMPERATURE_INTERRUPT() HAL_timer_enable_interrupt(TEMP_TIMER_NUM) -#define DISABLE_TEMPERATURE_INTERRUPT() HAL_timer_disable_interrupt(TEMP_TIMER_NUM) +#define ENABLE_TEMPERATURE_INTERRUPT() HAL_timer_enable_interrupt(MF_TIMER_TEMP) +#define DISABLE_TEMPERATURE_INTERRUPT() HAL_timer_disable_interrupt(MF_TIMER_TEMP) #ifndef HAL_STEP_TIMER_ISR #define HAL_STEP_TIMER_ISR() extern "C" void TIMER0_IRQHandler() @@ -77,7 +77,6 @@ typedef uint32_t hal_timer_t; #define HAL_PWM_TIMER_ISR() extern "C" void TIMER3_IRQHandler() #define HAL_PWM_TIMER_IRQn - void HAL_timer_init(); void HAL_timer_start(const uint8_t timer_num, const uint32_t frequency); @@ -93,5 +92,5 @@ void HAL_timer_enable_interrupt(const uint8_t timer_num); void HAL_timer_disable_interrupt(const uint8_t timer_num); bool HAL_timer_interrupt_enabled(const uint8_t timer_num); -#define HAL_timer_isr_prologue(TIMER_NUM) -#define HAL_timer_isr_epilogue(TIMER_NUM) +#define HAL_timer_isr_prologue(T) NOOP +#define HAL_timer_isr_epilogue(T) NOOP diff --git a/Marlin/src/HAL/LPC1768/HAL.cpp b/Marlin/src/HAL/LPC1768/HAL.cpp index 26a2c0e7db59..541848b08acc 100644 --- a/Marlin/src/HAL/LPC1768/HAL.cpp +++ b/Marlin/src/HAL/LPC1768/HAL.cpp @@ -29,9 +29,9 @@ #include "watchdog.h" #endif -DefaultSerial USBSerial(false, UsbSerial); +DefaultSerial1 USBSerial(false, UsbSerial); -uint32_t HAL_adc_reading = 0; +uint32_t MarlinHAL::adc_result = 0; // U8glib required functions extern "C" { @@ -41,8 +41,6 @@ extern "C" { void u8g_Delay(uint16_t val) { delay(val); } } -//************************// - // return free heap space int freeMemory() { char stack_end; @@ -54,31 +52,33 @@ int freeMemory() { return result; } -// scan command line for code -// return index into pin map array if found and the pin is valid. -// return dval if not found or not a valid pin. -int16_t PARSED_PIN_INDEX(const char code, const int16_t dval) { - const uint16_t val = (uint16_t)parser.intval(code, -1), port = val / 100, pin = val % 100; - const int16_t ind = (port < ((NUM_DIGITAL_PINS) >> 5) && pin < 32) ? ((port << 5) | pin) : -2; - return ind > -1 ? ind : dval; +void MarlinHAL::reboot() { NVIC_SystemReset(); } + +uint8_t MarlinHAL::get_reset_source() { + #if ENABLED(USE_WATCHDOG) + if (watchdog_timed_out()) return RST_WATCHDOG; + #endif + return RST_POWER_ON; +} + +void MarlinHAL::clear_reset_source() { + TERN_(USE_WATCHDOG, watchdog_clear_timeout_flag()); } void flashFirmware(const int16_t) { delay(500); // Give OS time to disconnect USB_Connect(false); // USB clear connection delay(1000); // Give OS time to notice - NVIC_SystemReset(); -} - -void HAL_clear_reset_source(void) { - TERN_(USE_WATCHDOG, watchdog_clear_timeout_flag()); + hal.reboot(); } -uint8_t HAL_get_reset_source(void) { - #if ENABLED(USE_WATCHDOG) - if (watchdog_timed_out()) return RST_WATCHDOG; - #endif - return RST_POWER_ON; +// For M42/M43, scan command line for pin code +// return index into pin map array if found and the pin is valid. +// return dval if not found or not a valid pin. +int16_t PARSED_PIN_INDEX(const char code, const int16_t dval) { + const uint16_t val = (uint16_t)parser.intval(code, -1), port = val / 100, pin = val % 100; + const int16_t ind = (port < ((NUM_DIGITAL_PINS) >> 5) && pin < 32) ? ((port << 5) | pin) : -2; + return ind > -1 ? ind : dval; } #endif // TARGET_LPC1768 diff --git a/Marlin/src/HAL/LPC1768/HAL.h b/Marlin/src/HAL/LPC1768/HAL.h index 1dc4fe6ff9c3..eefacae99549 100644 --- a/Marlin/src/HAL/LPC1768/HAL.h +++ b/Marlin/src/HAL/LPC1768/HAL.h @@ -28,8 +28,6 @@ #define CPU_32_BIT -void HAL_init(); - #include #include #include @@ -47,41 +45,41 @@ extern "C" volatile uint32_t _millis; #include #include -// -// Default graphical display delays -// -#ifndef ST7920_DELAY_1 - #define ST7920_DELAY_1 DELAY_NS(600) -#endif -#ifndef ST7920_DELAY_2 - #define ST7920_DELAY_2 DELAY_NS(750) -#endif -#ifndef ST7920_DELAY_3 - #define ST7920_DELAY_3 DELAY_NS(750) -#endif +// ------------------------ +// Serial ports +// ------------------------ -typedef ForwardSerial0Type< decltype(UsbSerial) > DefaultSerial; -extern DefaultSerial USBSerial; +typedef ForwardSerial1Class< decltype(UsbSerial) > DefaultSerial1; +extern DefaultSerial1 USBSerial; #define _MSERIAL(X) MSerial##X #define MSERIAL(X) _MSERIAL(X) -#define MSerial0 MSerial #if SERIAL_PORT == -1 - #define MYSERIAL0 USBSerial + #define MYSERIAL1 USBSerial #elif WITHIN(SERIAL_PORT, 0, 3) - #define MYSERIAL0 MSERIAL(SERIAL_PORT) + #define MYSERIAL1 MSERIAL(SERIAL_PORT) #else - #error "SERIAL_PORT must be from -1 to 3. Please update your configuration." + #error "SERIAL_PORT must be from 0 to 3. You can also use -1 if the board supports Native USB." #endif #ifdef SERIAL_PORT_2 #if SERIAL_PORT_2 == -1 - #define MYSERIAL1 USBSerial + #define MYSERIAL2 USBSerial #elif WITHIN(SERIAL_PORT_2, 0, 3) - #define MYSERIAL1 MSERIAL(SERIAL_PORT_2) + #define MYSERIAL2 MSERIAL(SERIAL_PORT_2) #else - #error "SERIAL_PORT_2 must be from -1 to 3. Please update your configuration." + #error "SERIAL_PORT_2 must be from 0 to 3. You can also use -1 if the board supports Native USB." + #endif +#endif + +#ifdef SERIAL_PORT_3 + #if SERIAL_PORT_3 == -1 + #define MYSERIAL3 USBSerial + #elif WITHIN(SERIAL_PORT_3, 0, 3) + #define MYSERIAL3 MSERIAL(SERIAL_PORT_3) + #else + #error "SERIAL_PORT_3 must be from 0 to 3. You can also use -1 if the board supports Native USB." #endif #endif @@ -91,7 +89,7 @@ extern DefaultSerial USBSerial; #elif WITHIN(MMU2_SERIAL_PORT, 0, 3) #define MMU2_SERIAL MSERIAL(MMU2_SERIAL_PORT) #else - #error "MMU2_SERIAL_PORT must be from -1 to 3. Please update your configuration." + #error "MMU2_SERIAL_PORT must be from 0 to 3. You can also use -1 if the board supports Native USB." #endif #endif @@ -101,35 +99,22 @@ extern DefaultSerial USBSerial; #elif WITHIN(LCD_SERIAL_PORT, 0, 3) #define LCD_SERIAL MSERIAL(LCD_SERIAL_PORT) #else - #error "LCD_SERIAL_PORT must be from -1 to 3. Please update your configuration." + #error "LCD_SERIAL_PORT must be from 0 to 3. You can also use -1 if the board supports Native USB." + #endif + #if HAS_DGUS_LCD + #define SERIAL_GET_TX_BUFFER_FREE() LCD_SERIAL.available() #endif #endif // // Interrupts // -#define CRITICAL_SECTION_START() uint32_t primask = __get_PRIMASK(); __disable_irq() -#define CRITICAL_SECTION_END() if (!primask) __enable_irq() -#define ISRS_ENABLED() (!__get_PRIMASK()) -#define ENABLE_ISRS() __enable_irq() -#define DISABLE_ISRS() __disable_irq() - -// -// Utility functions -// -#if GCC_VERSION <= 50000 - #pragma GCC diagnostic push - #pragma GCC diagnostic ignored "-Wunused-function" -#endif -int freeMemory(); - -#if GCC_VERSION <= 50000 - #pragma GCC diagnostic pop -#endif +#define CRITICAL_SECTION_START() const bool irqon = !__get_PRIMASK(); __disable_irq() +#define CRITICAL_SECTION_END() if (irqon) __enable_irq() // -// ADC API +// ADC // #define ADC_MEDIAN_FILTER_SIZE (23) // Higher values increase step delay (phase shift), @@ -148,20 +133,9 @@ int freeMemory(); #define HAL_ADC_RESOLUTION 12 // 15 bit maximum, raw temperature is stored as int16_t #define HAL_ADC_FILTERED // Disable oversampling done in Marlin as ADC values already filtered in HAL -using FilteredADC = LPC176x::ADC; -extern uint32_t HAL_adc_reading; -[[gnu::always_inline]] inline void HAL_start_adc(const pin_t pin) { - HAL_adc_reading = FilteredADC::read(pin) >> (16 - HAL_ADC_RESOLUTION); // returns 16bit value, reduce to required bits -} -[[gnu::always_inline]] inline uint16_t HAL_read_adc() { - return HAL_adc_reading; -} - -#define HAL_adc_init() -#define HAL_ANALOG_SELECT(pin) FilteredADC::enable_channel(pin) -#define HAL_START_ADC(pin) HAL_start_adc(pin) -#define HAL_READ_ADC() HAL_read_adc() -#define HAL_ADC_READY() (true) +// +// Pin Mapping for M42, M43, M226 +// // Test whether the pin is valid constexpr bool VALID_PIN(const pin_t pin) { @@ -186,34 +160,103 @@ constexpr pin_t GET_PIN_MAP_PIN(const int16_t index) { // Parse a G-code word into a pin index int16_t PARSED_PIN_INDEX(const char code, const int16_t dval); // P0.6 thru P0.9 are for the onboard SD card -#define HAL_SENSITIVE_PINS P0_06, P0_07, P0_08, P0_09 +#define HAL_SENSITIVE_PINS P0_06, P0_07, P0_08, P0_09, -#define HAL_IDLETASK 1 -void HAL_idletask(); +// ------------------------ +// Defines +// ------------------------ #define PLATFORM_M997_SUPPORT void flashFirmware(const int16_t); #define HAL_CAN_SET_PWM_FREQ // This HAL supports PWM Frequency adjustment -/** - * set_pwm_frequency - * Set the frequency of the timer corresponding to the provided pin - * All Hardware PWM pins run at the same frequency and all - * Software PWM pins run at the same frequency - */ -void set_pwm_frequency(const pin_t pin, int f_desired); +// Default graphical display delays +#define CPU_ST7920_DELAY_1 600 +#define CPU_ST7920_DELAY_2 750 +#define CPU_ST7920_DELAY_3 750 -/** - * set_pwm_duty - * Set the PWM duty cycle of the provided pin to the provided value - * Optionally allows inverting the duty cycle [default = false] - * Optionally allows changing the maximum size of the provided value to enable finer PWM duty control [default = 255] - */ -void set_pwm_duty(const pin_t pin, const uint16_t v, const uint16_t v_size=255, const bool invert=false); +// ------------------------ +// Class Utilities +// ------------------------ + +#pragma GCC diagnostic push +#if GCC_VERSION <= 50000 + #pragma GCC diagnostic ignored "-Wunused-function" +#endif + +int freeMemory(); + +#pragma GCC diagnostic pop + +// ------------------------ +// MarlinHAL Class +// ------------------------ + +class MarlinHAL { +public: + + // Earliest possible init, before setup() + MarlinHAL() {} + + static void init(); // Called early in setup() + static void init_board() {} // Called less early in setup() + static void reboot(); // Restart the firmware from 0x0 + + // Interrupts + static bool isr_state() { return !__get_PRIMASK(); } + static void isr_on() { __enable_irq(); } + static void isr_off() { __disable_irq(); } + + static void delay_ms(const int ms) { _delay_ms(ms); } + + // Tasks, called from idle() + static void idletask(); + + // Reset + static uint8_t get_reset_source(); + static void clear_reset_source(); + + // Free SRAM + static int freeMemory() { return ::freeMemory(); } + + // + // ADC Methods + // + + using FilteredADC = LPC176x::ADC; + + // Called by Temperature::init once at startup + static void adc_init() {} + + // Called by Temperature::init for each sensor at startup + static void adc_enable(const pin_t pin) { + FilteredADC::enable_channel(pin); + } + + // Begin ADC sampling on the given pin + static uint32_t adc_result; + static void adc_start(const pin_t pin) { + adc_result = FilteredADC::read(pin) >> (16 - HAL_ADC_RESOLUTION); // returns 16bit value, reduce to required bits + } + + // Is the ADC ready for reading? + static bool adc_ready() { return true; } + + // The current value of the ADC register + static uint16_t adc_value() { return uint16_t(adc_result); } -// Reset source -void HAL_clear_reset_source(void); -uint8_t HAL_get_reset_source(void); + /** + * Set the PWM duty cycle for the pin to the given value. + * Optionally invert the duty cycle [default = false] + * Optionally change the scale of the provided value to enable finer PWM duty control [default = 255] + */ + static void set_pwm_duty(const pin_t pin, const uint16_t v, const uint16_t v_size=255, const bool invert=false); -inline void HAL_reboot() {} // reboot the board or restart the bootloader + /** + * Set the frequency of the timer corresponding to the provided pin + * All Hardware PWM pins will run at the same frequency and + * All Software PWM pins will run at the same frequency + */ + static void set_pwm_frequency(const pin_t pin, const uint16_t f_desired); +}; diff --git a/Marlin/src/HAL/LPC1768/HAL_MinSerial.cpp b/Marlin/src/HAL/LPC1768/HAL_MinSerial.cpp index ab9af1fe002e..57065c49ac83 100644 --- a/Marlin/src/HAL/LPC1768/HAL_MinSerial.cpp +++ b/Marlin/src/HAL/LPC1768/HAL_MinSerial.cpp @@ -21,6 +21,7 @@ */ #ifdef TARGET_LPC1768 +#include "../../inc/MarlinConfig.h" #include "HAL.h" #if ENABLED(POSTMORTEM_DEBUGGING) diff --git a/Marlin/src/HAL/LPC1768/HAL_SPI.cpp b/Marlin/src/HAL/LPC1768/HAL_SPI.cpp index dbc89a33f547..29f9b43afef0 100644 --- a/Marlin/src/HAL/LPC1768/HAL_SPI.cpp +++ b/Marlin/src/HAL/LPC1768/HAL_SPI.cpp @@ -66,11 +66,7 @@ #include - #ifndef HAL_SPI_SPEED - #define HAL_SPI_SPEED SPI_FULL_SPEED - #endif - - static uint8_t SPI_speed = HAL_SPI_SPEED; + static uint8_t SPI_speed = SPI_FULL_SPEED; static uint8_t spiTransfer(uint8_t b) { return swSpiTransfer(b, SPI_speed, SD_SCK_PIN, SD_MISO_PIN, SD_MOSI_PIN); @@ -93,12 +89,12 @@ void spiSend(uint8_t b) { (void)spiTransfer(b); } - void spiSend(const uint8_t* buf, size_t nbyte) { + void spiSend(const uint8_t *buf, size_t nbyte) { for (uint16_t i = 0; i < nbyte; i++) (void)spiTransfer(buf[i]); } - void spiSendBlock(uint8_t token, const uint8_t* buf) { + void spiSendBlock(uint8_t token, const uint8_t *buf) { (void)spiTransfer(token); for (uint16_t i = 0; i < 512; i++) (void)spiTransfer(buf[i]); @@ -106,15 +102,13 @@ #else - #ifndef HAL_SPI_SPEED - #ifdef SD_SPI_SPEED - #define HAL_SPI_SPEED SD_SPI_SPEED - #else - #define HAL_SPI_SPEED SPI_FULL_SPEED - #endif + #ifdef SD_SPI_SPEED + #define INIT_SPI_SPEED SD_SPI_SPEED + #else + #define INIT_SPI_SPEED SPI_FULL_SPEED #endif - void spiBegin() { spiInit(HAL_SPI_SPEED); } // Set up SCK, MOSI & MISO pins for SSP0 + void spiBegin() { spiInit(INIT_SPI_SPEED); } // Set up SCK, MOSI & MISO pins for SSP0 void spiInit(uint8_t spiRate) { #if SD_MISO_PIN == BOARD_SPI1_MISO_PIN @@ -135,13 +129,13 @@ void spiSend(uint8_t b) { doio(b); } - void spiSend(const uint8_t* buf, size_t nbyte) { + void spiSend(const uint8_t *buf, size_t nbyte) { for (uint16_t i = 0; i < nbyte; i++) doio(buf[i]); } void spiSend(uint32_t chan, byte b) {} - void spiSend(uint32_t chan, const uint8_t* buf, size_t nbyte) {} + void spiSend(uint32_t chan, const uint8_t *buf, size_t nbyte) {} // Read single byte from SPI uint8_t spiRec() { return doio(0xFF); } @@ -156,7 +150,7 @@ uint8_t spiTransfer(uint8_t b) { return doio(b); } // Write from buffer to SPI - void spiSendBlock(uint8_t token, const uint8_t* buf) { + void spiSendBlock(uint8_t token, const uint8_t *buf) { (void)spiTransfer(token); for (uint16_t i = 0; i < 512; i++) (void)spiTransfer(buf[i]); diff --git a/Marlin/src/HAL/LPC1768/MarlinSerial.cpp b/Marlin/src/HAL/LPC1768/MarlinSerial.cpp index b0dfc0ae901c..f2aecf54a050 100644 --- a/Marlin/src/HAL/LPC1768/MarlinSerial.cpp +++ b/Marlin/src/HAL/LPC1768/MarlinSerial.cpp @@ -21,25 +21,26 @@ */ #ifdef TARGET_LPC1768 -#include "../../inc/MarlinConfigPre.h" #include "MarlinSerial.h" -#if ANY_SERIAL_IS(0) - MarlinSerial _MSerial(LPC_UART0); - MSerialT MSerial(true, _MSerial); - extern "C" void UART0_IRQHandler() { _MSerial.IRQHandler(); } +#include "../../inc/MarlinConfig.h" + +#if USING_HW_SERIAL0 + MarlinSerial _MSerial0(LPC_UART0); + MSerialT MSerial0(true, _MSerial0); + extern "C" void UART0_IRQHandler() { _MSerial0.IRQHandler(); } #endif -#if ANY_SERIAL_IS(1) +#if USING_HW_SERIAL1 MarlinSerial _MSerial1((LPC_UART_TypeDef *) LPC_UART1); MSerialT MSerial1(true, _MSerial1); extern "C" void UART1_IRQHandler() { _MSerial1.IRQHandler(); } #endif -#if ANY_SERIAL_IS(2) +#if USING_HW_SERIAL2 MarlinSerial _MSerial2(LPC_UART2); MSerialT MSerial2(true, _MSerial2); extern "C" void UART2_IRQHandler() { _MSerial2.IRQHandler(); } #endif -#if ANY_SERIAL_IS(3) +#if USING_HW_SERIAL3 MarlinSerial _MSerial3(LPC_UART3); MSerialT MSerial3(true, _MSerial3); extern "C" void UART3_IRQHandler() { _MSerial3.IRQHandler(); } @@ -50,16 +51,16 @@ bool MarlinSerial::recv_callback(const char c) { // Need to figure out which serial port we are and react in consequence (Marlin does not have CONTAINER_OF macro) if (false) {} - #if ANY_SERIAL_IS(0) - else if (this == &_MSerial) emergency_parser.update(MSerial.emergency_state, c); + #if USING_HW_SERIAL0 + else if (this == &_MSerial0) emergency_parser.update(MSerial0.emergency_state, c); #endif - #if ANY_SERIAL_IS(1) + #if USING_HW_SERIAL1 else if (this == &_MSerial1) emergency_parser.update(MSerial1.emergency_state, c); #endif - #if ANY_SERIAL_IS(2) + #if USING_HW_SERIAL2 else if (this == &_MSerial2) emergency_parser.update(MSerial2.emergency_state, c); #endif - #if ANY_SERIAL_IS(3) + #if USING_HW_SERIAL3 else if (this == &_MSerial3) emergency_parser.update(MSerial3.emergency_state, c); #endif return true; diff --git a/Marlin/src/HAL/LPC1768/MarlinSerial.h b/Marlin/src/HAL/LPC1768/MarlinSerial.h index 35c9362b9fb1..3e6848a1e3d0 100644 --- a/Marlin/src/HAL/LPC1768/MarlinSerial.h +++ b/Marlin/src/HAL/LPC1768/MarlinSerial.h @@ -46,6 +46,8 @@ class MarlinSerial : public HardwareSerial { void end() {} + uint8_t availableForWrite(void) { /* flushTX(); */ return TX_BUFFER_SIZE; } + #if ENABLED(EMERGENCY_PARSER) bool recv_callback(const char c) override; #endif @@ -54,14 +56,14 @@ class MarlinSerial : public HardwareSerial { // On LPC176x framework, HardwareSerial does not implement the same interface as Arduino's Serial, so overloads // of 'available' and 'read' method are not used in this multiple inheritance scenario. // Instead, use a ForwardSerial here that adapts the interface. -typedef ForwardSerial0Type MSerialT; -extern MSerialT MSerial; +typedef ForwardSerial1Class MSerialT; +extern MSerialT MSerial0; extern MSerialT MSerial1; extern MSerialT MSerial2; extern MSerialT MSerial3; -// Consequently, we can't use a RuntimeSerial either. The workaround would be to use a RuntimeSerial> type here -// Right now, let's ignore this until it's actually required. +// Consequently, we can't use a RuntimeSerial either. The workaround would be to use +// a RuntimeSerial> type here. Ignore for now until it's actually required. #if ENABLED(SERIAL_RUNTIME_HOOK) #error "SERIAL_RUNTIME_HOOK is not yet supported for LPC176x." #endif diff --git a/Marlin/src/HAL/LPC1768/Servo.h b/Marlin/src/HAL/LPC1768/Servo.h index eb12fd20f4d8..f02f503a67da 100644 --- a/Marlin/src/HAL/LPC1768/Servo.h +++ b/Marlin/src/HAL/LPC1768/Servo.h @@ -65,4 +65,5 @@ class libServo: public Servo { } }; -#define HAL_SERVO_LIB libServo +class libServo; +typedef libServo hal_servo_t; diff --git a/Marlin/src/HAL/LPC1768/eeprom_sdcard.cpp b/Marlin/src/HAL/LPC1768/eeprom_sdcard.cpp index 70395251df25..1991d7971959 100644 --- a/Marlin/src/HAL/LPC1768/eeprom_sdcard.cpp +++ b/Marlin/src/HAL/LPC1768/eeprom_sdcard.cpp @@ -1,10 +1,9 @@ /** * Marlin 3D Printer Firmware - * * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] - * Copyright (c) 2016 Bob Cousins bobcousins42@googlemail.com - * Copyright (c) 2015-2016 Nico Tonnhofer wurstnase.reprap@gmail.com - * Copyright (c) 2016 Victor Perez victor_pv@hotmail.com + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm * * This program is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by @@ -20,12 +19,19 @@ * along with this program. If not, see . * */ + +/** + * Implementation of EEPROM settings in SD Card + */ + #ifdef TARGET_LPC1768 #include "../../inc/MarlinConfig.h" #if ENABLED(SDCARD_EEPROM_EMULATION) +//#define DEBUG_SD_EEPROM_EMULATION + #include "../shared/eeprom_api.h" #include @@ -38,9 +44,11 @@ FATFS fat_fs; FIL eeprom_file; bool eeprom_file_open = false; +#define EEPROM_FILENAME "eeprom.dat" #ifndef MARLIN_EEPROM_SIZE #define MARLIN_EEPROM_SIZE size_t(0x1000) // 4KiB of Emulated EEPROM #endif + size_t PersistentStore::capacity() { return MARLIN_EEPROM_SIZE; } bool PersistentStore::access_start() { @@ -50,7 +58,7 @@ bool PersistentStore::access_start() { MSC_Release_Lock(); return false; } - FRESULT res = f_open(&eeprom_file, "eeprom.dat", FA_OPEN_ALWAYS | FA_WRITE | FA_READ); + FRESULT res = f_open(&eeprom_file, EEPROM_FILENAME, FA_OPEN_ALWAYS | FA_WRITE | FA_READ); if (res) MSC_Release_Lock(); if (res == FR_OK) { @@ -81,18 +89,20 @@ bool PersistentStore::access_finish() { // This extra chit-chat goes away soon, but is helpful for now // to see errors that are happening in read_data / write_data static void debug_rw(const bool write, int &pos, const uint8_t *value, const size_t size, const FRESULT s, const size_t total=0) { - PGM_P const rw_str = write ? PSTR("write") : PSTR("read"); - SERIAL_CHAR(' '); - SERIAL_ECHOPGM_P(rw_str); - SERIAL_ECHOLNPAIR("_data(", pos, ",", value, ",", size, ", ...)"); - if (total) { - SERIAL_ECHOPGM(" f_"); - SERIAL_ECHOPGM_P(rw_str); - SERIAL_ECHOPAIR("()=", s, "\n size=", size, "\n bytes_"); - SERIAL_ECHOLNPAIR_P(write ? PSTR("written=") : PSTR("read="), total); - } - else - SERIAL_ECHOLNPAIR(" f_lseek()=", s); + #if ENABLED(DEBUG_SD_EEPROM_EMULATION) + FSTR_P const rw_str = write ? F("write") : F("read"); + SERIAL_CHAR(' '); + SERIAL_ECHOF(rw_str); + SERIAL_ECHOLNPGM("_data(", pos, ",", *value, ",", size, ", ...)"); + if (total) { + SERIAL_ECHOPGM(" f_"); + SERIAL_ECHOF(rw_str); + SERIAL_ECHOPGM("()=", s, "\n size=", size, "\n bytes_"); + SERIAL_ECHOLNF(write ? F("written=") : F("read="), total); + } + else + SERIAL_ECHOLNPGM(" f_lseek()=", s); + #endif } // File function return codes for type FRESULT. This goes away soon, but diff --git a/Marlin/src/HAL/LPC1768/eeprom_wired.cpp b/Marlin/src/HAL/LPC1768/eeprom_wired.cpp index d94aba6119f1..f9286a74ac88 100644 --- a/Marlin/src/HAL/LPC1768/eeprom_wired.cpp +++ b/Marlin/src/HAL/LPC1768/eeprom_wired.cpp @@ -42,25 +42,22 @@ bool PersistentStore::access_start() { eeprom_init(); return true; } bool PersistentStore::access_finish() { return true; } bool PersistentStore::write_data(int &pos, const uint8_t *value, size_t size, uint16_t *crc) { + uint16_t written = 0; while (size--) { uint8_t v = *value; - - // EEPROM has only ~100,000 write cycles, - // so only write bytes that have changed! uint8_t * const p = (uint8_t * const)pos; - if (v != eeprom_read_byte(p)) { + if (v != eeprom_read_byte(p)) { // EEPROM has only ~100,000 write cycles, so only write bytes that have changed! eeprom_write_byte(p, v); + if (++written & 0x7F) delay(2); else safe_delay(2); // Avoid triggering watchdog during long EEPROM writes if (eeprom_read_byte(p) != v) { SERIAL_ECHO_MSG(STR_ERR_EEPROM_WRITE); return true; } } - crc16(crc, &v, 1); pos++; value++; - }; - + } return false; } @@ -68,7 +65,6 @@ bool PersistentStore::read_data(int &pos, uint8_t *value, size_t size, uint16_t do { // Read from external EEPROM const uint8_t c = eeprom_read_byte((uint8_t*)pos); - if (writing) *value = c; crc16(crc, &c, 1); pos++; diff --git a/Marlin/src/HAL/LPC1768/endstop_interrupts.h b/Marlin/src/HAL/LPC1768/endstop_interrupts.h index 126d6e7d5bb2..23bd0cc982b2 100644 --- a/Marlin/src/HAL/LPC1768/endstop_interrupts.h +++ b/Marlin/src/HAL/LPC1768/endstop_interrupts.h @@ -122,4 +122,37 @@ void setup_endstop_interrupts() { #endif _ATTACH(Z_MIN_PROBE_PIN); #endif + #if HAS_I_MAX + #if !LPC1768_PIN_INTERRUPT_M(I_MAX_PIN) + #error "I_MAX_PIN is not INTERRUPT-capable." + #endif + _ATTACH(I_MAX_PIN); + #elif HAS_I_MIN + #if !LPC1768_PIN_INTERRUPT_M(I_MIN_PIN) + #error "I_MIN_PIN is not INTERRUPT-capable." + #endif + _ATTACH(I_MIN_PIN); + #endif + #if HAS_J_MAX + #if !LPC1768_PIN_INTERRUPT_M(J_MAX_PIN) + #error "J_MAX_PIN is not INTERRUPT-capable." + #endif + _ATTACH(J_MAX_PIN); + #elif HAS_J_MIN + #if !LPC1768_PIN_INTERRUPT_M(J_MIN_PIN) + #error "J_MIN_PIN is not INTERRUPT-capable." + #endif + _ATTACH(J_MIN_PIN); + #endif + #if HAS_K_MAX + #if !LPC1768_PIN_INTERRUPT_M(K_MAX_PIN) + #error "K_MAX_PIN is not INTERRUPT-capable." + #endif + _ATTACH(K_MAX_PIN); + #elif HAS_K_MIN + #if !LPC1768_PIN_INTERRUPT_M(K_MIN_PIN) + #error "K_MIN_PIN is not INTERRUPT-capable." + #endif + _ATTACH(K_MIN_PIN); + #endif } diff --git a/Marlin/src/HAL/LPC1768/fast_pwm.cpp b/Marlin/src/HAL/LPC1768/fast_pwm.cpp index dd440b5e7730..6d2b1a9002c1 100644 --- a/Marlin/src/HAL/LPC1768/fast_pwm.cpp +++ b/Marlin/src/HAL/LPC1768/fast_pwm.cpp @@ -21,19 +21,17 @@ */ #ifdef TARGET_LPC1768 -#include "../../inc/MarlinConfigPre.h" - -#if NEEDS_HARDWARE_PWM // Specific meta-flag for features that mandate PWM - +#include "../../inc/MarlinConfig.h" #include -void set_pwm_frequency(const pin_t pin, int f_desired) { - LPC176x::pwm_set_frequency(pin, f_desired); +void MarlinHAL::set_pwm_duty(const pin_t pin, const uint16_t v, const uint16_t v_size/*=255*/, const bool invert/*=false*/) { + if (!LPC176x::pin_is_valid(pin)) return; + if (LPC176x::pwm_attach_pin(pin)) + LPC176x::pwm_write_ratio(pin, invert ? 1.0f - (float)v / v_size : (float)v / v_size); // map 1-254 onto PWM range } -void set_pwm_duty(const pin_t pin, const uint16_t v, const uint16_t v_size/*=255*/, const bool invert/*=false*/) { - LPC176x::pwm_write_ratio(pin, invert ? 1.0f - (float)v / v_size : (float)v / v_size); +void MarlinHAL::set_pwm_frequency(const pin_t pin, const uint16_t f_desired) { + LPC176x::pwm_set_frequency(pin, f_desired); } -#endif // NEEDS_HARDWARE_PWM #endif // TARGET_LPC1768 diff --git a/Marlin/src/HAL/LPC1768/inc/SanityCheck.h b/Marlin/src/HAL/LPC1768/inc/SanityCheck.h index dda1c640fa73..8265d58a6e8f 100644 --- a/Marlin/src/HAL/LPC1768/inc/SanityCheck.h +++ b/Marlin/src/HAL/LPC1768/inc/SanityCheck.h @@ -67,7 +67,7 @@ static_assert(!(NUM_SERVOS && ENABLED(FAST_PWM_FAN)), "BLTOUCH and Servos are in * Test LPC176x-specific configuration values for errors at compile-time. */ -//#if ENABLED(SPINDLE_LASER_PWM) && !(SPINDLE_LASER_PWM_PIN == 4 || SPINDLE_LASER_PWM_PIN == 6 || SPINDLE_LASER_PWM_PIN == 11) +//#if ENABLED(SPINDLE_LASER_USE_PWM) && !(SPINDLE_LASER_PWM_PIN == 4 || SPINDLE_LASER_PWM_PIN == 6 || SPINDLE_LASER_PWM_PIN == 11) // #error "SPINDLE_LASER_PWM_PIN must use SERVO0, SERVO1 or SERVO3 connector" //#endif @@ -92,7 +92,7 @@ static_assert(DISABLED(BAUD_RATE_GCODE), "BAUD_RATE_GCODE is not yet supported o #define ANY_TX(N,V...) DO(IS_TX##N,||,V) #define ANY_RX(N,V...) DO(IS_RX##N,||,V) -#if ANY_SERIAL_IS(0) +#if USING_HW_SERIAL0 #define IS_TX0(P) (P == P0_02) #define IS_RX0(P) (P == P0_03) #if IS_TX0(TMC_SW_MISO) || IS_RX0(TMC_SW_MOSI) @@ -106,14 +106,14 @@ static_assert(DISABLED(BAUD_RATE_GCODE), "BAUD_RATE_GCODE is not yet supported o #undef IS_RX0 #endif -#if ANY_SERIAL_IS(1) +#if USING_HW_SERIAL1 #define IS_TX1(P) (P == P0_15) #define IS_RX1(P) (P == P0_16) #define _IS_TX1_1 IS_TX1 #define _IS_RX1_1 IS_RX1 #if IS_TX1(TMC_SW_SCK) #error "Serial port pins (1) conflict with other pins!" - #elif HAS_WIRED_LCD + #elif HAS_ROTARY_ENCODER #if IS_TX1(BTN_EN2) || IS_RX1(BTN_EN1) #error "Serial port pins (1) conflict with Encoder Buttons!" #elif ANY_TX(1, SD_SCK_PIN, LCD_PINS_D4, DOGLCD_SCK, LCD_RESET_PIN, LCD_PINS_RS, SHIFT_CLK_PIN) \ @@ -127,7 +127,7 @@ static_assert(DISABLED(BAUD_RATE_GCODE), "BAUD_RATE_GCODE is not yet supported o #undef _IS_RX1_1 #endif -#if ANY_SERIAL_IS(2) +#if USING_HW_SERIAL2 #define IS_TX2(P) (P == P0_10) #define IS_RX2(P) (P == P0_11) #define _IS_TX2_1 IS_TX2 @@ -144,9 +144,9 @@ static_assert(DISABLED(BAUD_RATE_GCODE), "BAUD_RATE_GCODE is not yet supported o #error "Serial port pins (2) conflict with Z4 pins!" #elif ANY_RX(2, X_DIR_PIN, Y_DIR_PIN) #error "Serial port pins (2) conflict with other pins!" - #elif Y_HOME_DIR < 0 && IS_TX2(Y_STOP_PIN) + #elif Y_HOME_TO_MIN && IS_TX2(Y_STOP_PIN) #error "Serial port pins (2) conflict with Y endstop pin!" - #elif HAS_CUSTOM_PROBE_PIN && IS_TX2(Z_MIN_PROBE_PIN) + #elif USES_Z_MIN_PROBE_PIN && IS_TX2(Z_MIN_PROBE_PIN) #error "Serial port pins (2) conflict with probe pin!" #elif ANY_TX(2, X_ENABLE_PIN, Y_ENABLE_PIN) || ANY_RX(2, X_DIR_PIN, Y_DIR_PIN) #error "Serial port pins (2) conflict with X/Y stepper pins!" @@ -161,7 +161,7 @@ static_assert(DISABLED(BAUD_RATE_GCODE), "BAUD_RATE_GCODE is not yet supported o #undef _IS_RX2_1 #endif -#if ANY_SERIAL_IS(3) +#if USING_HW_SERIAL3 #define PIN_IS_TX3(P) (PIN_EXISTS(P) && P##_PIN == P0_00) #define PIN_IS_RX3(P) (P##_PIN == P0_01) #if PIN_IS_TX3(X_MIN) || PIN_IS_RX3(X_MAX) @@ -237,7 +237,7 @@ static_assert(DISABLED(BAUD_RATE_GCODE), "BAUD_RATE_GCODE is not yet supported o #define PIN_IS_SCL2(P) (P##_PIN == P0_11) #if PIN_IS_SDA2(Y_STOP) #error "i2c SDA2 overlaps with Y endstop pin!" - #elif HAS_CUSTOM_PROBE_PIN && PIN_IS_SDA2(Z_MIN_PROBE) + #elif USES_Z_MIN_PROBE_PIN && PIN_IS_SDA2(Z_MIN_PROBE) #error "i2c SDA2 overlaps with Z probe pin!" #elif PIN_IS_SDA2(X_ENABLE) || PIN_IS_SDA2(Y_ENABLE) #error "i2c SDA2 overlaps with X/Y ENABLE pin!" diff --git a/Marlin/src/HAL/LPC1768/include/SPI.h b/Marlin/src/HAL/LPC1768/include/SPI.h index ecd91f6a3b73..24f4759315bd 100644 --- a/Marlin/src/HAL/LPC1768/include/SPI.h +++ b/Marlin/src/HAL/LPC1768/include/SPI.h @@ -77,7 +77,7 @@ class SPISettings { //uint32_t spiRate() const { return spi_speed; } - static inline uint32_t spiRate2Clock(uint32_t spiRate) { + static uint32_t spiRate2Clock(uint32_t spiRate) { uint32_t Marlin_speed[7]; // CPSR is always 2 Marlin_speed[0] = 8333333; //(SCR: 2) desired: 8,000,000 actual: 8,333,333 +4.2% SPI_FULL_SPEED Marlin_speed[1] = 4166667; //(SCR: 5) desired: 4,000,000 actual: 4,166,667 +4.2% SPI_HALF_SPEED diff --git a/Marlin/src/HAL/LPC1768/main.cpp b/Marlin/src/HAL/LPC1768/main.cpp index 08fb1a1ed5d9..419c99793fb8 100644 --- a/Marlin/src/HAL/LPC1768/main.cpp +++ b/Marlin/src/HAL/LPC1768/main.cpp @@ -48,7 +48,7 @@ void SysTick_Callback() { disk_timerproc(); } TERN_(POSTMORTEM_DEBUGGING, extern void install_min_serial()); -void HAL_init() { +void MarlinHAL::init() { // Init LEDs #if PIN_EXISTS(LED) @@ -117,7 +117,7 @@ void HAL_init() { PinCfg.Pinmode = 2; // no pull-up/pull-down PINSEL_ConfigPin(&PinCfg); // now set CLKOUT_EN bit - LPC_SC->CLKOUTCFG |= (1<<8); + SBI(LPC_SC->CLKOUTCFG, 8); #endif USB_Init(); // USB Initialization @@ -130,7 +130,7 @@ void HAL_init() { const millis_t usb_timeout = millis() + 2000; while (!USB_Configuration && PENDING(millis(), usb_timeout)) { delay(50); - HAL_idletask(); + idletask(); #if PIN_EXISTS(LED) TOGGLE(LED_PIN); // Flash quickly during USB initialization #endif @@ -142,7 +142,7 @@ void HAL_init() { } // HAL idle task -void HAL_idletask() { +void MarlinHAL::idletask() { #if HAS_SHARED_MEDIA // If Marlin is using the SD card we need to lock it to prevent access from // a PC via USB. diff --git a/Marlin/src/HAL/LPC1768/pinsDebug.h b/Marlin/src/HAL/LPC1768/pinsDebug.h index f80551604fa1..a2f5c123a260 100644 --- a/Marlin/src/HAL/LPC1768/pinsDebug.h +++ b/Marlin/src/HAL/LPC1768/pinsDebug.h @@ -2,6 +2,9 @@ * Marlin 3D Printer Firmware * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * * This program is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation, either version 3 of the License, or @@ -33,8 +36,9 @@ #define PRINT_PORT(p) #define GET_ARRAY_PIN(p) pin_array[p].pin #define PRINT_ARRAY_NAME(x) do{ sprintf_P(buffer, PSTR("%-" STRINGIFY(MAX_NAME_LENGTH) "s"), pin_array[x].name); SERIAL_ECHO(buffer); }while(0) -#define PRINT_PIN(p) do{ sprintf_P(buffer, PSTR("%d.%02d"), LPC176x::pin_port(p), LPC176x::pin_bit(p)); SERIAL_ECHO(buffer); }while(0) -#define MULTI_NAME_PAD 16 // space needed to be pretty if not first name assigned to a pin +#define PRINT_PIN(p) do{ sprintf_P(buffer, PSTR("P%d_%02d"), LPC176x::pin_port(p), LPC176x::pin_bit(p)); SERIAL_ECHO(buffer); }while(0) +#define PRINT_PIN_ANALOG(p) do{ sprintf_P(buffer, PSTR("_A%d "), LPC176x::pin_get_adc_channel(pin)); SERIAL_ECHO(buffer); }while(0) +#define MULTI_NAME_PAD 17 // space needed to be pretty if not first name assigned to a pin // pins that will cause hang/reset/disconnect in M43 Toggle and Watch utilities #ifndef M43_NEVER_TOUCH @@ -48,6 +52,4 @@ bool GET_PINMODE(const pin_t pin) { return LPC176x::gpio_direction(pin); } -bool GET_ARRAY_IS_DIGITAL(const pin_t pin) { - return (!LPC176x::pin_has_adc(pin) || !LPC176x::pin_adc_enabled(pin)); -} +#define GET_ARRAY_IS_DIGITAL(x) ((bool) pin_array[x].is_digital) diff --git a/Marlin/src/HAL/LPC1768/tft/tft_spi.cpp b/Marlin/src/HAL/LPC1768/tft/tft_spi.cpp index a2cb66ab5bfd..a9847b2d2fa0 100644 --- a/Marlin/src/HAL/LPC1768/tft/tft_spi.cpp +++ b/Marlin/src/HAL/LPC1768/tft/tft_spi.cpp @@ -26,39 +26,22 @@ #include "tft_spi.h" -//TFT_SPI tft; - SPIClass TFT_SPI::SPIx(1); -#define TFT_CS_H WRITE(TFT_CS_PIN, HIGH) -#define TFT_CS_L WRITE(TFT_CS_PIN, LOW) - -#define TFT_DC_H WRITE(TFT_DC_PIN, HIGH) -#define TFT_DC_L WRITE(TFT_DC_PIN, LOW) - -#define TFT_RST_H WRITE(TFT_RESET_PIN, HIGH) -#define TFT_RST_L WRITE(TFT_RESET_PIN, LOW) - -#define TFT_BLK_H WRITE(TFT_BACKLIGHT_PIN, HIGH) -#define TFT_BLK_L WRITE(TFT_BACKLIGHT_PIN, LOW) - void TFT_SPI::Init() { #if PIN_EXISTS(TFT_RESET) - SET_OUTPUT(TFT_RESET_PIN); - TFT_RST_H; + OUT_WRITE(TFT_RESET_PIN, HIGH); delay(100); #endif #if PIN_EXISTS(TFT_BACKLIGHT) - SET_OUTPUT(TFT_BACKLIGHT_PIN); - TFT_BLK_H; + OUT_WRITE(TFT_BACKLIGHT_PIN, HIGH); #endif SET_OUTPUT(TFT_DC_PIN); SET_OUTPUT(TFT_CS_PIN); - - TFT_DC_H; - TFT_CS_H; + WRITE(TFT_DC_PIN, HIGH); + WRITE(TFT_CS_PIN, HIGH); /** * STM32F1 APB2 = 72MHz, APB1 = 36MHz, max SPI speed of this MCU if 18Mhz @@ -97,7 +80,7 @@ void TFT_SPI::Init() { void TFT_SPI::DataTransferBegin(uint16_t DataSize) { SPIx.setDataSize(DataSize); SPIx.begin(); - TFT_CS_L; + WRITE(TFT_CS_PIN, LOW); } uint32_t TFT_SPI::GetID() { @@ -116,7 +99,7 @@ uint32_t TFT_SPI::ReadID(uint16_t Reg) { SPIx.setDataSize(DATASIZE_8BIT); SPIx.setClock(SPI_CLOCK_DIV64); SPIx.begin(); - TFT_CS_L; + WRITE(TFT_CS_PIN, LOW); WriteReg(Reg); LOOP_L_N(i, 4) { @@ -131,21 +114,15 @@ uint32_t TFT_SPI::ReadID(uint16_t Reg) { return data >> 7; } -bool TFT_SPI::isBusy() { - return false; -} +bool TFT_SPI::isBusy() { return false; } -void TFT_SPI::Abort() { - DataTransferEnd(); -} +void TFT_SPI::Abort() { DataTransferEnd(); } -void TFT_SPI::Transmit(uint16_t Data) { - SPIx.transfer(Data); -} +void TFT_SPI::Transmit(uint16_t Data) { SPIx.transfer(Data); } void TFT_SPI::TransmitDMA(uint32_t MemoryIncrease, uint16_t *Data, uint16_t Count) { - DataTransferBegin(DATASIZE_16BIT); //16 - TFT_DC_H; + DataTransferBegin(DATASIZE_16BIT); + WRITE(TFT_DC_PIN, HIGH); SPIx.dmaSend(Data, Count, MemoryIncrease); DataTransferEnd(); } diff --git a/Marlin/src/HAL/LPC1768/tft/xpt2046.cpp b/Marlin/src/HAL/LPC1768/tft/xpt2046.cpp index cf14405484ff..9c1e158981da 100644 --- a/Marlin/src/HAL/LPC1768/tft/xpt2046.cpp +++ b/Marlin/src/HAL/LPC1768/tft/xpt2046.cpp @@ -22,7 +22,7 @@ #include "../../../inc/MarlinConfig.h" -#if HAS_TFT_XPT2046 || HAS_TOUCH_BUTTONS +#if HAS_TFT_XPT2046 || HAS_RES_TOUCH_BUTTONS #include "xpt2046.h" #include diff --git a/Marlin/src/HAL/LPC1768/tft/xpt2046.h b/Marlin/src/HAL/LPC1768/tft/xpt2046.h index 65602bda0f40..7c456cf00e1b 100644 --- a/Marlin/src/HAL/LPC1768/tft/xpt2046.h +++ b/Marlin/src/HAL/LPC1768/tft/xpt2046.h @@ -54,7 +54,7 @@ enum XPTCoordinate : uint8_t { XPT2046_Z2 = 0x40 | XPT2046_CONTROL | XPT2046_DFR_MODE, }; -#if !defined(XPT2046_Z1_THRESHOLD) +#ifndef XPT2046_Z1_THRESHOLD #define XPT2046_Z1_THRESHOLD 10 #endif @@ -65,8 +65,8 @@ class XPT2046 { static uint16_t getRawData(const XPTCoordinate coordinate); static bool isTouched(); - static inline void DataTransferBegin() { WRITE(TOUCH_CS_PIN, LOW); }; - static inline void DataTransferEnd() { WRITE(TOUCH_CS_PIN, HIGH); }; + static void DataTransferBegin() { WRITE(TOUCH_CS_PIN, LOW); }; + static void DataTransferEnd() { WRITE(TOUCH_CS_PIN, HIGH); }; #if ENABLED(TOUCH_BUTTONS_HW_SPI) static uint16_t HardwareIO(uint16_t data); #endif diff --git a/Marlin/src/HAL/LPC1768/timers.cpp b/Marlin/src/HAL/LPC1768/timers.cpp index a7a40584dabc..bbb13f81da05 100644 --- a/Marlin/src/HAL/LPC1768/timers.cpp +++ b/Marlin/src/HAL/LPC1768/timers.cpp @@ -40,7 +40,7 @@ void HAL_timer_init() { void HAL_timer_start(const uint8_t timer_num, const uint32_t frequency) { switch (timer_num) { - case 0: + case MF_TIMER_STEP: LPC_TIM0->MCR = _BV(SBIT_MR0I) | _BV(SBIT_MR0R); // Match on MR0, reset on MR0, interrupts when NVIC enables them LPC_TIM0->MR0 = uint32_t(STEPPER_TIMER_RATE) / frequency; // Match value (period) to set frequency LPC_TIM0->TCR = _BV(SBIT_CNTEN); // Counter Enable @@ -49,7 +49,7 @@ void HAL_timer_start(const uint8_t timer_num, const uint32_t frequency) { NVIC_EnableIRQ(TIMER0_IRQn); break; - case 1: + case MF_TIMER_TEMP: LPC_TIM1->MCR = _BV(SBIT_MR0I) | _BV(SBIT_MR0R); // Match on MR0, reset on MR0, interrupts when NVIC enables them LPC_TIM1->MR0 = uint32_t(TEMP_TIMER_RATE) / frequency; LPC_TIM1->TCR = _BV(SBIT_CNTEN); // Counter Enable diff --git a/Marlin/src/HAL/LPC1768/timers.h b/Marlin/src/HAL/LPC1768/timers.h index 4b638546856f..c6d7bc632e2e 100644 --- a/Marlin/src/HAL/LPC1768/timers.h +++ b/Marlin/src/HAL/LPC1768/timers.h @@ -60,17 +60,17 @@ typedef uint32_t hal_timer_t; #define HAL_TIMER_RATE ((F_CPU) / 4) // frequency of timers peripherals -#ifndef STEP_TIMER_NUM - #define STEP_TIMER_NUM 0 // Timer Index for Stepper +#ifndef MF_TIMER_STEP + #define MF_TIMER_STEP 0 // Timer Index for Stepper #endif -#ifndef PULSE_TIMER_NUM - #define PULSE_TIMER_NUM STEP_TIMER_NUM +#ifndef MF_TIMER_PULSE + #define MF_TIMER_PULSE MF_TIMER_STEP #endif -#ifndef TEMP_TIMER_NUM - #define TEMP_TIMER_NUM 1 // Timer Index for Temperature +#ifndef MF_TIMER_TEMP + #define MF_TIMER_TEMP 1 // Timer Index for Temperature #endif -#ifndef PWM_TIMER_NUM - #define PWM_TIMER_NUM 3 // Timer Index for PWM +#ifndef MF_TIMER_PWM + #define MF_TIMER_PWM 3 // Timer Index for PWM #endif #define TEMP_TIMER_RATE 1000000 @@ -84,23 +84,23 @@ typedef uint32_t hal_timer_t; #define PULSE_TIMER_PRESCALE STEPPER_TIMER_PRESCALE #define PULSE_TIMER_TICKS_PER_US STEPPER_TIMER_TICKS_PER_US -#define ENABLE_STEPPER_DRIVER_INTERRUPT() HAL_timer_enable_interrupt(STEP_TIMER_NUM) -#define DISABLE_STEPPER_DRIVER_INTERRUPT() HAL_timer_disable_interrupt(STEP_TIMER_NUM) -#define STEPPER_ISR_ENABLED() HAL_timer_interrupt_enabled(STEP_TIMER_NUM) +#define ENABLE_STEPPER_DRIVER_INTERRUPT() HAL_timer_enable_interrupt(MF_TIMER_STEP) +#define DISABLE_STEPPER_DRIVER_INTERRUPT() HAL_timer_disable_interrupt(MF_TIMER_STEP) +#define STEPPER_ISR_ENABLED() HAL_timer_interrupt_enabled(MF_TIMER_STEP) -#define ENABLE_TEMPERATURE_INTERRUPT() HAL_timer_enable_interrupt(TEMP_TIMER_NUM) -#define DISABLE_TEMPERATURE_INTERRUPT() HAL_timer_disable_interrupt(TEMP_TIMER_NUM) +#define ENABLE_TEMPERATURE_INTERRUPT() HAL_timer_enable_interrupt(MF_TIMER_TEMP) +#define DISABLE_TEMPERATURE_INTERRUPT() HAL_timer_disable_interrupt(MF_TIMER_TEMP) #ifndef HAL_STEP_TIMER_ISR - #define HAL_STEP_TIMER_ISR() _HAL_TIMER_ISR(STEP_TIMER_NUM) + #define HAL_STEP_TIMER_ISR() _HAL_TIMER_ISR(MF_TIMER_STEP) #endif #ifndef HAL_TEMP_TIMER_ISR - #define HAL_TEMP_TIMER_ISR() _HAL_TIMER_ISR(TEMP_TIMER_NUM) + #define HAL_TEMP_TIMER_ISR() _HAL_TIMER_ISR(MF_TIMER_TEMP) #endif // Timer references by index -#define STEP_TIMER_PTR _HAL_TIMER(STEP_TIMER_NUM) -#define TEMP_TIMER_PTR _HAL_TIMER(TEMP_TIMER_NUM) +#define STEP_TIMER_PTR _HAL_TIMER(MF_TIMER_STEP) +#define TEMP_TIMER_PTR _HAL_TIMER(MF_TIMER_TEMP) // ------------------------ // Public functions @@ -110,38 +110,38 @@ void HAL_timer_start(const uint8_t timer_num, const uint32_t frequency); FORCE_INLINE static void HAL_timer_set_compare(const uint8_t timer_num, const hal_timer_t compare) { switch (timer_num) { - case 0: STEP_TIMER_PTR->MR0 = compare; break; // Stepper Timer Match Register 0 - case 1: TEMP_TIMER_PTR->MR0 = compare; break; // Temp Timer Match Register 0 + case MF_TIMER_STEP: STEP_TIMER_PTR->MR0 = compare; break; // Stepper Timer Match Register 0 + case MF_TIMER_TEMP: TEMP_TIMER_PTR->MR0 = compare; break; // Temp Timer Match Register 0 } } FORCE_INLINE static hal_timer_t HAL_timer_get_compare(const uint8_t timer_num) { switch (timer_num) { - case 0: return STEP_TIMER_PTR->MR0; // Stepper Timer Match Register 0 - case 1: return TEMP_TIMER_PTR->MR0; // Temp Timer Match Register 0 + case MF_TIMER_STEP: return STEP_TIMER_PTR->MR0; // Stepper Timer Match Register 0 + case MF_TIMER_TEMP: return TEMP_TIMER_PTR->MR0; // Temp Timer Match Register 0 } return 0; } FORCE_INLINE static hal_timer_t HAL_timer_get_count(const uint8_t timer_num) { switch (timer_num) { - case 0: return STEP_TIMER_PTR->TC; // Stepper Timer Count - case 1: return TEMP_TIMER_PTR->TC; // Temp Timer Count + case MF_TIMER_STEP: return STEP_TIMER_PTR->TC; // Stepper Timer Count + case MF_TIMER_TEMP: return TEMP_TIMER_PTR->TC; // Temp Timer Count } return 0; } FORCE_INLINE static void HAL_timer_enable_interrupt(const uint8_t timer_num) { switch (timer_num) { - case 0: NVIC_EnableIRQ(TIMER0_IRQn); break; // Enable interrupt handler - case 1: NVIC_EnableIRQ(TIMER1_IRQn); break; // Enable interrupt handler + case MF_TIMER_STEP: NVIC_EnableIRQ(TIMER0_IRQn); break; // Enable interrupt handler + case MF_TIMER_TEMP: NVIC_EnableIRQ(TIMER1_IRQn); break; // Enable interrupt handler } } FORCE_INLINE static void HAL_timer_disable_interrupt(const uint8_t timer_num) { switch (timer_num) { - case 0: NVIC_DisableIRQ(TIMER0_IRQn); break; // Disable interrupt handler - case 1: NVIC_DisableIRQ(TIMER1_IRQn); break; // Disable interrupt handler + case MF_TIMER_STEP: NVIC_DisableIRQ(TIMER0_IRQn); break; // Disable interrupt handler + case MF_TIMER_TEMP: NVIC_DisableIRQ(TIMER1_IRQn); break; // Disable interrupt handler } // We NEED memory barriers to ensure Interrupts are actually disabled! @@ -157,17 +157,17 @@ FORCE_INLINE static bool NVIC_GetEnableIRQ(IRQn_Type IRQn) { FORCE_INLINE static bool HAL_timer_interrupt_enabled(const uint8_t timer_num) { switch (timer_num) { - case 0: return NVIC_GetEnableIRQ(TIMER0_IRQn); // Check if interrupt is enabled or not - case 1: return NVIC_GetEnableIRQ(TIMER1_IRQn); // Check if interrupt is enabled or not + case MF_TIMER_STEP: return NVIC_GetEnableIRQ(TIMER0_IRQn); // Check if interrupt is enabled or not + case MF_TIMER_TEMP: return NVIC_GetEnableIRQ(TIMER1_IRQn); // Check if interrupt is enabled or not } return false; } FORCE_INLINE static void HAL_timer_isr_prologue(const uint8_t timer_num) { switch (timer_num) { - case 0: SBI(STEP_TIMER_PTR->IR, SBIT_CNTEN); break; - case 1: SBI(TEMP_TIMER_PTR->IR, SBIT_CNTEN); break; + case MF_TIMER_STEP: SBI(STEP_TIMER_PTR->IR, SBIT_CNTEN); break; + case MF_TIMER_TEMP: SBI(TEMP_TIMER_PTR->IR, SBIT_CNTEN); break; } } -#define HAL_timer_isr_epilogue(TIMER_NUM) +#define HAL_timer_isr_epilogue(T) NOOP diff --git a/Marlin/src/HAL/LPC1768/u8g/u8g_com_HAL_LPC1768_hw_spi.cpp b/Marlin/src/HAL/LPC1768/u8g/u8g_com_HAL_LPC1768_hw_spi.cpp index b1eea13d5747..0118f92847de 100644 --- a/Marlin/src/HAL/LPC1768/u8g/u8g_com_HAL_LPC1768_hw_spi.cpp +++ b/Marlin/src/HAL/LPC1768/u8g/u8g_com_HAL_LPC1768_hw_spi.cpp @@ -59,7 +59,7 @@ #if HAS_MARLINUI_U8GLIB -#include +#include #include "../../shared/HAL_SPI.h" #ifndef LCD_SPI_SPEED diff --git a/Marlin/src/HAL/LPC1768/u8g/u8g_com_HAL_LPC1768_ssd_hw_i2c.cpp b/Marlin/src/HAL/LPC1768/u8g/u8g_com_HAL_LPC1768_ssd_hw_i2c.cpp index 6f7efba4ae20..bf76eaf0f491 100644 --- a/Marlin/src/HAL/LPC1768/u8g/u8g_com_HAL_LPC1768_ssd_hw_i2c.cpp +++ b/Marlin/src/HAL/LPC1768/u8g/u8g_com_HAL_LPC1768_ssd_hw_i2c.cpp @@ -79,7 +79,7 @@ #if HAS_MARLINUI_U8GLIB -#include +#include #define I2C_SLA (0x3C*2) //#define I2C_CMD_MODE 0x080 diff --git a/Marlin/src/HAL/LPC1768/u8g/u8g_com_HAL_LPC1768_st7920_hw_spi.cpp b/Marlin/src/HAL/LPC1768/u8g/u8g_com_HAL_LPC1768_st7920_hw_spi.cpp index 592e27f6c006..ce7b33801931 100644 --- a/Marlin/src/HAL/LPC1768/u8g/u8g_com_HAL_LPC1768_st7920_hw_spi.cpp +++ b/Marlin/src/HAL/LPC1768/u8g/u8g_com_HAL_LPC1768_st7920_hw_spi.cpp @@ -59,14 +59,14 @@ #if HAS_MARLINUI_U8GLIB -#include +#include #include "../../shared/HAL_SPI.h" #include "../../shared/Delay.h" void spiBegin(); void spiInit(uint8_t spiRate); void spiSend(uint8_t b); -void spiSend(const uint8_t* buf, size_t n); +void spiSend(const uint8_t *buf, size_t n); static uint8_t rs_last_state = 255; diff --git a/Marlin/src/HAL/LPC1768/u8g/u8g_com_HAL_LPC1768_st7920_sw_spi.cpp b/Marlin/src/HAL/LPC1768/u8g/u8g_com_HAL_LPC1768_st7920_sw_spi.cpp index 61211d9d881c..e159ebaa0ca8 100644 --- a/Marlin/src/HAL/LPC1768/u8g/u8g_com_HAL_LPC1768_st7920_sw_spi.cpp +++ b/Marlin/src/HAL/LPC1768/u8g/u8g_com_HAL_LPC1768_st7920_sw_spi.cpp @@ -57,9 +57,9 @@ #include "../../../inc/MarlinConfigPre.h" -#if ENABLED(U8GLIB_ST7920) +#if IS_U8GLIB_ST7920 -#include +#include #include #include "../../shared/Delay.h" #include "../../shared/HAL_SPI.h" @@ -143,5 +143,5 @@ uint8_t u8g_com_HAL_LPC1768_ST7920_sw_spi_fn(u8g_t *u8g, uint8_t msg, uint8_t ar return 1; } -#endif // U8GLIB_ST7920 +#endif // IS_U8GLIB_ST7920 #endif // TARGET_LPC1768 diff --git a/Marlin/src/HAL/LPC1768/u8g/u8g_com_HAL_LPC1768_sw_spi.cpp b/Marlin/src/HAL/LPC1768/u8g/u8g_com_HAL_LPC1768_sw_spi.cpp index 7f38ec54aff7..f116a9b80aa6 100644 --- a/Marlin/src/HAL/LPC1768/u8g/u8g_com_HAL_LPC1768_sw_spi.cpp +++ b/Marlin/src/HAL/LPC1768/u8g/u8g_com_HAL_LPC1768_sw_spi.cpp @@ -57,7 +57,7 @@ #include "../../../inc/MarlinConfigPre.h" -#if HAS_MARLINUI_U8GLIB && DISABLED(U8GLIB_ST7920) +#if HAS_MARLINUI_U8GLIB && !IS_U8GLIB_ST7920 #include #include "../../shared/HAL_SPI.h" @@ -71,7 +71,7 @@ #include #include -#include +#include uint8_t swSpiTransfer_mode_0(uint8_t b, const uint8_t spi_speed, const pin_t sck_pin, const pin_t miso_pin, const pin_t mosi_pin ) { @@ -205,5 +205,5 @@ uint8_t u8g_com_HAL_LPC1768_sw_spi_fn(u8g_t *u8g, uint8_t msg, uint8_t arg_val, return 1; } -#endif // HAS_MARLINUI_U8GLIB && !U8GLIB_ST7920 +#endif // HAS_MARLINUI_U8GLIB && !IS_U8GLIB_ST7920 #endif // TARGET_LPC1768 diff --git a/Marlin/src/HAL/LPC1768/upload_extra_script.py b/Marlin/src/HAL/LPC1768/upload_extra_script.py index 5967a9970f4c..7975f151f712 100755 --- a/Marlin/src/HAL/LPC1768/upload_extra_script.py +++ b/Marlin/src/HAL/LPC1768/upload_extra_script.py @@ -1,120 +1,127 @@ # -# sets output_port +# upload_extra_script.py +# set the output_port # if target_filename is found then that drive is used # else if target_drive is found then that drive is used # from __future__ import print_function -target_filename = "FIRMWARE.CUR" -target_drive = "REARM" +import pioutil +if pioutil.is_pio_build(): -import os,getpass,platform + target_filename = "FIRMWARE.CUR" + target_drive = "REARM" -current_OS = platform.system() -Import("env") + import os,getpass,platform -def print_error(e): - print('\nUnable to find destination disk (%s)\n' \ - 'Please select it in platformio.ini using the upload_port keyword ' \ - '(https://docs.platformio.org/en/latest/projectconf/section_env_upload.html) ' \ - 'or copy the firmware (.pio/build/%s/firmware.bin) manually to the appropriate disk\n' \ - %(e, env.get('PIOENV'))) + current_OS = platform.system() + Import("env") -try: - # - # Find a disk for upload - # - upload_disk = 'Disk not found' - target_file_found = False - target_drive_found = False - if current_OS == 'Windows': - # - # platformio.ini will accept this for a Windows upload port designation: 'upload_port = L:' - # Windows - doesn't care about the disk's name, only cares about the drive letter - import subprocess,string - from ctypes import windll + def print_error(e): + print('\nUnable to find destination disk (%s)\n' \ + 'Please select it in platformio.ini using the upload_port keyword ' \ + '(https://docs.platformio.org/en/latest/projectconf/section_env_upload.html) ' \ + 'or copy the firmware (.pio/build/%s/firmware.bin) manually to the appropriate disk\n' \ + %(e, env.get('PIOENV'))) - # getting list of drives - # https://stackoverflow.com/questions/827371/is-there-a-way-to-list-all-the-available-drive-letters-in-python - drives = [] - bitmask = windll.kernel32.GetLogicalDrives() - for letter in string.ascii_uppercase: - if bitmask & 1: - drives.append(letter) - bitmask >>= 1 + def before_upload(source, target, env): + try: + # + # Find a disk for upload + # + upload_disk = 'Disk not found' + target_file_found = False + target_drive_found = False + if current_OS == 'Windows': + # + # platformio.ini will accept this for a Windows upload port designation: 'upload_port = L:' + # Windows - doesn't care about the disk's name, only cares about the drive letter + import subprocess,string + from ctypes import windll - for drive in drives: - final_drive_name = drive + ':\\' - # print ('disc check: {}'.format(final_drive_name)) - try: - volume_info = str(subprocess.check_output('cmd /C dir ' + final_drive_name, stderr=subprocess.STDOUT)) - except Exception as e: - print ('error:{}'.format(e)) - continue - else: - if target_drive in volume_info and not target_file_found: # set upload if not found target file yet - target_drive_found = True - upload_disk = final_drive_name - if target_filename in volume_info: - if not target_file_found: - upload_disk = final_drive_name - target_file_found = True + # getting list of drives + # https://stackoverflow.com/questions/827371/is-there-a-way-to-list-all-the-available-drive-letters-in-python + drives = [] + bitmask = windll.kernel32.GetLogicalDrives() + for letter in string.ascii_uppercase: + if bitmask & 1: + drives.append(letter) + bitmask >>= 1 + + for drive in drives: + final_drive_name = drive + ':\\' + # print ('disc check: {}'.format(final_drive_name)) + try: + volume_info = str(subprocess.check_output('cmd /C dir ' + final_drive_name, stderr=subprocess.STDOUT)) + except Exception as e: + print ('error:{}'.format(e)) + continue + else: + if target_drive in volume_info and not target_file_found: # set upload if not found target file yet + target_drive_found = True + upload_disk = final_drive_name + if target_filename in volume_info: + if not target_file_found: + upload_disk = final_drive_name + target_file_found = True - elif current_OS == 'Linux': - # - # platformio.ini will accept this for a Linux upload port designation: 'upload_port = /media/media_name/drive' - # - drives = os.listdir(os.path.join(os.sep, 'media', getpass.getuser())) - if target_drive in drives: # If target drive is found, use it. - target_drive_found = True - upload_disk = os.path.join(os.sep, 'media', getpass.getuser(), target_drive) + os.sep - else: - for drive in drives: - try: - files = os.listdir(os.path.join(os.sep, 'media', getpass.getuser(), drive)) - except: - continue + elif current_OS == 'Linux': + # + # platformio.ini will accept this for a Linux upload port designation: 'upload_port = /media/media_name/drive' + # + drives = os.listdir(os.path.join(os.sep, 'media', getpass.getuser())) + if target_drive in drives: # If target drive is found, use it. + target_drive_found = True + upload_disk = os.path.join(os.sep, 'media', getpass.getuser(), target_drive) + os.sep else: - if target_filename in files: - upload_disk = os.path.join(os.sep, 'media', getpass.getuser(), drive) + os.sep - target_file_found = True - break - # - # set upload_port to drive if found - # + for drive in drives: + try: + files = os.listdir(os.path.join(os.sep, 'media', getpass.getuser(), drive)) + except: + continue + else: + if target_filename in files: + upload_disk = os.path.join(os.sep, 'media', getpass.getuser(), drive) + os.sep + target_file_found = True + break + # + # set upload_port to drive if found + # + + if target_file_found or target_drive_found: + env.Replace( + UPLOAD_FLAGS="-P$UPLOAD_PORT" + ) - if target_file_found or target_drive_found: - env.Replace( - UPLOAD_FLAGS="-P$UPLOAD_PORT" - ) + elif current_OS == 'Darwin': # MAC + # + # platformio.ini will accept this for a OSX upload port designation: 'upload_port = /media/media_name/drive' + # + drives = os.listdir('/Volumes') # human readable names + if target_drive in drives and not target_file_found: # set upload if not found target file yet + target_drive_found = True + upload_disk = '/Volumes/' + target_drive + '/' + for drive in drives: + try: + filenames = os.listdir('/Volumes/' + drive + '/') # will get an error if the drive is protected + except: + continue + else: + if target_filename in filenames: + if not target_file_found: + upload_disk = '/Volumes/' + drive + '/' + target_file_found = True - elif current_OS == 'Darwin': # MAC - # - # platformio.ini will accept this for a OSX upload port designation: 'upload_port = /media/media_name/drive' - # - drives = os.listdir('/Volumes') # human readable names - if target_drive in drives and not target_file_found: # set upload if not found target file yet - target_drive_found = True - upload_disk = '/Volumes/' + target_drive + '/' - for drive in drives: - try: - filenames = os.listdir('/Volumes/' + drive + '/') # will get an error if the drive is protected - except: - continue + # + # Set upload_port to drive if found + # + if target_file_found or target_drive_found: + env.Replace(UPLOAD_PORT=upload_disk) + print('\nUpload disk: ', upload_disk, '\n') else: - if target_filename in filenames: - if not target_file_found: - upload_disk = '/Volumes/' + drive + '/' - target_file_found = True + print_error('Autodetect Error') - # - # Set upload_port to drive if found - # - if target_file_found or target_drive_found: - env.Replace(UPLOAD_PORT=upload_disk) - print('\nUpload disk: ', upload_disk, '\n') - else: - print_error('Autodetect Error') + except Exception as e: + print_error(str(e)) -except Exception as e: - print_error(str(e)) + env.AddPreAction("upload", before_upload) diff --git a/Marlin/src/HAL/NATIVE_SIM/HAL.h b/Marlin/src/HAL/NATIVE_SIM/HAL.h new file mode 100644 index 000000000000..ee2e31fc7fa4 --- /dev/null +++ b/Marlin/src/HAL/NATIVE_SIM/HAL.h @@ -0,0 +1,263 @@ +/** + * Marlin 3D Printer Firmware + * + * Copyright (c) 2021 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * Copyright (c) 2016 Bob Cousins bobcousins42@googlemail.com + * Copyright (c) 2015-2016 Nico Tonnhofer wurstnase.reprap@gmail.com + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + * + */ +#pragma once + +#include +#include +#undef min +#undef max +#include +#include "pinmapping.h" + +void _printf (const char *format, ...); +void _putc(uint8_t c); +uint8_t _getc(); + +//arduino: Print.h +#define DEC 10 +#define HEX 16 +#define OCT 8 +#define BIN 2 +//arduino: binary.h (weird defines) +#define B01 1 +#define B10 2 + +#include "../shared/Marduino.h" +#include "../shared/math_32bit.h" +#include "../shared/HAL_SPI.h" +#include "fastio.h" +#include "watchdog.h" +#include "serial.h" + +// ------------------------ +// Defines +// ------------------------ + +#define CPU_32_BIT +#define SHARED_SERVOS HAS_SERVOS // Use shared/servos.cpp + +#define F_CPU 100000000 +#define SystemCoreClock F_CPU + +#define CPU_ST7920_DELAY_1 600 +#define CPU_ST7920_DELAY_2 750 +#define CPU_ST7920_DELAY_3 750 + +// ------------------------ +// Serial ports +// ------------------------ + +extern MSerialT serial_stream_0; +extern MSerialT serial_stream_1; +extern MSerialT serial_stream_2; +extern MSerialT serial_stream_3; + +#define _MSERIAL(X) serial_stream_##X +#define MSERIAL(X) _MSERIAL(X) + +#if WITHIN(SERIAL_PORT, 0, 3) + #define MYSERIAL1 MSERIAL(SERIAL_PORT) +#else + #error "SERIAL_PORT must be from 0 to 3. Please update your configuration." +#endif + +#ifdef SERIAL_PORT_2 + #if WITHIN(SERIAL_PORT_2, 0, 3) + #define MYSERIAL2 MSERIAL(SERIAL_PORT_2) + #else + #error "SERIAL_PORT_2 must be from 0 to 3. Please update your configuration." + #endif +#endif + +#ifdef MMU2_SERIAL_PORT + #if WITHIN(MMU2_SERIAL_PORT, 0, 3) + #define MMU2_SERIAL MSERIAL(MMU2_SERIAL_PORT) + #else + #error "MMU2_SERIAL_PORT must be from 0 to 3. Please update your configuration." + #endif +#endif + +#ifdef LCD_SERIAL_PORT + #if WITHIN(LCD_SERIAL_PORT, 0, 3) + #define LCD_SERIAL MSERIAL(LCD_SERIAL_PORT) + #else + #error "LCD_SERIAL_PORT must be from 0 to 3. Please update your configuration." + #endif +#endif + +// ------------------------ +// Interrupts +// ------------------------ + +#define CRITICAL_SECTION_START() +#define CRITICAL_SECTION_END() + +// ------------------------ +// ADC +// ------------------------ + +#define HAL_ADC_VREF 5.0 +#define HAL_ADC_RESOLUTION 10 + +/* ---------------- Delay in cycles */ + +#define DELAY_CYCLES(x) Kernel::delayCycles(x) +#define SYSTEM_YIELD() Kernel::yield() + +// Maple Compatibility +typedef void (*systickCallback_t)(void); +void systick_attach_callback(systickCallback_t cb); +extern volatile uint32_t systick_uptime_millis; + +// Marlin uses strstr in constexpr context, this is not supported, workaround by defining constexpr versions of the required functions. +#define strstr(a, b) strstr_constexpr((a), (b)) + +constexpr inline std::size_t strlen_constexpr(const char* str) { + // https://github.com/gcc-mirror/gcc/blob/5c7634a0e5f202935aa6c11b6ea953b8bf80a00a/libstdc%2B%2B-v3/include/bits/char_traits.h#L329 + if (str != nullptr) { + std::size_t i = 0; + while (str[i] != '\0') ++i; + return i; + } + return 0; +} + +constexpr inline int strncmp_constexpr(const char* lhs, const char* rhs, std::size_t count) { + // https://github.com/gcc-mirror/gcc/blob/13b9cbfc32fe3ac4c81c4dd9c42d141c8fb95db4/libstdc%2B%2B-v3/include/bits/char_traits.h#L655 + if (lhs == nullptr || rhs == nullptr) + return rhs != nullptr ? -1 : 1; + + for (std::size_t i = 0; i < count; ++i) + if (lhs[i] != rhs[i]) + return lhs[i] < rhs[i] ? -1 : 1; + else if (lhs[i] == '\0') + return 0; + + return 0; +} + +constexpr inline const char* strstr_constexpr(const char* str, const char* target) { + // https://github.com/freebsd/freebsd/blob/master/sys/libkern/strstr.c + if (char c = target != nullptr ? *target++ : '\0'; c != '\0' && str != nullptr) { + std::size_t len = strlen_constexpr(target); + do { + char sc = {}; + do { + if ((sc = *str++) == '\0') return nullptr; + } while (sc != c); + } while (strncmp_constexpr(str, target, len) != 0); + --str; + } + return str; +} + +constexpr inline char* strstr_constexpr(char* str, const char* target) { + // https://github.com/freebsd/freebsd/blob/master/sys/libkern/strstr.c + if (char c = target != nullptr ? *target++ : '\0'; c != '\0' && str != nullptr) { + std::size_t len = strlen_constexpr(target); + do { + char sc = {}; + do { + if ((sc = *str++) == '\0') return nullptr; + } while (sc != c); + } while (strncmp_constexpr(str, target, len) != 0); + --str; + } + return str; +} + +// ------------------------ +// Class Utilities +// ------------------------ + +#pragma GCC diagnostic push +#if GCC_VERSION <= 50000 + #pragma GCC diagnostic ignored "-Wunused-function" +#endif + +int freeMemory(); + +#pragma GCC diagnostic pop + +// ------------------------ +// MarlinHAL Class +// ------------------------ + +class MarlinHAL { +public: + + // Earliest possible init, before setup() + MarlinHAL() {} + + static void init() {} // Called early in setup() + static void init_board() {} // Called less early in setup() + static void reboot(); // Restart the firmware from 0x0 + + // Interrupts + static bool isr_state() { return true; } + static void isr_on() {} + static void isr_off() {} + + static void delay_ms(const int ms) { _delay_ms(ms); } + + // Tasks, called from idle() + static void idletask(); + + // Reset + static constexpr uint8_t reset_reason = RST_POWER_ON; + static uint8_t get_reset_source() { return reset_reason; } + static void clear_reset_source() {} + + // Free SRAM + static int freeMemory() { return ::freeMemory(); } + + // + // ADC Methods + // + + static uint8_t active_ch; + + // Called by Temperature::init once at startup + static void adc_init(); + + // Called by Temperature::init for each sensor at startup + static void adc_enable(const uint8_t ch); + + // Begin ADC sampling on the given channel + static void adc_start(const uint8_t ch); + + // Is the ADC ready for reading? + static bool adc_ready(); + + // The current value of the ADC register + static uint16_t adc_value(); + + /** + * Set the PWM duty cycle for the pin to the given value. + * No option to invert the duty cycle [default = false] + * No option to change the scale of the provided value to enable finer PWM duty control [default = 255] + */ + static void set_pwm_duty(const pin_t pin, const uint16_t v, const uint16_t=255, const bool=false) { + analogWrite(pin, v); + } + +}; diff --git a/Marlin/src/HAL/NATIVE_SIM/MarlinSPI.h b/Marlin/src/HAL/NATIVE_SIM/MarlinSPI.h new file mode 100644 index 000000000000..b5cc6f02a45a --- /dev/null +++ b/Marlin/src/HAL/NATIVE_SIM/MarlinSPI.h @@ -0,0 +1,26 @@ +/** + * Marlin 3D Printer Firmware + * + * Copyright (c) 2021 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * Copyright (c) 2016 Bob Cousins bobcousins42@googlemail.com + * Copyright (c) 2015-2016 Nico Tonnhofer wurstnase.reprap@gmail.com + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + * + */ +#pragma once + +#include + +using MarlinSPI = SPIClass; diff --git a/Marlin/src/HAL/NATIVE_SIM/fastio.h b/Marlin/src/HAL/NATIVE_SIM/fastio.h new file mode 100644 index 000000000000..de8013b1e542 --- /dev/null +++ b/Marlin/src/HAL/NATIVE_SIM/fastio.h @@ -0,0 +1,111 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2021 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + * + */ +#pragma once + +/** + * Fast I/O Routines for X86_64 + */ + +#include "../shared/Marduino.h" +#include + +#define SET_DIR_INPUT(IO) Gpio::setDir(IO, 1) +#define SET_DIR_OUTPUT(IO) Gpio::setDir(IO, 0) + +#define SET_MODE(IO, mode) Gpio::setMode(IO, mode) + +#define WRITE_PIN_SET(IO) Gpio::set(IO) +#define WRITE_PIN_CLR(IO) Gpio::clear(IO) + +#define READ_PIN(IO) Gpio::get(IO) +#define WRITE_PIN(IO,V) Gpio::set(IO, V) + +/** + * Magic I/O routines + * + * Now you can simply SET_OUTPUT(STEP); WRITE(STEP, HIGH); WRITE(STEP, LOW); + * + * Why double up on these macros? see http://gcc.gnu.org/onlinedocs/cpp/Stringification.html + */ + +/// Read a pin +#define _READ(IO) READ_PIN(IO) + +/// Write to a pin +#define _WRITE(IO,V) WRITE_PIN(IO,V) + +/// toggle a pin +#define _TOGGLE(IO) _WRITE(IO, !READ(IO)) + +/// set pin as input +#define _SET_INPUT(IO) SET_DIR_INPUT(IO) + +/// set pin as output +#define _SET_OUTPUT(IO) SET_DIR_OUTPUT(IO) + +/// set pin as input with pullup mode +#define _PULLUP(IO,V) pinMode(IO, (V) ? INPUT_PULLUP : INPUT) + +/// set pin as input with pulldown mode +#define _PULLDOWN(IO,V) pinMode(IO, (V) ? INPUT_PULLDOWN : INPUT) + +// hg42: all pins can be input or output (I hope) +// hg42: undefined pins create compile error (IO, is no pin) +// hg42: currently not used, but was used by pinsDebug + +/// check if pin is an input +#define _IS_INPUT(IO) (IO >= 0) + +/// check if pin is an output +#define _IS_OUTPUT(IO) (IO >= 0) + +/// Read a pin wrapper +#define READ(IO) _READ(IO) + +/// Write to a pin wrapper +#define WRITE(IO,V) _WRITE(IO,V) + +/// toggle a pin wrapper +#define TOGGLE(IO) _TOGGLE(IO) + +/// set pin as input wrapper +#define SET_INPUT(IO) _SET_INPUT(IO) +/// set pin as input with pullup wrapper +#define SET_INPUT_PULLUP(IO) do{ _SET_INPUT(IO); _PULLUP(IO, HIGH); }while(0) +/// set pin as input with pulldown wrapper +#define SET_INPUT_PULLDOWN(IO) do{ _SET_INPUT(IO); _PULLDOWN(IO, HIGH); }while(0) +/// set pin as output wrapper - reads the pin and sets the output to that value +#define SET_OUTPUT(IO) do{ _WRITE(IO, _READ(IO)); _SET_OUTPUT(IO); }while(0) +// set pin as PWM +#define SET_PWM(IO) SET_OUTPUT(IO) + +/// check if pin is an input wrapper +#define IS_INPUT(IO) _IS_INPUT(IO) +/// check if pin is an output wrapper +#define IS_OUTPUT(IO) _IS_OUTPUT(IO) + +// Shorthand +#define OUT_WRITE(IO,V) do{ SET_OUTPUT(IO); WRITE(IO,V); }while(0) + +// digitalRead/Write wrappers +#define extDigitalRead(IO) digitalRead(IO) +#define extDigitalWrite(IO,V) digitalWrite(IO,V) diff --git a/Marlin/src/HAL/NATIVE_SIM/inc/Conditionals_LCD.h b/Marlin/src/HAL/NATIVE_SIM/inc/Conditionals_LCD.h new file mode 100644 index 000000000000..1ac02f118285 --- /dev/null +++ b/Marlin/src/HAL/NATIVE_SIM/inc/Conditionals_LCD.h @@ -0,0 +1,22 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2021 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + * + */ +#pragma once diff --git a/Marlin/src/HAL/NATIVE_SIM/inc/Conditionals_adv.h b/Marlin/src/HAL/NATIVE_SIM/inc/Conditionals_adv.h new file mode 100644 index 000000000000..69b6b4848f6e --- /dev/null +++ b/Marlin/src/HAL/NATIVE_SIM/inc/Conditionals_adv.h @@ -0,0 +1,31 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2021 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + * + */ +#pragma once + +// Add strcmp_P if missing +#ifndef strcmp_P + #define strcmp_P(a, b) strcmp((a), (b)) +#endif + +#ifndef strcat_P + #define strcat_P(dest, src) strcat((dest), (src)) +#endif diff --git a/Marlin/src/HAL/NATIVE_SIM/inc/Conditionals_post.h b/Marlin/src/HAL/NATIVE_SIM/inc/Conditionals_post.h new file mode 100644 index 000000000000..1ac02f118285 --- /dev/null +++ b/Marlin/src/HAL/NATIVE_SIM/inc/Conditionals_post.h @@ -0,0 +1,22 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2021 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + * + */ +#pragma once diff --git a/Marlin/src/HAL/NATIVE_SIM/inc/SanityCheck.h b/Marlin/src/HAL/NATIVE_SIM/inc/SanityCheck.h new file mode 100644 index 000000000000..2d7bef23a36d --- /dev/null +++ b/Marlin/src/HAL/NATIVE_SIM/inc/SanityCheck.h @@ -0,0 +1,43 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2021 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + * + */ +#pragma once + +/** + * Test X86_64-specific configuration values for errors at compile-time. + */ + +// Emulating RAMPS +#if ENABLED(SPINDLE_LASER_USE_PWM) && !(SPINDLE_LASER_PWM_PIN == 4 || SPINDLE_LASER_PWM_PIN == 6 || SPINDLE_LASER_PWM_PIN == 11) + #error "SPINDLE_LASER_PWM_PIN must use SERVO0, SERVO1 or SERVO3 connector" +#endif + +#if ENABLED(FAST_PWM_FAN) || SPINDLE_LASER_FREQUENCY + #error "Features requiring Hardware PWM (FAST_PWM_FAN, SPINDLE_LASER_FREQUENCY) are not yet supported on LINUX." +#endif + +#if HAS_TMC_SW_SERIAL + #error "TMC220x Software Serial is not supported on LINUX." +#endif + +#if ENABLED(POSTMORTEM_DEBUGGING) + #error "POSTMORTEM_DEBUGGING is not yet supported on LINUX." +#endif diff --git a/Marlin/src/HAL/NATIVE_SIM/pinsDebug.h b/Marlin/src/HAL/NATIVE_SIM/pinsDebug.h new file mode 100644 index 000000000000..aa90eb39a330 --- /dev/null +++ b/Marlin/src/HAL/NATIVE_SIM/pinsDebug.h @@ -0,0 +1,61 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2021 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + * + */ + +/** + * Support routines for X86_64 + */ +#pragma once + +/** + * Translation of routines & variables used by pinsDebug.h + */ + +#define NUMBER_PINS_TOTAL NUM_DIGITAL_PINS +#define pwm_details(pin) pin = pin // do nothing // print PWM details +#define pwm_status(pin) false //Print a pin's PWM status. Return true if it's currently a PWM pin. +#define IS_ANALOG(P) (DIGITAL_PIN_TO_ANALOG_PIN(P) >= 0 ? 1 : 0) +#define digitalRead_mod(p) digitalRead(p) +#define PRINT_PORT(p) +#define GET_ARRAY_PIN(p) pin_array[p].pin +#define PRINT_ARRAY_NAME(x) do{ sprintf_P(buffer, PSTR("%-" STRINGIFY(MAX_NAME_LENGTH) "s"), pin_array[x].name); SERIAL_ECHO(buffer); }while(0) +#define PRINT_PIN(p) do{ sprintf_P(buffer, PSTR("%3d "), p); SERIAL_ECHO(buffer); }while(0) +#define PRINT_PIN_ANALOG(p) do{ sprintf_P(buffer, PSTR(" (A%2d) "), DIGITAL_PIN_TO_ANALOG_PIN(pin)); SERIAL_ECHO(buffer); }while(0) +#define MULTI_NAME_PAD 16 // space needed to be pretty if not first name assigned to a pin + +// active ADC function/mode/code values for PINSEL registers +inline constexpr int8_t ADC_pin_mode(pin_t pin) { + return (-1); +} + +inline int8_t get_pin_mode(pin_t pin) { + if (!VALID_PIN(pin)) return -1; + return 0; +} + +inline bool GET_PINMODE(pin_t pin) { + int8_t pin_mode = get_pin_mode(pin); + if (pin_mode == -1 || pin_mode == ADC_pin_mode(pin)) // found an invalid pin or active analog pin + return false; + + return (Gpio::getMode(pin) != 0); //input/output state +} + +inline bool GET_ARRAY_IS_DIGITAL(pin_t pin) { + return (!IS_ANALOG(pin) || get_pin_mode(pin) != ADC_pin_mode(pin)); +} diff --git a/Marlin/src/HAL/NATIVE_SIM/servo_private.h b/Marlin/src/HAL/NATIVE_SIM/servo_private.h new file mode 100644 index 000000000000..06be1893f6eb --- /dev/null +++ b/Marlin/src/HAL/NATIVE_SIM/servo_private.h @@ -0,0 +1,80 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2021 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + * + */ +#pragma once + +/** + * servo.h - Interrupt driven Servo library for Arduino using 16 bit timers- Version 2 + * Copyright (c) 2009 Michael Margolis. All right reserved. + * + * This library is free software; you can redistribute it and/or + * modify it under the terms of the GNU Lesser General Public + * License as published by the Free Software Foundation; either + * version 2.1 of the License, or (at your option) any later version. + * + * This library is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public + * License along with this library; if not, write to the Free Software + * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA + */ + +/** + * Based on "servo.h - Interrupt driven Servo library for Arduino using 16 bit timers - + * Version 2 Copyright (c) 2009 Michael Margolis. All right reserved. + * + * The only modification was to update/delete macros to match the LPC176x. + * + */ + +#include + +// Macros +//values in microseconds +#define MIN_PULSE_WIDTH 544 // the shortest pulse sent to a servo +#define MAX_PULSE_WIDTH 2400 // the longest pulse sent to a servo +#define DEFAULT_PULSE_WIDTH 1500 // default pulse width when servo is attached +#define REFRESH_INTERVAL 20000 // minimum time to refresh servos in microseconds + +#define MAX_SERVOS 4 + +#define INVALID_SERVO 255 // flag indicating an invalid servo index + + +// Types + +typedef struct { + uint8_t nbr : 8 ; // a pin number from 0 to 254 (255 signals invalid pin) + uint8_t isActive : 1 ; // true if this channel is enabled, pin not pulsed if false +} ServoPin_t; + +typedef struct { + ServoPin_t Pin; + unsigned int pulse_width; // pulse width in microseconds +} ServoInfo_t; + +// Global variables + +extern uint8_t ServoCount; +extern ServoInfo_t servo_info[MAX_SERVOS]; diff --git a/Marlin/src/HAL/NATIVE_SIM/spi_pins.h b/Marlin/src/HAL/NATIVE_SIM/spi_pins.h new file mode 100644 index 000000000000..a5138e0ccbe8 --- /dev/null +++ b/Marlin/src/HAL/NATIVE_SIM/spi_pins.h @@ -0,0 +1,55 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2021 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + * + */ +#pragma once + +#include "../../core/macros.h" +#include "../../inc/MarlinConfigPre.h" + +#if BOTH(HAS_MARLINUI_U8GLIB, SDSUPPORT) && (LCD_PINS_D4 == SD_SCK_PIN || LCD_PINS_ENABLE == SD_MOSI_PIN || DOGLCD_SCK == SD_SCK_PIN || DOGLCD_MOSI == SD_MOSI_PIN) + #define LPC_SOFTWARE_SPI // If the SD card and LCD adapter share the same SPI pins, then software SPI is currently + // needed due to the speed and mode required for communicating with each device being different. + // This requirement can be removed if the SPI access to these devices is updated to use + // spiBeginTransaction. +#endif + +// Onboard SD +//#define SD_SCK_PIN P0_07 +//#define SD_MISO_PIN P0_08 +//#define SD_MOSI_PIN P0_09 +//#define SD_SS_PIN P0_06 + +// External SD +#ifndef SD_SCK_PIN + #define SD_SCK_PIN 50 +#endif +#ifndef SD_MISO_PIN + #define SD_MISO_PIN 51 +#endif +#ifndef SD_MOSI_PIN + #define SD_MOSI_PIN 52 +#endif +#ifndef SD_SS_PIN + #define SD_SS_PIN 53 +#endif +#ifndef SDSS + #define SDSS SD_SS_PIN +#endif diff --git a/Marlin/src/HAL/NATIVE_SIM/tft/tft_spi.h b/Marlin/src/HAL/NATIVE_SIM/tft/tft_spi.h new file mode 100644 index 000000000000..b3e622f19ac4 --- /dev/null +++ b/Marlin/src/HAL/NATIVE_SIM/tft/tft_spi.h @@ -0,0 +1,64 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2021 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + * + */ +#pragma once + +#include "../../../inc/MarlinConfig.h" + +#ifndef LCD_READ_ID + #define LCD_READ_ID 0x04 // Read display identification information (0xD3 on ILI9341) +#endif +#ifndef LCD_READ_ID4 + #define LCD_READ_ID4 0xD3 // Read display identification information (0xD3 on ILI9341) +#endif + +#define DATASIZE_8BIT 8 +#define DATASIZE_16BIT 16 +#define TFT_IO_DRIVER TFT_SPI + +#define DMA_MINC_ENABLE 1 +#define DMA_MINC_DISABLE 0 + +class TFT_SPI { +private: + static uint32_t ReadID(uint16_t Reg); + static void Transmit(uint16_t Data); + static void TransmitDMA(uint32_t MemoryIncrease, uint16_t *Data, uint16_t Count); + +public: + // static SPIClass SPIx; + + static void Init(); + static uint32_t GetID(); + static bool isBusy(); + static void Abort(); + + static void DataTransferBegin(uint16_t DataWidth = DATASIZE_16BIT); + static void DataTransferEnd(); + static void DataTransferAbort(); + + static void WriteData(uint16_t Data); + static void WriteReg(uint16_t Reg); + + static void WriteSequence(uint16_t *Data, uint16_t Count); + // static void WriteMultiple(uint16_t Color, uint16_t Count); + static void WriteMultiple(uint16_t Color, uint32_t Count); +}; diff --git a/Marlin/src/HAL/NATIVE_SIM/tft/xpt2046.h b/Marlin/src/HAL/NATIVE_SIM/tft/xpt2046.h new file mode 100644 index 000000000000..b131853643a8 --- /dev/null +++ b/Marlin/src/HAL/NATIVE_SIM/tft/xpt2046.h @@ -0,0 +1,80 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2021 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + * + */ +#pragma once + +#include "../../../inc/MarlinConfig.h" + +#if ENABLED(TOUCH_BUTTONS_HW_SPI) + #include +#endif + +#ifndef TOUCH_MISO_PIN + #define TOUCH_MISO_PIN SD_MISO_PIN +#endif +#ifndef TOUCH_MOSI_PIN + #define TOUCH_MOSI_PIN SD_MOSI_PIN +#endif +#ifndef TOUCH_SCK_PIN + #define TOUCH_SCK_PIN SD_SCK_PIN +#endif +#ifndef TOUCH_CS_PIN + #define TOUCH_CS_PIN SD_SS_PIN +#endif +#ifndef TOUCH_INT_PIN + #define TOUCH_INT_PIN -1 +#endif + +#define XPT2046_DFR_MODE 0x00 +#define XPT2046_SER_MODE 0x04 +#define XPT2046_CONTROL 0x80 + +enum XPTCoordinate : uint8_t { + XPT2046_X = 0x10 | XPT2046_CONTROL | XPT2046_DFR_MODE, + XPT2046_Y = 0x50 | XPT2046_CONTROL | XPT2046_DFR_MODE, + XPT2046_Z1 = 0x30 | XPT2046_CONTROL | XPT2046_DFR_MODE, + XPT2046_Z2 = 0x40 | XPT2046_CONTROL | XPT2046_DFR_MODE, +}; + +#if !defined(XPT2046_Z1_THRESHOLD) + #define XPT2046_Z1_THRESHOLD 10 +#endif + +class XPT2046 { +private: + static bool isBusy() { return false; } + + static uint16_t getRawData(const XPTCoordinate coordinate); + static bool isTouched(); + + static void DataTransferBegin(); + static void DataTransferEnd(); + #if ENABLED(TOUCH_BUTTONS_HW_SPI) + static uint16_t HardwareIO(uint16_t data); + #endif + static uint16_t SoftwareIO(uint16_t data); + static uint16_t IO(uint16_t data = 0); + +public: + #if ENABLED(TOUCH_BUTTONS_HW_SPI) + static SPIClass SPIx; + #endif + + static void Init(); + static bool getRawPoint(int16_t *x, int16_t *y); +}; diff --git a/Marlin/src/HAL/NATIVE_SIM/timers.h b/Marlin/src/HAL/NATIVE_SIM/timers.h new file mode 100644 index 000000000000..be38d583b686 --- /dev/null +++ b/Marlin/src/HAL/NATIVE_SIM/timers.h @@ -0,0 +1,91 @@ +/** + * Marlin 3D Printer Firmware + * + * Copyright (c) 2021 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * Copyright (c) 2016 Bob Cousins bobcousins42@googlemail.com + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + * + */ +#pragma once + +/** + * HAL timers for Linux X86_64 + */ + +#include + +// ------------------------ +// Defines +// ------------------------ + +#define FORCE_INLINE __attribute__((always_inline)) inline + +typedef uint64_t hal_timer_t; +#define HAL_TIMER_TYPE_MAX 0xFFFFFFFFFFFFFFFF + +#define HAL_TIMER_RATE ((SystemCoreClock) / 4) // frequency of timers peripherals + +#ifndef MF_TIMER_STEP + #define MF_TIMER_STEP 0 // Timer Index for Stepper +#endif +#ifndef MF_TIMER_PULSE + #define MF_TIMER_PULSE MF_TIMER_STEP +#endif +#ifndef MF_TIMER_TEMP + #define MF_TIMER_TEMP 1 // Timer Index for Temperature +#endif +#ifndef MF_TIMER_SYSTICK + #define MF_TIMER_SYSTICK 2 // Timer Index for Systick +#endif +#define SYSTICK_TIMER_FREQUENCY 1000 + +#define TEMP_TIMER_RATE 1000000 +#define TEMP_TIMER_FREQUENCY 1000 // temperature interrupt frequency + +#define STEPPER_TIMER_RATE HAL_TIMER_RATE // frequency of stepper timer (HAL_TIMER_RATE / STEPPER_TIMER_PRESCALE) +#define STEPPER_TIMER_TICKS_PER_US ((STEPPER_TIMER_RATE) / 1000000) // stepper timer ticks per µs +#define STEPPER_TIMER_PRESCALE (CYCLES_PER_MICROSECOND / STEPPER_TIMER_TICKS_PER_US) + +#define PULSE_TIMER_RATE STEPPER_TIMER_RATE // frequency of pulse timer +#define PULSE_TIMER_PRESCALE STEPPER_TIMER_PRESCALE +#define PULSE_TIMER_TICKS_PER_US STEPPER_TIMER_TICKS_PER_US + +#define ENABLE_STEPPER_DRIVER_INTERRUPT() HAL_timer_enable_interrupt(MF_TIMER_STEP) +#define DISABLE_STEPPER_DRIVER_INTERRUPT() HAL_timer_disable_interrupt(MF_TIMER_STEP) +#define STEPPER_ISR_ENABLED() HAL_timer_interrupt_enabled(MF_TIMER_STEP) + +#define ENABLE_TEMPERATURE_INTERRUPT() HAL_timer_enable_interrupt(MF_TIMER_TEMP) +#define DISABLE_TEMPERATURE_INTERRUPT() HAL_timer_disable_interrupt(MF_TIMER_TEMP) + +#ifndef HAL_STEP_TIMER_ISR + #define HAL_STEP_TIMER_ISR() extern "C" void TIMER0_IRQHandler() +#endif +#ifndef HAL_TEMP_TIMER_ISR + #define HAL_TEMP_TIMER_ISR() extern "C" void TIMER1_IRQHandler() +#endif + +void HAL_timer_init(); +void HAL_timer_start(const uint8_t timer_num, const uint32_t frequency); + +void HAL_timer_set_compare(const uint8_t timer_num, const hal_timer_t compare); +hal_timer_t HAL_timer_get_compare(const uint8_t timer_num); +hal_timer_t HAL_timer_get_count(const uint8_t timer_num); + +void HAL_timer_enable_interrupt(const uint8_t timer_num); +void HAL_timer_disable_interrupt(const uint8_t timer_num); +bool HAL_timer_interrupt_enabled(const uint8_t timer_num); + +#define HAL_timer_isr_prologue(T) NOOP +#define HAL_timer_isr_epilogue(T) NOOP diff --git a/Marlin/src/HAL/NATIVE_SIM/u8g/LCD_I2C_routines.cpp b/Marlin/src/HAL/NATIVE_SIM/u8g/LCD_I2C_routines.cpp new file mode 100644 index 000000000000..745454394aae --- /dev/null +++ b/Marlin/src/HAL/NATIVE_SIM/u8g/LCD_I2C_routines.cpp @@ -0,0 +1,52 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2021 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + * + */ + +// adapted from I2C/master/master.c example +// https://www-users.cs.york.ac.uk/~pcc/MCP/HAPR-Course-web/CMSIS/examples/html/master_8c_source.html + +#ifdef __PLAT_NATIVE_SIM__ + +#include + +#ifdef __cplusplus + extern "C" { +#endif + +uint8_t u8g_i2c_start(const uint8_t sla) { + return 1; +} + +void u8g_i2c_init(const uint8_t clock_option) { +} + +uint8_t u8g_i2c_send_byte(uint8_t data) { + return 1; +} + +void u8g_i2c_stop() { +} + +#ifdef __cplusplus + } +#endif + +#endif // __PLAT_NATIVE_SIM__ diff --git a/Marlin/src/HAL/NATIVE_SIM/u8g/LCD_I2C_routines.h b/Marlin/src/HAL/NATIVE_SIM/u8g/LCD_I2C_routines.h new file mode 100644 index 000000000000..6d5f91d3ba45 --- /dev/null +++ b/Marlin/src/HAL/NATIVE_SIM/u8g/LCD_I2C_routines.h @@ -0,0 +1,37 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2021 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + * + */ +#pragma once + +#ifdef __cplusplus + extern "C" { +#endif + +void u8g_i2c_init(const uint8_t clock_options); +//uint8_t u8g_i2c_wait(uint8_t mask, uint8_t pos); +uint8_t u8g_i2c_start(uint8_t sla); +uint8_t u8g_i2c_send_byte(uint8_t data); +void u8g_i2c_stop(); + +#ifdef __cplusplus + } +#endif + diff --git a/Marlin/src/HAL/NATIVE_SIM/u8g/LCD_defines.h b/Marlin/src/HAL/NATIVE_SIM/u8g/LCD_defines.h new file mode 100644 index 000000000000..44ffbfeb90e5 --- /dev/null +++ b/Marlin/src/HAL/NATIVE_SIM/u8g/LCD_defines.h @@ -0,0 +1,44 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2021 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + * + */ +#pragma once + +void usleep(uint64_t microsec); +// The following are optional depending on the platform. + +// definitions of HAL specific com and device drivers. +uint8_t u8g_com_sw_spi_fn(u8g_t *u8g, uint8_t msg, uint8_t arg_val, void *arg_ptr); +uint8_t u8g_com_ST7920_sw_spi_fn(u8g_t *u8g, uint8_t msg, uint8_t arg_val, void *arg_ptr); + +// connect U8g com generic com names to the desired driver +#define U8G_COM_SW_SPI u8g_com_sw_spi_fn +#define U8G_COM_ST7920_SW_SPI u8g_com_ST7920_sw_spi_fn + +// let these default for now +#define U8G_COM_HW_SPI u8g_com_null_fn +#define U8G_COM_ST7920_HW_SPI u8g_com_null_fn +#define U8G_COM_SSD_I2C u8g_com_null_fn +#define U8G_COM_PARALLEL u8g_com_null_fn +#define U8G_COM_T6963 u8g_com_null_fn +#define U8G_COM_FAST_PARALLEL u8g_com_null_fn +#define U8G_COM_UC_I2C u8g_com_null_fn + + diff --git a/Marlin/src/HAL/NATIVE_SIM/u8g/LCD_delay.h b/Marlin/src/HAL/NATIVE_SIM/u8g/LCD_delay.h new file mode 100644 index 000000000000..297361cd448f --- /dev/null +++ b/Marlin/src/HAL/NATIVE_SIM/u8g/LCD_delay.h @@ -0,0 +1,43 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2021 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + * + */ +#pragma once + +/** + * LCD delay routines - used by all the drivers. + * + * These are based on the LPC1768 routines. + * + * Couldn't just call exact copies because the overhead + * results in a one microsecond delay taking about 4µS. + */ + +#ifdef __cplusplus + extern "C" { +#endif + +void U8g_delay(int msec); +void u8g_MicroDelay(); +void u8g_10MicroDelay(); + +#ifdef __cplusplus + } +#endif diff --git a/Marlin/src/HAL/NATIVE_SIM/u8g/LCD_pin_routines.cpp b/Marlin/src/HAL/NATIVE_SIM/u8g/LCD_pin_routines.cpp new file mode 100644 index 000000000000..3b5acc1656cd --- /dev/null +++ b/Marlin/src/HAL/NATIVE_SIM/u8g/LCD_pin_routines.cpp @@ -0,0 +1,52 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2021 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + * + */ + +/** + * Low level pin manipulation routines - used by all the drivers. + * + * These are based on the LPC1768 pinMode, digitalRead & digitalWrite routines. + * + * Couldn't just call exact copies because the overhead killed the LCD update speed + * With an intermediate level the softspi was running in the 10-20kHz range which + * resulted in using about about 25% of the CPU's time. + */ + +#ifdef __PLAT_NATIVE_SIM__ + +#include "../fastio.h" +#include "LCD_pin_routines.h" + +#ifdef __cplusplus + extern "C" { +#endif +void u8g_SetPinOutput(uint8_t internal_pin_number){SET_DIR_OUTPUT(internal_pin_number);} +void u8g_SetPinInput(uint8_t internal_pin_number){SET_DIR_INPUT(internal_pin_number);} +void u8g_SetPinLevel(uint8_t pin, uint8_t pin_status){WRITE_PIN(pin, pin_status);} +uint8_t u8g_GetPinLevel(uint8_t pin){return READ_PIN(pin);} +void usleep(uint64_t microsec){ +assert(false); // why we here? +} +#ifdef __cplusplus + } +#endif + +#endif // __PLAT_NATIVE_SIM__ diff --git a/Marlin/src/HAL/NATIVE_SIM/u8g/LCD_pin_routines.h b/Marlin/src/HAL/NATIVE_SIM/u8g/LCD_pin_routines.h new file mode 100644 index 000000000000..c27c84e8c398 --- /dev/null +++ b/Marlin/src/HAL/NATIVE_SIM/u8g/LCD_pin_routines.h @@ -0,0 +1,46 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2021 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + * + */ +#pragma once + +/** + * Low level pin manipulation routines - used by all the drivers. + * + * These are based on the LPC1768 pinMode, digitalRead & digitalWrite routines. + * + * Couldn't just call exact copies because the overhead killed the LCD update speed + * With an intermediate level the softspi was running in the 10-20kHz range which + * resulted in using about about 25% of the CPU's time. + */ + + +#ifdef __cplusplus + extern "C" { +#endif + +void u8g_SetPinOutput(uint8_t internal_pin_number); +void u8g_SetPinInput(uint8_t internal_pin_number); +void u8g_SetPinLevel(uint8_t pin, uint8_t pin_status); +uint8_t u8g_GetPinLevel(uint8_t pin); + +#ifdef __cplusplus + } +#endif diff --git a/Marlin/src/HAL/NATIVE_SIM/u8g/u8g_com_st7920_sw_spi.cpp b/Marlin/src/HAL/NATIVE_SIM/u8g/u8g_com_st7920_sw_spi.cpp new file mode 100644 index 000000000000..c384cdd75185 --- /dev/null +++ b/Marlin/src/HAL/NATIVE_SIM/u8g/u8g_com_st7920_sw_spi.cpp @@ -0,0 +1,171 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2021 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + * + */ + +/** + * Based on u8g_com_st7920_hw_spi.c + * + * Universal 8bit Graphics Library + * + * Copyright (c) 2011, olikraus@gmail.com + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright notice, this list + * of conditions and the following disclaimer. + * + * * Redistributions in binary form must reproduce the above copyright notice, this + * list of conditions and the following disclaimer in the documentation and/or other + * materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND + * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF + * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT + * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, + * STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF + * ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +#ifdef __PLAT_NATIVE_SIM__ + +#include "../../../inc/MarlinConfig.h" + +#if IS_U8GLIB_ST7920 + +#include +#include "../../shared/Delay.h" + +#undef SPI_SPEED +#define SPI_SPEED 6 +#define SPI_DELAY_CYCLES (1 + SPI_SPEED * 10) + +static pin_t SCK_pin_ST7920_HAL, MOSI_pin_ST7920_HAL_HAL; +static uint8_t SPI_speed = 0; + +static uint8_t swSpiTransfer(uint8_t b, const uint8_t spi_speed, const pin_t sck_pin, const pin_t miso_pin, const pin_t mosi_pin) { + for (uint8_t i = 0; i < 8; i++) { + WRITE_PIN(mosi_pin, !!(b & 0x80)); + DELAY_CYCLES(SPI_SPEED); + WRITE_PIN(sck_pin, HIGH); + DELAY_CYCLES(SPI_SPEED); + b <<= 1; + if (miso_pin >= 0 && READ_PIN(miso_pin)) b |= 1; + WRITE_PIN(sck_pin, LOW); + DELAY_CYCLES(SPI_SPEED); + } + return b; +} + +static uint8_t swSpiInit(const uint8_t spiRate, const pin_t sck_pin, const pin_t mosi_pin) { + WRITE_PIN(mosi_pin, HIGH); + WRITE_PIN(sck_pin, LOW); + return spiRate; +} + +static void u8g_com_st7920_write_byte_sw_spi(uint8_t rs, uint8_t val) { + static uint8_t rs_last_state = 255; + if (rs != rs_last_state) { + // Transfer Data (FA) or Command (F8) + swSpiTransfer(rs ? 0x0FA : 0x0F8, SPI_speed, SCK_pin_ST7920_HAL, -1, MOSI_pin_ST7920_HAL_HAL); + rs_last_state = rs; + DELAY_US(40); // Give the controller time to process the data: 20 is bad, 30 is OK, 40 is safe + } + swSpiTransfer(val & 0x0F0, SPI_speed, SCK_pin_ST7920_HAL, -1, MOSI_pin_ST7920_HAL_HAL); + swSpiTransfer(val << 4, SPI_speed, SCK_pin_ST7920_HAL, -1, MOSI_pin_ST7920_HAL_HAL); +} +#ifdef __cplusplus + extern "C" { +#endif + +uint8_t u8g_com_ST7920_sw_spi_fn(u8g_t *u8g, uint8_t msg, uint8_t arg_val, void *arg_ptr) { + switch (msg) { + case U8G_COM_MSG_INIT: + SCK_pin_ST7920_HAL = u8g->pin_list[U8G_PI_SCK]; + MOSI_pin_ST7920_HAL_HAL = u8g->pin_list[U8G_PI_MOSI]; + + u8g_SetPIOutput(u8g, U8G_PI_CS); + u8g_SetPIOutput(u8g, U8G_PI_SCK); + u8g_SetPIOutput(u8g, U8G_PI_MOSI); + u8g_Delay(5); + + SPI_speed = swSpiInit(SPI_SPEED, SCK_pin_ST7920_HAL, MOSI_pin_ST7920_HAL_HAL); + + u8g_SetPILevel(u8g, U8G_PI_CS, 0); + u8g_SetPILevel(u8g, U8G_PI_SCK, 0); + u8g_SetPILevel(u8g, U8G_PI_MOSI, 0); + + u8g->pin_list[U8G_PI_A0_STATE] = 0; /* initial RS state: command mode */ + break; + + case U8G_COM_MSG_STOP: + break; + + case U8G_COM_MSG_RESET: + if (U8G_PIN_NONE != u8g->pin_list[U8G_PI_RESET]) u8g_SetPILevel(u8g, U8G_PI_RESET, arg_val); + break; + + case U8G_COM_MSG_ADDRESS: /* define cmd (arg_val = 0) or data mode (arg_val = 1) */ + u8g->pin_list[U8G_PI_A0_STATE] = arg_val; + break; + + case U8G_COM_MSG_CHIP_SELECT: + if (U8G_PIN_NONE != u8g->pin_list[U8G_PI_CS]) u8g_SetPILevel(u8g, U8G_PI_CS, arg_val); //note: the st7920 has an active high chip select + break; + + case U8G_COM_MSG_WRITE_BYTE: + u8g_com_st7920_write_byte_sw_spi(u8g->pin_list[U8G_PI_A0_STATE], arg_val); + break; + + case U8G_COM_MSG_WRITE_SEQ: { + uint8_t *ptr = (uint8_t*) arg_ptr; + while (arg_val > 0) { + u8g_com_st7920_write_byte_sw_spi(u8g->pin_list[U8G_PI_A0_STATE], *ptr++); + arg_val--; + } + } + break; + + case U8G_COM_MSG_WRITE_SEQ_P: { + uint8_t *ptr = (uint8_t*) arg_ptr; + while (arg_val > 0) { + u8g_com_st7920_write_byte_sw_spi(u8g->pin_list[U8G_PI_A0_STATE], *ptr++); + arg_val--; + } + } + break; + } + return 1; +} +#ifdef __cplusplus + } +#endif + +#endif // IS_U8GLIB_ST7920 +#endif // TARGET_LPC1768 diff --git a/Marlin/src/HAL/NATIVE_SIM/u8g/u8g_com_sw_spi.cpp b/Marlin/src/HAL/NATIVE_SIM/u8g/u8g_com_sw_spi.cpp new file mode 100644 index 000000000000..7be84580b133 --- /dev/null +++ b/Marlin/src/HAL/NATIVE_SIM/u8g/u8g_com_sw_spi.cpp @@ -0,0 +1,218 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2021 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + * + */ + +/** + * Based on u8g_com_std_sw_spi.c + * + * Universal 8bit Graphics Library + * + * Copyright (c) 2015, olikraus@gmail.com + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright notice, this list + * of conditions and the following disclaimer. + * + * * Redistributions in binary form must reproduce the above copyright notice, this + * list of conditions and the following disclaimer in the documentation and/or other + * materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND + * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF + * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT + * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, + * STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF + * ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +#ifdef __PLAT_NATIVE_SIM__ + +#include "../../../inc/MarlinConfig.h" + +#if HAS_MARLINUI_U8GLIB && !IS_U8GLIB_ST7920 + +#undef SPI_SPEED +#define SPI_SPEED 2 // About 2 MHz + +#include +#include + +#ifdef __cplusplus + extern "C" { +#endif + +uint8_t swSpiTransfer_mode_0(uint8_t b, const uint8_t spi_speed, const pin_t sck_pin, const pin_t miso_pin, const pin_t mosi_pin ) { + LOOP_L_N(i, 8) { + if (spi_speed == 0) { + WRITE_PIN(mosi_pin, !!(b & 0x80)); + WRITE_PIN(sck_pin, HIGH); + b <<= 1; + if (miso_pin >= 0 && READ_PIN(miso_pin)) b |= 1; + WRITE_PIN(sck_pin, LOW); + } + else { + const uint8_t state = (b & 0x80) ? HIGH : LOW; + LOOP_L_N(j, spi_speed) + WRITE_PIN(mosi_pin, state); + + LOOP_L_N(j, spi_speed + (miso_pin >= 0 ? 0 : 1)) + WRITE_PIN(sck_pin, HIGH); + + b <<= 1; + if (miso_pin >= 0 && READ_PIN(miso_pin)) b |= 1; + + LOOP_L_N(j, spi_speed) + WRITE_PIN(sck_pin, LOW); + } + } + + return b; +} + +uint8_t swSpiTransfer_mode_3(uint8_t b, const uint8_t spi_speed, const pin_t sck_pin, const pin_t miso_pin, const pin_t mosi_pin ) { + + LOOP_L_N(i, 8) { + const uint8_t state = (b & 0x80) ? HIGH : LOW; + if (spi_speed == 0) { + WRITE_PIN(sck_pin, LOW); + WRITE_PIN(mosi_pin, state); + WRITE_PIN(mosi_pin, state); // need some setup time + WRITE_PIN(sck_pin, HIGH); + } + else { + LOOP_L_N(j, spi_speed + (miso_pin >= 0 ? 0 : 1)) + WRITE_PIN(sck_pin, LOW); + + LOOP_L_N(j, spi_speed) + WRITE_PIN(mosi_pin, state); + + LOOP_L_N(j, spi_speed) + WRITE_PIN(sck_pin, HIGH); + } + b <<= 1; + if (miso_pin >= 0 && READ_PIN(miso_pin)) b |= 1; + } + + return b; +} + +static uint8_t SPI_speed = 0; + +static uint8_t swSpiInit(const uint8_t spi_speed, const uint8_t clk_pin, const uint8_t mosi_pin) { + return spi_speed; +} + +static void u8g_sw_spi_shift_out(uint8_t dataPin, uint8_t clockPin, uint8_t val) { + #if EITHER(FYSETC_MINI_12864, MKS_MINI_12864) + swSpiTransfer_mode_3(val, SPI_speed, clockPin, -1, dataPin); + #else + swSpiTransfer_mode_0(val, SPI_speed, clockPin, -1, dataPin); + #endif +} + +uint8_t u8g_com_sw_spi_fn(u8g_t *u8g, uint8_t msg, uint8_t arg_val, void *arg_ptr) { + switch (msg) { + case U8G_COM_MSG_INIT: + u8g_SetPIOutput(u8g, U8G_PI_SCK); + u8g_SetPIOutput(u8g, U8G_PI_MOSI); + u8g_SetPIOutput(u8g, U8G_PI_CS); + u8g_SetPIOutput(u8g, U8G_PI_A0); + if (U8G_PIN_NONE != u8g->pin_list[U8G_PI_RESET]) u8g_SetPIOutput(u8g, U8G_PI_RESET); + SPI_speed = swSpiInit(SPI_SPEED, u8g->pin_list[U8G_PI_SCK], u8g->pin_list[U8G_PI_MOSI]); + u8g_SetPILevel(u8g, U8G_PI_SCK, 0); + u8g_SetPILevel(u8g, U8G_PI_MOSI, 0); + break; + + case U8G_COM_MSG_STOP: + break; + + case U8G_COM_MSG_RESET: + if (U8G_PIN_NONE != u8g->pin_list[U8G_PI_RESET]) u8g_SetPILevel(u8g, U8G_PI_RESET, arg_val); + break; + + case U8G_COM_MSG_CHIP_SELECT: + #if EITHER(FYSETC_MINI_12864, MKS_MINI_12864) // LCD SPI is running mode 3 while SD card is running mode 0 + if (arg_val) { // SCK idle state needs to be set to the proper idle state before + // the next chip select goes active + u8g_SetPILevel(u8g, U8G_PI_SCK, 1); // Set SCK to mode 3 idle state before CS goes active + u8g_SetPILevel(u8g, U8G_PI_CS, LOW); + } + else { + u8g_SetPILevel(u8g, U8G_PI_CS, HIGH); + u8g_SetPILevel(u8g, U8G_PI_SCK, 0); // Set SCK to mode 0 idle state after CS goes inactive + } + #else + u8g_SetPILevel(u8g, U8G_PI_CS, !arg_val); + #endif + break; + + case U8G_COM_MSG_WRITE_BYTE: + u8g_sw_spi_shift_out(u8g->pin_list[U8G_PI_MOSI], u8g->pin_list[U8G_PI_SCK], arg_val); + break; + + case U8G_COM_MSG_WRITE_SEQ: { + uint8_t *ptr = (uint8_t *)arg_ptr; + while (arg_val > 0) { + u8g_sw_spi_shift_out(u8g->pin_list[U8G_PI_MOSI], u8g->pin_list[U8G_PI_SCK], *ptr++); + arg_val--; + } + } + break; + + case U8G_COM_MSG_WRITE_SEQ_P: { + uint8_t *ptr = (uint8_t *)arg_ptr; + while (arg_val > 0) { + u8g_sw_spi_shift_out(u8g->pin_list[U8G_PI_MOSI], u8g->pin_list[U8G_PI_SCK], u8g_pgm_read(ptr)); + ptr++; + arg_val--; + } + } + break; + + case U8G_COM_MSG_ADDRESS: /* define cmd (arg_val = 0) or data mode (arg_val = 1) */ + u8g_SetPILevel(u8g, U8G_PI_A0, arg_val); + break; + } + return 1; +} + +#ifdef __cplusplus + } +#endif + +#elif NONE(TFT_COLOR_UI, TFT_CLASSIC_UI, TFT_LVGL_UI, HAS_MARLINUI_HD44780) && HAS_MARLINUI_U8GLIB + + #include + uint8_t u8g_com_sw_spi_fn(u8g_t *u8g, uint8_t msg, uint8_t arg_val, void *arg_ptr) {return 0;} + +#endif // HAS_MARLINUI_U8GLIB && !IS_U8GLIB_ST7920 + +#endif // __PLAT_NATIVE_SIM__ diff --git a/Marlin/src/HAL/NATIVE_SIM/watchdog.h b/Marlin/src/HAL/NATIVE_SIM/watchdog.h new file mode 100644 index 000000000000..4e404c3887da --- /dev/null +++ b/Marlin/src/HAL/NATIVE_SIM/watchdog.h @@ -0,0 +1,27 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2021 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + * + */ +#pragma once + +#define WDT_TIMEOUT 4000000 // 4 second timeout + +void watchdog_init(); +void HAL_watchdog_refresh(); diff --git a/Marlin/src/HAL/SAMD51/HAL.cpp b/Marlin/src/HAL/SAMD51/HAL.cpp index 17e89c723ff1..14b6a437dccc 100644 --- a/Marlin/src/HAL/SAMD51/HAL.cpp +++ b/Marlin/src/HAL/SAMD51/HAL.cpp @@ -25,27 +25,23 @@ #include #ifdef ADAFRUIT_GRAND_CENTRAL_M4 - #if ANY_SERIAL_IS(-1) - DefaultSerial MSerial(false, Serial); + #if USING_HW_SERIALUSB + DefaultSerial1 MSerial0(false, Serial); #endif - #if ANY_SERIAL_IS(0) - DefaultSerial1 MSerial1(false, Serial1); + #if USING_HW_SERIAL0 + DefaultSerial2 MSerial1(false, Serial1); #endif - #if ANY_SERIAL_IS(1) - DefaultSerial2 MSerial2(false, Serial2); + #if USING_HW_SERIAL1 + DefaultSerial3 MSerial2(false, Serial2); #endif - #if ANY_SERIAL_IS(2) - DefaultSerial3 MSerial3(false, Serial3); + #if USING_HW_SERIAL2 + DefaultSerial4 MSerial3(false, Serial3); #endif - #if ANY_SERIAL_IS(3) - DefaultSerial4 MSerial4(false, Serial4); + #if USING_HW_SERIAL3 + DefaultSerial5 MSerial4(false, Serial4); #endif #endif -// ------------------------ -// Local defines -// ------------------------ - #define GET_TEMP_0_ADC() TERN(HAS_TEMP_ADC_0, PIN_TO_ADC(TEMP_0_PIN), -1) #define GET_TEMP_1_ADC() TERN(HAS_TEMP_ADC_1, PIN_TO_ADC(TEMP_1_PIN), -1) #define GET_TEMP_2_ADC() TERN(HAS_TEMP_ADC_2, PIN_TO_ADC(TEMP_2_PIN), -1) @@ -54,20 +50,28 @@ #define GET_TEMP_5_ADC() TERN(HAS_TEMP_ADC_5, PIN_TO_ADC(TEMP_5_PIN), -1) #define GET_TEMP_6_ADC() TERN(HAS_TEMP_ADC_6, PIN_TO_ADC(TEMP_6_PIN), -1) #define GET_TEMP_7_ADC() TERN(HAS_TEMP_ADC_7, PIN_TO_ADC(TEMP_7_PIN), -1) -#define GET_PROBE_ADC() TERN(HAS_TEMP_PROBE, PIN_TO_ADC(TEMP_PROBE_PIN), -1) #define GET_BED_ADC() TERN(HAS_TEMP_ADC_BED, PIN_TO_ADC(TEMP_BED_PIN), -1) #define GET_CHAMBER_ADC() TERN(HAS_TEMP_ADC_CHAMBER, PIN_TO_ADC(TEMP_CHAMBER_PIN), -1) +#define GET_PROBE_ADC() TERN(HAS_TEMP_ADC_PROBE, PIN_TO_ADC(TEMP_PROBE_PIN), -1) +#define GET_COOLER_ADC() TERN(HAS_TEMP_ADC_COOLER, PIN_TO_ADC(TEMP_COOLER_PIN), -1) +#define GET_BOARD_ADC() TERN(HAS_TEMP_ADC_BOARD, PIN_TO_ADC(TEMP_BOARD_PIN), -1) #define GET_FILAMENT_WIDTH_ADC() TERN(FILAMENT_WIDTH_SENSOR, PIN_TO_ADC(FILWIDTH_PIN), -1) #define GET_BUTTONS_ADC() TERN(HAS_ADC_BUTTONS, PIN_TO_ADC(ADC_KEYPAD_PIN), -1) +#define GET_JOY_ADC_X() TERN(HAS_JOY_ADC_X, PIN_TO_ADC(JOY_X_PIN), -1) +#define GET_JOY_ADC_Y() TERN(HAS_JOY_ADC_Y, PIN_TO_ADC(JOY_Y_PIN), -1) +#define GET_JOY_ADC_Z() TERN(HAS_JOY_ADC_Z, PIN_TO_ADC(JOY_Z_PIN), -1) #define IS_ADC_REQUIRED(n) ( \ GET_TEMP_0_ADC() == n || GET_TEMP_1_ADC() == n || GET_TEMP_2_ADC() == n || GET_TEMP_3_ADC() == n \ || GET_TEMP_4_ADC() == n || GET_TEMP_5_ADC() == n || GET_TEMP_6_ADC() == n || GET_TEMP_7_ADC() == n \ - || GET_PROBE_ADC() == n \ - || GET_BED_ADC() == n \ - || GET_CHAMBER_ADC() == n \ + || GET_BED_ADC() == n \ + || GET_CHAMBER_ADC() == n \ + || GET_PROBE_ADC() == n \ + || GET_COOLER_ADC() == n \ + || GET_BOARD_ADC() == n \ || GET_FILAMENT_WIDTH_ADC() == n \ - || GET_BUTTONS_ADC() == n \ + || GET_BUTTONS_ADC() == n \ + || GET_JOY_ADC_X() == n || GET_JOY_ADC_Y() == n || GET_JOY_ADC_Z() == n \ ) #if IS_ADC_REQUIRED(0) @@ -87,6 +91,118 @@ #define DMA_IS_REQUIRED 1 #endif +enum ADCIndex { + #if GET_TEMP_0_ADC() == 0 + TEMP_0, + #endif + #if GET_TEMP_1_ADC() == 0 + TEMP_1, + #endif + #if GET_TEMP_2_ADC() == 0 + TEMP_2, + #endif + #if GET_TEMP_3_ADC() == 0 + TEMP_3, + #endif + #if GET_TEMP_4_ADC() == 0 + TEMP_4, + #endif + #if GET_TEMP_5_ADC() == 0 + TEMP_5, + #endif + #if GET_TEMP_6_ADC() == 0 + TEMP_6, + #endif + #if GET_TEMP_7_ADC() == 0 + TEMP_7, + #endif + #if GET_BED_ADC() == 0 + TEMP_BED, + #endif + #if GET_CHAMBER_ADC() == 0 + TEMP_CHAMBER, + #endif + #if GET_PROBE_ADC() == 0 + TEMP_PROBE, + #endif + #if GET_COOLER_ADC() == 0 + TEMP_COOLER, + #endif + #if GET_BOARD_ADC() == 0 + TEMP_BOARD, + #endif + #if GET_FILAMENT_WIDTH_ADC() == 0 + FILWIDTH, + #endif + #if GET_BUTTONS_ADC() == 0 + ADC_KEY, + #endif + #if GET_JOY_ADC_X() == 0 + JOY_X, + #endif + #if GET_JOY_ADC_Y() == 0 + JOY_Y, + #endif + #if GET_JOY_ADC_Z() == 0 + JOY_Z, + #endif + #if GET_TEMP_0_ADC() == 1 + TEMP_0, + #endif + #if GET_TEMP_1_ADC() == 1 + TEMP_1, + #endif + #if GET_TEMP_2_ADC() == 1 + TEMP_2, + #endif + #if GET_TEMP_3_ADC() == 1 + TEMP_3, + #endif + #if GET_TEMP_4_ADC() == 1 + TEMP_4, + #endif + #if GET_TEMP_5_ADC() == 1 + TEMP_5, + #endif + #if GET_TEMP_6_ADC() == 1 + TEMP_6, + #endif + #if GET_TEMP_7_ADC() == 1 + TEMP_7, + #endif + #if GET_BED_ADC() == 1 + TEMP_BED, + #endif + #if GET_CHAMBER_ADC() == 1 + TEMP_CHAMBER, + #endif + #if GET_PROBE_ADC() == 1 + TEMP_PROBE, + #endif + #if GET_COOLER_ADC() == 1 + TEMP_COOLER, + #endif + #if GET_BOARD_ADC() == 1 + TEMP_BOARD, + #endif + #if GET_FILAMENT_WIDTH_ADC() == 1 + FILWIDTH, + #endif + #if GET_BUTTONS_ADC() == 1 + ADC_KEY, + #endif + #if GET_JOY_ADC_X() == 1 + JOY_X, + #endif + #if GET_JOY_ADC_Y() == 1 + JOY_Y, + #endif + #if GET_JOY_ADC_Z() == 1 + JOY_Z, + #endif + ADC_COUNT +}; + // ------------------------ // Types // ------------------------ @@ -96,7 +212,7 @@ // Struct must be 32 bits aligned because of DMA accesses but fields needs to be 8 bits packed typedef struct __attribute__((aligned(4), packed)) { ADC_INPUTCTRL_Type INPUTCTRL; - } HAL_DMA_DAC_Registers; // DMA transfered registers + } HAL_DMA_DAC_Registers; // DMA transferred registers #endif @@ -104,12 +220,10 @@ // Private Variables // ------------------------ -uint16_t HAL_adc_result; - #if ADC_IS_REQUIRED // Pins used by ADC inputs. Order must be ADC0 inputs first then ADC1 - const uint8_t adc_pins[] = { + static constexpr uint8_t adc_pins[ADC_COUNT] = { // ADC0 pins #if GET_TEMP_0_ADC() == 0 TEMP_0_PIN, @@ -135,21 +249,36 @@ uint16_t HAL_adc_result; #if GET_TEMP_7_ADC() == 0 TEMP_7_PIN, #endif - #if GET_PROBE_ADC() == 0 - TEMP_PROBE_PIN, - #endif #if GET_BED_ADC() == 0 TEMP_BED_PIN, #endif #if GET_CHAMBER_ADC() == 0 TEMP_CHAMBER_PIN, #endif + #if GET_PROBE_ADC() == 0 + TEMP_PROBE_PIN, + #endif + #if GET_COOLER_ADC() == 0 + TEMP_COOLER_PIN, + #endif + #if GET_BOARD_ADC() == 0 + TEMP_BOARD_PIN, + #endif #if GET_FILAMENT_WIDTH_ADC() == 0 FILWIDTH_PIN, #endif #if GET_BUTTONS_ADC() == 0 ADC_KEYPAD_PIN, #endif + #if GET_JOY_ADC_X() == 0 + JOY_X_PIN, + #endif + #if GET_JOY_ADC_Y() == 0 + JOY_Y_PIN, + #endif + #if GET_JOY_ADC_Z() == 0 + JOY_Z_PIN, + #endif // ADC1 pins #if GET_TEMP_0_ADC() == 1 TEMP_0_PIN, @@ -175,30 +304,44 @@ uint16_t HAL_adc_result; #if GET_TEMP_7_ADC() == 1 TEMP_7_PIN, #endif - #if GET_PROBE_ADC() == 1 - TEMP_PROBE_PIN, - #endif #if GET_BED_ADC() == 1 TEMP_BED_PIN, #endif #if GET_CHAMBER_ADC() == 1 TEMP_CHAMBER_PIN, #endif + #if GET_PROBE_ADC() == 1 + TEMP_PROBE_PIN, + #endif + #if GET_COOLER_ADC() == 1 + TEMP_COOLER_PIN, + #endif + #if GET_BOARD_ADC() == 1 + TEMP_BOARD_PIN, + #endif #if GET_FILAMENT_WIDTH_ADC() == 1 FILWIDTH_PIN, #endif #if GET_BUTTONS_ADC() == 1 ADC_KEYPAD_PIN, #endif + #if GET_JOY_ADC_X() == 1 + JOY_X_PIN, + #endif + #if GET_JOY_ADC_Y() == 1 + JOY_Y_PIN, + #endif + #if GET_JOY_ADC_Z() == 1 + JOY_Z_PIN, + #endif }; - uint16_t HAL_adc_results[COUNT(adc_pins)]; + static uint16_t adc_results[ADC_COUNT]; #if ADC0_IS_REQUIRED - Adafruit_ZeroDMA adc0DMAProgram, - adc0DMARead; + Adafruit_ZeroDMA adc0DMAProgram, adc0DMARead; - const HAL_DMA_DAC_Registers adc0_dma_regs_list[] = { + static constexpr HAL_DMA_DAC_Registers adc0_dma_regs_list[ADC_COUNT] = { #if GET_TEMP_0_ADC() == 0 { PIN_TO_INPUTCTRL(TEMP_0_PIN) }, #endif @@ -223,31 +366,45 @@ uint16_t HAL_adc_result; #if GET_TEMP_7_ADC() == 0 { PIN_TO_INPUTCTRL(TEMP_7_PIN) }, #endif - #if GET_PROBE_ADC() == 0 - { PIN_TO_INPUTCTRL(TEMP_PROBE_PIN) }, - #endif #if GET_BED_ADC() == 0 { PIN_TO_INPUTCTRL(TEMP_BED_PIN) }, #endif #if GET_CHAMBER_ADC() == 0 { PIN_TO_INPUTCTRL(TEMP_CHAMBER_PIN) }, #endif + #if GET_PROBE_ADC() == 0 + { PIN_TO_INPUTCTRL(TEMP_PROBE_PIN) }, + #endif + #if GET_COOLER_ADC() == 0 + { PIN_TO_INPUTCTRL(TEMP_COOLER_PIN) }, + #endif + #if GET_BOARD_ADC() == 0 + { PIN_TO_INPUTCTRL(TEMP_BOARD_PIN) }, + #endif #if GET_FILAMENT_WIDTH_ADC() == 0 { PIN_TO_INPUTCTRL(FILWIDTH_PIN) }, #endif #if GET_BUTTONS_ADC() == 0 { PIN_TO_INPUTCTRL(ADC_KEYPAD_PIN) }, #endif + #if GET_JOY_ADC_X() == 0 + { PIN_TO_INPUTCTRL(JOY_X_PIN) }, + #endif + #if GET_JOY_ADC_Y() == 0 + { PIN_TO_INPUTCTRL(JOY_Y_PIN) }, + #endif + #if GET_JOY_ADC_Z() == 0 + { PIN_TO_INPUTCTRL(JOY_Z_PIN) }, + #endif }; #define ADC0_AINCOUNT COUNT(adc0_dma_regs_list) #endif // ADC0_IS_REQUIRED #if ADC1_IS_REQUIRED - Adafruit_ZeroDMA adc1DMAProgram, - adc1DMARead; + Adafruit_ZeroDMA adc1DMAProgram, adc1DMARead; - const HAL_DMA_DAC_Registers adc1_dma_regs_list[] = { + static constexpr HAL_DMA_DAC_Registers adc1_dma_regs_list[ADC_COUNT] = { #if GET_TEMP_0_ADC() == 1 { PIN_TO_INPUTCTRL(TEMP_0_PIN) }, #endif @@ -272,21 +429,36 @@ uint16_t HAL_adc_result; #if GET_TEMP_7_ADC() == 1 { PIN_TO_INPUTCTRL(TEMP_7_PIN) }, #endif - #if GET_PROBE_ADC() == 1 - { PIN_TO_INPUTCTRL(TEMP_PROBE_PIN) }, - #endif #if GET_BED_ADC() == 1 { PIN_TO_INPUTCTRL(TEMP_BED_PIN) }, #endif #if GET_CHAMBER_ADC() == 1 { PIN_TO_INPUTCTRL(TEMP_CHAMBER_PIN) }, #endif + #if GET_PROBE_ADC() == 1 + { PIN_TO_INPUTCTRL(TEMP_PROBE_PIN) }, + #endif + #if GET_COOLER_ADC() == 1 + { PIN_TO_INPUTCTRL(TEMP_COOLER_PIN) }, + #endif + #if GET_BOARD_ADC() == 1 + { PIN_TO_INPUTCTRL(TEMP_BOARD_PIN) }, + #endif #if GET_FILAMENT_WIDTH_ADC() == 1 { PIN_TO_INPUTCTRL(FILWIDTH_PIN) }, #endif #if GET_BUTTONS_ADC() == 1 { PIN_TO_INPUTCTRL(ADC_KEYPAD_PIN) }, #endif + #if GET_JOY_ADC_X() == 1 + { PIN_TO_INPUTCTRL(JOY_X_PIN) }, + #endif + #if GET_JOY_ADC_Y() == 1 + { PIN_TO_INPUTCTRL(JOY_Y_PIN) }, + #endif + #if GET_JOY_ADC_Z() == 1 + { PIN_TO_INPUTCTRL(JOY_Z_PIN) }, + #endif }; #define ADC1_AINCOUNT COUNT(adc1_dma_regs_list) @@ -298,9 +470,10 @@ uint16_t HAL_adc_result; // Private functions // ------------------------ -#if DMA_IS_REQUIRED +void MarlinHAL::dma_init() { + + #if DMA_IS_REQUIRED - void dma_init() { DmacDescriptor *descriptor; #if ADC0_IS_REQUIRED @@ -329,7 +502,7 @@ uint16_t HAL_adc_result; if (adc0DMARead.allocate() == DMA_STATUS_OK) { adc0DMARead.addDescriptor( (void *)&ADC0->RESULT.reg, // SRC - &HAL_adc_results, // DEST + &adc_results, // DEST ADC0_AINCOUNT, // CNT DMA_BEAT_SIZE_HWORD, false, // SRCINC @@ -366,7 +539,7 @@ uint16_t HAL_adc_result; if (adc1DMARead.allocate() == DMA_STATUS_OK) { adc1DMARead.addDescriptor( (void *)&ADC1->RESULT.reg, // SRC - &HAL_adc_results[ADC0_AINCOUNT], // DEST + &adc_results[ADC0_AINCOUNT], // DEST ADC1_AINCOUNT, // CNT DMA_BEAT_SIZE_HWORD, false, // SRCINC @@ -379,16 +552,16 @@ uint16_t HAL_adc_result; #endif DMAC->PRICTRL0.bit.RRLVLEN0 = true; // Activate round robin for DMA channels required by ADCs - } -#endif // DMA_IS_REQUIRED + #endif // DMA_IS_REQUIRED +} // ------------------------ // Public functions // ------------------------ // HAL initialization task -void HAL_init() { +void MarlinHAL::init() { TERN_(DMA_IS_REQUIRED, dma_init()); #if ENABLED(SDSUPPORT) #if SD_CONNECTION_IS(ONBOARD) && PIN_EXISTS(SD_DETECT) @@ -398,17 +571,9 @@ void HAL_init() { #endif } -// HAL idle task -/* -void HAL_idletask() { -} -*/ - -void HAL_clear_reset_source() { } - #pragma push_macro("WDT") #undef WDT // Required to be able to use '.bit.WDT'. Compiler wrongly replace struct field with WDT define -uint8_t HAL_get_reset_source() { +uint8_t MarlinHAL::get_reset_source() { RSTC_RCAUSE_Type resetCause; resetCause.reg = REG_RSTC_RCAUSE; @@ -422,6 +587,8 @@ uint8_t HAL_get_reset_source() { } #pragma pop_macro("WDT") +void MarlinHAL::reboot() { NVIC_SystemReset(); } + extern "C" { void * _sbrk(int incr); @@ -438,9 +605,11 @@ int freeMemory() { // ADC // ------------------------ -void HAL_adc_init() { +uint16_t MarlinHAL::adc_result; + +void MarlinHAL::adc_init() { #if ADC_IS_REQUIRED - memset(HAL_adc_results, 0xFF, sizeof(HAL_adc_results)); // Fill result with invalid values + memset(adc_results, 0xFF, sizeof(adc_results)); // Fill result with invalid values LOOP_L_N(pi, COUNT(adc_pins)) pinPeripheral(adc_pins[pi], PIO_ANALOG); @@ -475,17 +644,13 @@ void HAL_adc_init() { #endif // ADC_IS_REQUIRED } -void HAL_adc_start_conversion(const uint8_t adc_pin) { +void MarlinHAL::adc_start(const pin_t pin) { #if ADC_IS_REQUIRED - LOOP_L_N(pi, COUNT(adc_pins)) { - if (adc_pin == adc_pins[pi]) { - HAL_adc_result = HAL_adc_results[pi]; - return; - } - } + LOOP_L_N(pi, COUNT(adc_pins)) + if (pin == adc_pins[pi]) { adc_result = adc_results[pi]; return; } #endif - HAL_adc_result = 0xFFFF; + adc_result = 0xFFFF; } #endif // __SAMD51__ diff --git a/Marlin/src/HAL/SAMD51/HAL.h b/Marlin/src/HAL/SAMD51/HAL.h index 7b272af84247..3b09a885a53b 100644 --- a/Marlin/src/HAL/SAMD51/HAL.h +++ b/Marlin/src/HAL/SAMD51/HAL.h @@ -32,58 +32,56 @@ #include "MarlinSerial_AGCM4.h" // Serial ports - typedef ForwardSerial0Type< decltype(Serial) > DefaultSerial; - typedef ForwardSerial0Type< decltype(Serial1) > DefaultSerial1; - typedef ForwardSerial0Type< decltype(Serial2) > DefaultSerial2; - typedef ForwardSerial0Type< decltype(Serial3) > DefaultSerial3; - typedef ForwardSerial0Type< decltype(Serial4) > DefaultSerial4; - extern DefaultSerial MSerial; - extern DefaultSerial1 MSerial1; - extern DefaultSerial2 MSerial2; - extern DefaultSerial3 MSerial3; - extern DefaultSerial4 MSerial4; - - // MYSERIAL0 required before MarlinSerial includes! + typedef ForwardSerial1Class< decltype(Serial) > DefaultSerial1; + typedef ForwardSerial1Class< decltype(Serial1) > DefaultSerial2; + typedef ForwardSerial1Class< decltype(Serial2) > DefaultSerial3; + typedef ForwardSerial1Class< decltype(Serial3) > DefaultSerial4; + typedef ForwardSerial1Class< decltype(Serial4) > DefaultSerial5; + extern DefaultSerial1 MSerial0; + extern DefaultSerial2 MSerial1; + extern DefaultSerial3 MSerial2; + extern DefaultSerial4 MSerial3; + extern DefaultSerial5 MSerial4; #define __MSERIAL(X) MSerial##X #define _MSERIAL(X) __MSERIAL(X) #define MSERIAL(X) _MSERIAL(INCREMENT(X)) #if SERIAL_PORT == -1 - #define MYSERIAL0 MSerial + #define MYSERIAL1 MSerial0 #elif WITHIN(SERIAL_PORT, 0, 3) - #define MYSERIAL0 MSERIAL(SERIAL_PORT) + #define MYSERIAL1 MSERIAL(SERIAL_PORT) #else - #error "SERIAL_PORT must be from -1 to 3. Please update your configuration." + #error "SERIAL_PORT must be from 0 to 3. You can also use -1 if the board supports Native USB." #endif #ifdef SERIAL_PORT_2 #if SERIAL_PORT_2 == -1 - #define MYSERIAL1 MSerial + #define MYSERIAL2 MSerial0 #elif WITHIN(SERIAL_PORT_2, 0, 3) - #define MYSERIAL1 MSERIAL(SERIAL_PORT_2) + #define MYSERIAL2 MSERIAL(SERIAL_PORT_2) #else - #error "SERIAL_PORT_2 must be from -1 to 3. Please update your configuration." + #error "SERIAL_PORT_2 must be from 0 to 3. You can also use -1 if the board supports Native USB." #endif #endif #ifdef MMU2_SERIAL_PORT #if MMU2_SERIAL_PORT == -1 - #define MMU2_SERIAL MSerial + #define MMU2_SERIAL MSerial0 #elif WITHIN(MMU2_SERIAL_PORT, 0, 3) #define MMU2_SERIAL MSERIAL(MMU2_SERIAL_PORT) #else - #error "MMU2_SERIAL_PORT must be from -1 to 3. Please update your configuration." + #error "MMU2_SERIAL_PORT must be from 0 to 3. You can also use -1 if the board supports Native USB." #endif #endif #ifdef LCD_SERIAL_PORT #if LCD_SERIAL_PORT == -1 - #define LCD_SERIAL MSerial + #define LCD_SERIAL MSerial0 #elif WITHIN(LCD_SERIAL_PORT, 0, 3) #define LCD_SERIAL MSERIAL(LCD_SERIAL_PORT) #else - #error "LCD_SERIAL_PORT must be from -1 to 3. Please update your configuration." + #error "LCD_SERIAL_PORT must be from 0 to 3. You can also use -1 if the board supports Native USB." #endif #endif @@ -91,46 +89,30 @@ typedef int8_t pin_t; -#define SHARED_SERVOS HAS_SERVOS -#define HAL_SERVO_LIB Servo +#define SHARED_SERVOS HAS_SERVOS // Use shared/servos.cpp + +class Servo; +typedef Servo hal_servo_t; // // Interrupts // -#define CRITICAL_SECTION_START() uint32_t primask = __get_PRIMASK(); __disable_irq() -#define CRITICAL_SECTION_END() if (!primask) __enable_irq() -#define ISRS_ENABLED() (!__get_PRIMASK()) -#define ENABLE_ISRS() __enable_irq() -#define DISABLE_ISRS() __disable_irq() - -#define cli() __disable_irq() // Disable interrupts -#define sei() __enable_irq() // Enable interrupts - -void HAL_clear_reset_source(); // clear reset reason -uint8_t HAL_get_reset_source(); // get reset reason +#define CRITICAL_SECTION_START() const bool irqon = !__get_PRIMASK(); __disable_irq() +#define CRITICAL_SECTION_END() if (irqon) __enable_irq() -inline void HAL_reboot() {} // reboot the board or restart the bootloader +#define cli() __disable_irq() // Disable interrupts +#define sei() __enable_irq() // Enable interrupts // // ADC // -extern uint16_t HAL_adc_result; // Most recent ADC conversion - -#define HAL_ANALOG_SELECT(pin) - -void HAL_adc_init(); //#define HAL_ADC_FILTERED // Disable Marlin's oversampling. The HAL filters ADC values. #define HAL_ADC_VREF 3.3 #define HAL_ADC_RESOLUTION 10 // ... 12 -#define HAL_START_ADC(pin) HAL_adc_start_conversion(pin) -#define HAL_READ_ADC() HAL_adc_result -#define HAL_ADC_READY() true - -void HAL_adc_start_conversion(const uint8_t adc_pin); // -// Pin Map +// Pin Mapping for M42, M43, M226 // #define GET_PIN_MAP_PIN(index) index #define GET_PIN_MAP_INDEX(pin) pin @@ -139,37 +121,93 @@ void HAL_adc_start_conversion(const uint8_t adc_pin); // // Tone // -void toneInit(); void tone(const pin_t _pin, const unsigned int frequency, const unsigned long duration=0); void noTone(const pin_t _pin); -// Enable hooks into idle and setup for HAL -void HAL_init(); -/* -#define HAL_IDLETASK 1 -void HAL_idletask(); -*/ - -// -// Utility functions -// -FORCE_INLINE void _delay_ms(const int delay_ms) { delay(delay_ms); } +// ------------------------ +// Class Utilities +// ------------------------ +#pragma GCC diagnostic push #if GCC_VERSION <= 50000 - #pragma GCC diagnostic push #pragma GCC diagnostic ignored "-Wunused-function" #endif -int freeMemory(); - -#if GCC_VERSION <= 50000 - #pragma GCC diagnostic pop -#endif - #ifdef __cplusplus extern "C" { #endif + char *dtostrf(double __val, signed char __width, unsigned char __prec, char *__s); + +extern "C" int freeMemory(); + #ifdef __cplusplus } #endif + +#pragma GCC diagnostic pop + +// ------------------------ +// MarlinHAL Class +// ------------------------ + +class MarlinHAL { +public: + + // Earliest possible init, before setup() + MarlinHAL() {} + + static void init(); // Called early in setup() + static void init_board() {} // Called less early in setup() + static void reboot(); // Restart the firmware from 0x0 + + // Interrupts + static bool isr_state() { return !__get_PRIMASK(); } + static void isr_on() { sei(); } + static void isr_off() { cli(); } + + static void delay_ms(const int ms) { delay(ms); } + + // Tasks, called from idle() + static void idletask() {} + + // Reset + static uint8_t get_reset_source(); + static void clear_reset_source() {} + + // Free SRAM + static int freeMemory() { return ::freeMemory(); } + + // + // ADC Methods + // + + static uint16_t adc_result; + + // Called by Temperature::init once at startup + static void adc_init(); + + // Called by Temperature::init for each sensor at startup + static void adc_enable(const uint8_t ch) {} + + // Begin ADC sampling on the given channel + static void adc_start(const pin_t pin); + + // Is the ADC ready for reading? + static bool adc_ready() { return true; } + + // The current value of the ADC register + static uint16_t adc_value() { return adc_result; } + + /** + * Set the PWM duty cycle for the pin to the given value. + * No option to invert the duty cycle [default = false] + * No option to change the scale of the provided value to enable finer PWM duty control [default = 255] + */ + static void set_pwm_duty(const pin_t pin, const uint16_t v, const uint16_t=255, const bool=false) { + analogWrite(pin, v); + } + +private: + static void dma_init(); +}; diff --git a/Marlin/src/HAL/SAMD51/HAL_SPI.cpp b/Marlin/src/HAL/SAMD51/HAL_SPI.cpp index c3acd38237c6..77f4d5ecd513 100644 --- a/Marlin/src/HAL/SAMD51/HAL_SPI.cpp +++ b/Marlin/src/HAL/SAMD51/HAL_SPI.cpp @@ -103,7 +103,7 @@ * @param nbyte Number of bytes to receive. * @return Nothing */ - void spiRead(uint8_t* buf, uint16_t nbyte) { + void spiRead(uint8_t *buf, uint16_t nbyte) { if (nbyte == 0) return; memset(buf, 0xFF, nbyte); sdSPI.beginTransaction(spiConfig); @@ -132,7 +132,7 @@ * * @details Uses DMA */ - void spiSendBlock(uint8_t token, const uint8_t* buf) { + void spiSendBlock(uint8_t token, const uint8_t *buf) { sdSPI.beginTransaction(spiConfig); sdSPI.transfer(token); sdSPI.transfer((uint8_t*)buf, nullptr, 512); diff --git a/Marlin/src/HAL/SAMD51/MarlinSPI.h b/Marlin/src/HAL/SAMD51/MarlinSPI.h new file mode 100644 index 000000000000..0c447ba4cb3d --- /dev/null +++ b/Marlin/src/HAL/SAMD51/MarlinSPI.h @@ -0,0 +1,26 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2021 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + * + */ +#pragma once + +#include + +using MarlinSPI = SPIClass; diff --git a/Marlin/src/HAL/SAMD51/MarlinSerial_AGCM4.cpp b/Marlin/src/HAL/SAMD51/MarlinSerial_AGCM4.cpp index 3f43585cf242..a16ea2f75821 100644 --- a/Marlin/src/HAL/SAMD51/MarlinSerial_AGCM4.cpp +++ b/Marlin/src/HAL/SAMD51/MarlinSerial_AGCM4.cpp @@ -27,7 +27,7 @@ #include "../../inc/MarlinConfig.h" -#if ANY_SERIAL_IS(1) +#if USING_HW_SERIAL1 UartT Serial2(false, &sercom4, PIN_SERIAL2_RX, PIN_SERIAL2_TX, PAD_SERIAL2_RX, PAD_SERIAL2_TX); void SERCOM4_0_Handler() { Serial2.IrqHandler(); } void SERCOM4_1_Handler() { Serial2.IrqHandler(); } @@ -35,7 +35,7 @@ void SERCOM4_3_Handler() { Serial2.IrqHandler(); } #endif -#if ANY_SERIAL_IS(2) +#if USING_HW_SERIAL2 UartT Serial3(false, &sercom1, PIN_SERIAL3_RX, PIN_SERIAL3_TX, PAD_SERIAL3_RX, PAD_SERIAL3_TX); void SERCOM1_0_Handler() { Serial3.IrqHandler(); } void SERCOM1_1_Handler() { Serial3.IrqHandler(); } @@ -43,7 +43,7 @@ void SERCOM1_3_Handler() { Serial3.IrqHandler(); } #endif -#if ANY_SERIAL_IS(3) +#if USING_HW_SERIAL3 UartT Serial4(false, &sercom5, PIN_SERIAL4_RX, PIN_SERIAL4_TX, PAD_SERIAL4_RX, PAD_SERIAL4_TX); void SERCOM5_0_Handler() { Serial4.IrqHandler(); } void SERCOM5_1_Handler() { Serial4.IrqHandler(); } diff --git a/Marlin/src/HAL/SAMD51/MarlinSerial_AGCM4.h b/Marlin/src/HAL/SAMD51/MarlinSerial_AGCM4.h index b7293415d136..ac5a3793983e 100644 --- a/Marlin/src/HAL/SAMD51/MarlinSerial_AGCM4.h +++ b/Marlin/src/HAL/SAMD51/MarlinSerial_AGCM4.h @@ -22,7 +22,7 @@ #include "../../core/serial_hook.h" -typedef Serial0Type UartT; +typedef Serial1Class UartT; extern UartT Serial2; extern UartT Serial3; diff --git a/Marlin/src/HAL/SAMD51/QSPIFlash.h b/Marlin/src/HAL/SAMD51/QSPIFlash.h index db4abec91caf..58822fe05f3e 100644 --- a/Marlin/src/HAL/SAMD51/QSPIFlash.h +++ b/Marlin/src/HAL/SAMD51/QSPIFlash.h @@ -25,7 +25,6 @@ * * Derived from Adafruit_SPIFlash class with no SdFat references */ - #pragma once #include diff --git a/Marlin/src/HAL/SAMD51/Servo.cpp b/Marlin/src/HAL/SAMD51/Servo.cpp index 9bab8e89be23..23ab21c615c4 100644 --- a/Marlin/src/HAL/SAMD51/Servo.cpp +++ b/Marlin/src/HAL/SAMD51/Servo.cpp @@ -53,7 +53,7 @@ static volatile int8_t currentServoIndex[_Nbr_16timers]; // index for the servo being pulsed for each timer (or -1 if refresh interval) FORCE_INLINE static uint16_t getTimerCount() { - Tc * const tc = TimerConfig[SERVO_TC].pTc; + Tc * const tc = timer_config[SERVO_TC].pTc; tc->COUNT16.CTRLBSET.reg = TC_CTRLBCLR_CMD_READSYNC; SYNC(tc->COUNT16.SYNCBUSY.bit.CTRLB || tc->COUNT16.SYNCBUSY.bit.COUNT); @@ -65,7 +65,7 @@ FORCE_INLINE static uint16_t getTimerCount() { // Interrupt handler for the TC // ---------------------------- HAL_SERVO_TIMER_ISR() { - Tc * const tc = TimerConfig[SERVO_TC].pTc; + Tc * const tc = timer_config[SERVO_TC].pTc; const timer16_Sequence_t timer = #ifndef _useTimer1 _timer2 @@ -125,7 +125,7 @@ HAL_SERVO_TIMER_ISR() { } void initISR(timer16_Sequence_t timer) { - Tc * const tc = TimerConfig[SERVO_TC].pTc; + Tc * const tc = timer_config[SERVO_TC].pTc; const uint8_t tcChannel = TIMER_TCCHANNEL(timer); static bool initialized = false; // Servo TC has been initialized @@ -202,7 +202,7 @@ void initISR(timer16_Sequence_t timer) { } void finISR(timer16_Sequence_t timer) { - Tc * const tc = TimerConfig[SERVO_TC].pTc; + Tc * const tc = timer_config[SERVO_TC].pTc; const uint8_t tcChannel = TIMER_TCCHANNEL(timer); // Disable the match channel interrupt request diff --git a/Marlin/src/HAL/SAMD51/eeprom_wired.cpp b/Marlin/src/HAL/SAMD51/eeprom_wired.cpp index d9a0225a7ae4..3481fe539c3e 100644 --- a/Marlin/src/HAL/SAMD51/eeprom_wired.cpp +++ b/Marlin/src/HAL/SAMD51/eeprom_wired.cpp @@ -41,12 +41,13 @@ bool PersistentStore::access_start() { eeprom_init(); return true; } bool PersistentStore::access_finish() { return true; } bool PersistentStore::write_data(int &pos, const uint8_t *value, size_t size, uint16_t *crc) { + uint16_t written = 0; while (size--) { const uint8_t v = *value; uint8_t * const p = (uint8_t * const)pos; - if (v != eeprom_read_byte(p)) { + if (v != eeprom_read_byte(p)) { // EEPROM has only ~100,000 write cycles, so only write bytes that have changed! eeprom_write_byte(p, v); - delay(2); + if (++written & 0x7F) delay(2); else safe_delay(2); // Avoid triggering watchdog during long EEPROM writes if (eeprom_read_byte(p) != v) { SERIAL_ECHO_MSG(STR_ERR_EEPROM_WRITE); return true; diff --git a/Marlin/src/HAL/SAMD51/endstop_interrupts.h b/Marlin/src/HAL/SAMD51/endstop_interrupts.h index daac7733875b..61a06c0d4b42 100644 --- a/Marlin/src/HAL/SAMD51/endstop_interrupts.h +++ b/Marlin/src/HAL/SAMD51/endstop_interrupts.h @@ -47,80 +47,38 @@ #include "../../module/endstops.h" -#define MATCH_EILINE(P1,P2) (P1 != P2 && PIN_TO_EILINE(P1) == PIN_TO_EILINE(P2)) -#if HAS_X_MAX - #define MATCH_X_MAX_EILINE(P) MATCH_EILINE(P, X_MAX_PIN) -#else - #define MATCH_X_MAX_EILINE(P) false -#endif -#if HAS_X_MIN - #define MATCH_X_MIN_EILINE(P) MATCH_EILINE(P, X_MIN_PIN) -#else - #define MATCH_X_MIN_EILINE(P) false -#endif -#if HAS_Y_MAX - #define MATCH_Y_MAX_EILINE(P) MATCH_EILINE(P, Y_MAX_PIN) -#else - #define MATCH_Y_MAX_EILINE(P) false -#endif -#if HAS_Y_MIN - #define MATCH_Y_MIN_EILINE(P) MATCH_EILINE(P, Y_MIN_PIN) -#else - #define MATCH_Y_MIN_EILINE(P) false -#endif -#if HAS_Z_MAX - #define MATCH_Z_MAX_EILINE(P) MATCH_EILINE(P, Z_MAX_PIN) -#else - #define MATCH_Z_MAX_EILINE(P) false -#endif -#if HAS_Z_MIN - #define MATCH_Z_MIN_EILINE(P) MATCH_EILINE(P, Z_MIN_PIN) -#else - #define MATCH_Z_MIN_EILINE(P) false -#endif -#if HAS_Z2_MAX - #define MATCH_Z2_MAX_EILINE(P) MATCH_EILINE(P, Z2_MAX_PIN) -#else - #define MATCH_Z2_MAX_EILINE(P) false -#endif -#if HAS_Z2_MIN - #define MATCH_Z2_MIN_EILINE(P) MATCH_EILINE(P, Z2_MIN_PIN) -#else - #define MATCH_Z2_MIN_EILINE(P) false -#endif -#if HAS_Z3_MAX - #define MATCH_Z3_MAX_EILINE(P) MATCH_EILINE(P, Z3_MAX_PIN) -#else - #define MATCH_Z3_MAX_EILINE(P) false -#endif -#if HAS_Z3_MIN - #define MATCH_Z3_MIN_EILINE(P) MATCH_EILINE(P, Z3_MIN_PIN) -#else - #define MATCH_Z3_MIN_EILINE(P) false -#endif -#if HAS_Z4_MAX - #define MATCH_Z4_MAX_EILINE(P) MATCH_EILINE(P, Z4_MAX_PIN) -#else - #define MATCH_Z4_MAX_EILINE(P) false -#endif -#if HAS_Z4_MIN - #define MATCH_Z4_MIN_EILINE(P) MATCH_EILINE(P, Z4_MIN_PIN) -#else - #define MATCH_Z4_MIN_EILINE(P) false -#endif -#if HAS_Z_MIN_PROBE_PIN - #define MATCH_Z_MIN_PROBE_EILINE(P) MATCH_EILINE(P, Z_MIN_PROBE_PIN) -#else - #define MATCH_Z_MIN_PROBE_EILINE(P) false -#endif -#define AVAILABLE_EILINE(P) (PIN_TO_EILINE(P) != -1 \ - && !MATCH_X_MAX_EILINE(P) && !MATCH_X_MIN_EILINE(P) \ - && !MATCH_Y_MAX_EILINE(P) && !MATCH_Y_MIN_EILINE(P) \ - && !MATCH_Z_MAX_EILINE(P) && !MATCH_Z_MIN_EILINE(P) \ - && !MATCH_Z2_MAX_EILINE(P) && !MATCH_Z2_MIN_EILINE(P) \ - && !MATCH_Z3_MAX_EILINE(P) && !MATCH_Z3_MIN_EILINE(P) \ - && !MATCH_Z4_MAX_EILINE(P) && !MATCH_Z4_MIN_EILINE(P) \ - && !MATCH_Z_MIN_PROBE_EILINE(P)) +#define MATCH_EILINE(P1,P2) (P1 != P2 && PIN_TO_EILINE(P1) == PIN_TO_EILINE(P2)) +#define MATCH_X_MAX_EILINE(P) TERN0(HAS_X_MAX, DEFER4(MATCH_EILINE)(P, X_MAX_PIN)) +#define MATCH_X_MIN_EILINE(P) TERN0(HAS_X_MIN, DEFER4(MATCH_EILINE)(P, X_MIN_PIN)) +#define MATCH_Y_MAX_EILINE(P) TERN0(HAS_Y_MAX, DEFER4(MATCH_EILINE)(P, Y_MAX_PIN)) +#define MATCH_Y_MIN_EILINE(P) TERN0(HAS_Y_MIN, DEFER4(MATCH_EILINE)(P, Y_MIN_PIN)) +#define MATCH_Z_MAX_EILINE(P) TERN0(HAS_Z_MAX, DEFER4(MATCH_EILINE)(P, Z_MAX_PIN)) +#define MATCH_Z_MIN_EILINE(P) TERN0(HAS_Z_MIN, DEFER4(MATCH_EILINE)(P, Z_MIN_PIN)) +#define MATCH_I_MAX_EILINE(P) TERN0(HAS_I_MAX, DEFER4(MATCH_EILINE)(P, I_MAX_PIN)) +#define MATCH_I_MIN_EILINE(P) TERN0(HAS_I_MIN, DEFER4(MATCH_EILINE)(P, I_MIN_PIN)) +#define MATCH_J_MAX_EILINE(P) TERN0(HAS_J_MAX, DEFER4(MATCH_EILINE)(P, J_MAX_PIN)) +#define MATCH_J_MIN_EILINE(P) TERN0(HAS_J_MIN, DEFER4(MATCH_EILINE)(P, J_MIN_PIN)) +#define MATCH_K_MAX_EILINE(P) TERN0(HAS_K_MAX, DEFER4(MATCH_EILINE)(P, K_MAX_PIN)) +#define MATCH_K_MIN_EILINE(P) TERN0(HAS_K_MIN, DEFER4(MATCH_EILINE)(P, K_MIN_PIN)) +#define MATCH_Z2_MAX_EILINE(P) TERN0(HAS_Z2_MAX, DEFER4(MATCH_EILINE)(P, Z2_MAX_PIN)) +#define MATCH_Z2_MIN_EILINE(P) TERN0(HAS_Z2_MIN, DEFER4(MATCH_EILINE)(P, Z2_MIN_PIN)) +#define MATCH_Z3_MAX_EILINE(P) TERN0(HAS_Z3_MAX, DEFER4(MATCH_EILINE)(P, Z3_MAX_PIN)) +#define MATCH_Z3_MIN_EILINE(P) TERN0(HAS_Z3_MIN, DEFER4(MATCH_EILINE)(P, Z3_MIN_PIN)) +#define MATCH_Z4_MAX_EILINE(P) TERN0(HAS_Z4_MAX, DEFER4(MATCH_EILINE)(P, Z4_MAX_PIN)) +#define MATCH_Z4_MIN_EILINE(P) TERN0(HAS_Z4_MIN, DEFER4(MATCH_EILINE)(P, Z4_MIN_PIN)) +#define MATCH_Z_MIN_PROBE_EILINE(P) TERN0(HAS_Z_MIN_PROBE_PIN, DEFER4(MATCH_EILINE)(P, Z_MIN_PROBE_PIN)) + +#define AVAILABLE_EILINE(P) ( PIN_TO_EILINE(P) != -1 \ + && !MATCH_X_MAX_EILINE(P) && !MATCH_X_MIN_EILINE(P) \ + && !MATCH_Y_MAX_EILINE(P) && !MATCH_Y_MIN_EILINE(P) \ + && !MATCH_Z_MAX_EILINE(P) && !MATCH_Z_MIN_EILINE(P) \ + && !MATCH_I_MAX_EILINE(P) && !MATCH_I_MIN_EILINE(P) \ + && !MATCH_J_MAX_EILINE(P) && !MATCH_J_MIN_EILINE(P) \ + && !MATCH_K_MAX_EILINE(P) && !MATCH_K_MIN_EILINE(P) \ + && !MATCH_Z2_MAX_EILINE(P) && !MATCH_Z2_MIN_EILINE(P) \ + && !MATCH_Z3_MAX_EILINE(P) && !MATCH_Z3_MIN_EILINE(P) \ + && !MATCH_Z4_MAX_EILINE(P) && !MATCH_Z4_MIN_EILINE(P) \ + && !MATCH_Z_MIN_PROBE_EILINE(P) ) // One ISR for all EXT-Interrupts void endstop_ISR() { endstops.update(); } @@ -205,4 +163,40 @@ void setup_endstop_interrupts() { #endif _ATTACH(Z_MIN_PROBE_PIN); #endif + #if HAS_I_MAX + #if !AVAILABLE_EILINE(I_MAX_PIN) + #error "I_MAX_PIN has no EXTINT line available." + #endif + attachInterrupt(I_MAX_PIN, endstop_ISR, CHANGE); + #endif + #if HAS_I_MIN + #if !AVAILABLE_EILINE(I_MIN_PIN) + #error "I_MIN_PIN has no EXTINT line available." + #endif + attachInterrupt(I_MIN_PIN, endstop_ISR, CHANGE); + #endif + #if HAS_J_MAX + #if !AVAILABLE_EILINE(J_MAX_PIN) + #error "J_MAX_PIN has no EXTINT line available." + #endif + attachInterrupt(J_MAX_PIN, endstop_ISR, CHANGE); + #endif + #if HAS_J_MIN + #if !AVAILABLE_EILINE(J_MIN_PIN) + #error "J_MIN_PIN has no EXTINT line available." + #endif + attachInterrupt(J_MIN_PIN, endstop_ISR, CHANGE); + #endif + #if HAS_K_MAX + #if !AVAILABLE_EILINE(K_MAX_PIN) + #error "K_MAX_PIN has no EXTINT line available." + #endif + attachInterrupt(K_MAX_PIN, endstop_ISR, CHANGE); + #endif + #if HAS_K_MIN + #if !AVAILABLE_EILINE(K_MIN_PIN) + #error "K_MIN_PIN has no EXTINT line available." + #endif + attachInterrupt(K_MIN_PIN, endstop_ISR, CHANGE); + #endif } diff --git a/Marlin/src/HAL/SAMD51/fastio.h b/Marlin/src/HAL/SAMD51/fastio.h index a95b7cac0ce9..79aede579044 100644 --- a/Marlin/src/HAL/SAMD51/fastio.h +++ b/Marlin/src/HAL/SAMD51/fastio.h @@ -131,7 +131,7 @@ */ #define PWM_PIN(P) (WITHIN(P, 2, 13) || WITHIN(P, 22, 23) || WITHIN(P, 44, 45) || P == 48) - // Return fullfilled ADCx->INPUTCTRL.reg + // Return fulfilled ADCx->INPUTCTRL.reg #define PIN_TO_INPUTCTRL(P) ( (PIN_TO_AIN(P) == 0) ? ADC_INPUTCTRL_MUXPOS_AIN0 \ : (PIN_TO_AIN(P) == 1) ? ADC_INPUTCTRL_MUXPOS_AIN1 \ : (PIN_TO_AIN(P) == 2) ? ADC_INPUTCTRL_MUXPOS_AIN2 \ diff --git a/Marlin/src/HAL/SAMD51/inc/SanityCheck.h b/Marlin/src/HAL/SAMD51/inc/SanityCheck.h index 2a4bde98e6cb..1b876c947dc7 100644 --- a/Marlin/src/HAL/SAMD51/inc/SanityCheck.h +++ b/Marlin/src/HAL/SAMD51/inc/SanityCheck.h @@ -31,11 +31,12 @@ #error "No custom SD drive cable defined for this board." #endif -#if defined(MAX6675_SCK_PIN) && defined(MAX6675_DO_PIN) && (MAX6675_SCK_PIN == SCK1 || MAX6675_DO_PIN == MISO1) +#if (defined(TEMP_0_SCK_PIN) && defined(TEMP_0_MISO_PIN) && (TEMP_0_SCK_PIN == SCK1 || TEMP_0_MISO_PIN == MISO1)) || \ + (defined(TEMP_1_SCK_PIN) && defined(TEMP_1_MISO_PIN) && (TEMP_1_SCK_PIN == SCK1 || TEMP_1_MISO_PIN == MISO1)) #error "OnBoard SPI BUS can't be shared with other devices." #endif -#if SERVO_TC == RTC_TIMER_NUM +#if SERVO_TC == MF_TIMER_RTC #error "Servos can't use RTC timer" #endif diff --git a/Marlin/src/HAL/SAMD51/pinsDebug.h b/Marlin/src/HAL/SAMD51/pinsDebug.h index 81376db79add..f0a46fd7c567 100644 --- a/Marlin/src/HAL/SAMD51/pinsDebug.h +++ b/Marlin/src/HAL/SAMD51/pinsDebug.h @@ -26,6 +26,7 @@ #define PRINT_PORT(p) do{ SERIAL_ECHOPGM(" Port: "); sprintf_P(buffer, PSTR("%c%02ld"), 'A' + g_APinDescription[p].ulPort, g_APinDescription[p].ulPin); SERIAL_ECHO(buffer); }while (0) #define PRINT_ARRAY_NAME(x) do{ sprintf_P(buffer, PSTR("%-" STRINGIFY(MAX_NAME_LENGTH) "s"), pin_array[x].name); SERIAL_ECHO(buffer); }while(0) #define PRINT_PIN(p) do{ sprintf_P(buffer, PSTR("%3d "), p); SERIAL_ECHO(buffer); }while(0) +#define PRINT_PIN_ANALOG(p) do{ sprintf_P(buffer, PSTR(" (A%2d) "), DIGITAL_PIN_TO_ANALOG_PIN(pin)); SERIAL_ECHO(buffer); }while(0) #define GET_ARRAY_PIN(p) pin_array[p].pin #define GET_ARRAY_IS_DIGITAL(p) pin_array[p].is_digital #define VALID_PIN(pin) (pin >= 0 && pin < (int8_t)NUMBER_PINS_TOTAL) @@ -47,7 +48,7 @@ bool GET_PINMODE(int8_t pin) { // 1: output, 0: input void pwm_details(int32_t pin) { if (pwm_status(pin)) { //uint32_t chan = g_APinDescription[pin].ulPWMChannel TODO when fast pwm is operative; - //SERIAL_ECHOPAIR("PWM = ", duty); + //SERIAL_ECHOPGM("PWM = ", duty); } } diff --git a/Marlin/src/HAL/SAMD51/timers.cpp b/Marlin/src/HAL/SAMD51/timers.cpp index 5c55d324079a..1ad0e360736a 100644 --- a/Marlin/src/HAL/SAMD51/timers.cpp +++ b/Marlin/src/HAL/SAMD51/timers.cpp @@ -31,13 +31,13 @@ // Local defines // -------------------------------------------------------------------------- -#define NUM_HARDWARE_TIMERS 8 +#define NUM_HARDWARE_TIMERS 9 // -------------------------------------------------------------------------- // Private Variables // -------------------------------------------------------------------------- -const tTimerConfig TimerConfig[NUM_HARDWARE_TIMERS+1] = { +const tTimerConfig timer_config[NUM_HARDWARE_TIMERS] = { { {.pTc=TC0}, TC0_IRQn, TC_PRIORITY(0) }, // 0 - stepper (assigned priority 2) { {.pTc=TC1}, TC1_IRQn, TC_PRIORITY(1) }, // 1 - stepper (needed by 32 bit timers) { {.pTc=TC2}, TC2_IRQn, 5 }, // 2 - tone (reserved by framework and fixed assigned priority 5) @@ -67,19 +67,19 @@ FORCE_INLINE void Disable_Irq(IRQn_Type irq) { // -------------------------------------------------------------------------- void HAL_timer_start(const uint8_t timer_num, const uint32_t frequency) { - IRQn_Type irq = TimerConfig[timer_num].IRQ_Id; + IRQn_Type irq = timer_config[timer_num].IRQ_Id; // Disable interrupt, just in case it was already enabled Disable_Irq(irq); - if (timer_num == RTC_TIMER_NUM) { - Rtc * const rtc = TimerConfig[timer_num].pRtc; + if (timer_num == MF_TIMER_RTC) { + Rtc * const rtc = timer_config[timer_num].pRtc; // Disable timer interrupt rtc->MODE0.INTENCLR.reg = RTC_MODE0_INTENCLR_CMP0; // RTC clock setup - OSC32KCTRL->RTCCTRL.reg = OSC32KCTRL_RTCCTRL_RTCSEL_XOSC32K; // External 32.768KHz oscillator + OSC32KCTRL->RTCCTRL.reg = OSC32KCTRL_RTCCTRL_RTCSEL_XOSC32K; // External 32.768kHz oscillator // Stop timer, just in case, to be able to reconfigure it rtc->MODE0.CTRLA.bit.ENABLE = false; @@ -101,13 +101,13 @@ void HAL_timer_start(const uint8_t timer_num, const uint32_t frequency) { SYNC(rtc->MODE0.SYNCBUSY.bit.ENABLE); } else { - Tc * const tc = TimerConfig[timer_num].pTc; + Tc * const tc = timer_config[timer_num].pTc; // Disable timer interrupt tc->COUNT32.INTENCLR.reg = TC_INTENCLR_OVF; // disable overflow interrupt // TCn clock setup - const uint8_t clockID = GCLK_CLKCTRL_IDs[TCC_INST_NUM + timer_num]; // TC clock are preceeded by TCC ones + const uint8_t clockID = GCLK_CLKCTRL_IDs[TCC_INST_NUM + timer_num]; // TC clock are preceded by TCC ones GCLK->PCHCTRL[clockID].bit.CHEN = false; SYNC(GCLK->PCHCTRL[clockID].bit.CHEN); GCLK->PCHCTRL[clockID].reg = GCLK_PCHCTRL_GEN_GCLK0 | GCLK_PCHCTRL_CHEN; // 120MHz startup code programmed @@ -141,17 +141,17 @@ void HAL_timer_start(const uint8_t timer_num, const uint32_t frequency) { } // Finally, enable IRQ - NVIC_SetPriority(irq, TimerConfig[timer_num].priority); + NVIC_SetPriority(irq, timer_config[timer_num].priority); NVIC_EnableIRQ(irq); } void HAL_timer_enable_interrupt(const uint8_t timer_num) { - const IRQn_Type irq = TimerConfig[timer_num].IRQ_Id; + const IRQn_Type irq = timer_config[timer_num].IRQ_Id; NVIC_EnableIRQ(irq); } void HAL_timer_disable_interrupt(const uint8_t timer_num) { - const IRQn_Type irq = TimerConfig[timer_num].IRQ_Id; + const IRQn_Type irq = timer_config[timer_num].IRQ_Id; Disable_Irq(irq); } @@ -161,7 +161,7 @@ static bool NVIC_GetEnabledIRQ(IRQn_Type IRQn) { } bool HAL_timer_interrupt_enabled(const uint8_t timer_num) { - const IRQn_Type irq = TimerConfig[timer_num].IRQ_Id; + const IRQn_Type irq = timer_config[timer_num].IRQ_Id; return NVIC_GetEnabledIRQ(irq); } diff --git a/Marlin/src/HAL/SAMD51/timers.h b/Marlin/src/HAL/SAMD51/timers.h index dc6e38b7307c..86e980c56690 100644 --- a/Marlin/src/HAL/SAMD51/timers.h +++ b/Marlin/src/HAL/SAMD51/timers.h @@ -25,21 +25,22 @@ // -------------------------------------------------------------------------- // Defines // -------------------------------------------------------------------------- -#define RTC_TIMER_NUM 8 // This is not a TC but a RTC typedef uint32_t hal_timer_t; #define HAL_TIMER_TYPE_MAX 0xFFFFFFFF #define HAL_TIMER_RATE F_CPU // frequency of timers peripherals -#ifndef STEP_TIMER_NUM - #define STEP_TIMER_NUM 0 // Timer Index for Stepper +#define MF_TIMER_RTC 8 // This is not a TC but a RTC + +#ifndef MF_TIMER_STEP + #define MF_TIMER_STEP 0 // Timer Index for Stepper #endif -#ifndef PULSE_TIMER_NUM - #define PULSE_TIMER_NUM STEP_TIMER_NUM +#ifndef MF_TIMER_PULSE + #define MF_TIMER_PULSE MF_TIMER_STEP #endif -#ifndef TEMP_TIMER_NUM - #define TEMP_TIMER_NUM RTC_TIMER_NUM // Timer Index for Temperature +#ifndef MF_TIMER_TEMP + #define MF_TIMER_TEMP MF_TIMER_RTC // Timer Index for Temperature #endif #define TEMP_TIMER_FREQUENCY 1000 // temperature interrupt frequency @@ -52,30 +53,29 @@ typedef uint32_t hal_timer_t; #define PULSE_TIMER_PRESCALE STEPPER_TIMER_PRESCALE #define PULSE_TIMER_TICKS_PER_US STEPPER_TIMER_TICKS_PER_US -#define ENABLE_STEPPER_DRIVER_INTERRUPT() HAL_timer_enable_interrupt(STEP_TIMER_NUM) -#define DISABLE_STEPPER_DRIVER_INTERRUPT() HAL_timer_disable_interrupt(STEP_TIMER_NUM) -#define STEPPER_ISR_ENABLED() HAL_timer_interrupt_enabled(STEP_TIMER_NUM) +#define ENABLE_STEPPER_DRIVER_INTERRUPT() HAL_timer_enable_interrupt(MF_TIMER_STEP) +#define DISABLE_STEPPER_DRIVER_INTERRUPT() HAL_timer_disable_interrupt(MF_TIMER_STEP) +#define STEPPER_ISR_ENABLED() HAL_timer_interrupt_enabled(MF_TIMER_STEP) -#define ENABLE_TEMPERATURE_INTERRUPT() HAL_timer_enable_interrupt(TEMP_TIMER_NUM) -#define DISABLE_TEMPERATURE_INTERRUPT() HAL_timer_disable_interrupt(TEMP_TIMER_NUM) +#define ENABLE_TEMPERATURE_INTERRUPT() HAL_timer_enable_interrupt(MF_TIMER_TEMP) +#define DISABLE_TEMPERATURE_INTERRUPT() HAL_timer_disable_interrupt(MF_TIMER_TEMP) -#define TC_PRIORITY(t) t == SERVO_TC ? 1 \ - : (t == STEP_TIMER_NUM || t == PULSE_TIMER_NUM) ? 2 \ - : (t == TEMP_TIMER_NUM) ? 6 \ - : 7 +#define TC_PRIORITY(t) ( t == SERVO_TC ? 1 \ + : (t == MF_TIMER_STEP || t == MF_TIMER_PULSE) ? 2 \ + : (t == MF_TIMER_TEMP) ? 6 : 7 ) #define _TC_HANDLER(t) void TC##t##_Handler() #define TC_HANDLER(t) _TC_HANDLER(t) #ifndef HAL_STEP_TIMER_ISR - #define HAL_STEP_TIMER_ISR() TC_HANDLER(STEP_TIMER_NUM) + #define HAL_STEP_TIMER_ISR() TC_HANDLER(MF_TIMER_STEP) #endif -#if STEP_TIMER_NUM != PULSE_TIMER_NUM - #define HAL_PULSE_TIMER_ISR() TC_HANDLER(PULSE_TIMER_NUM) +#if MF_TIMER_STEP != MF_TIMER_PULSE + #define HAL_PULSE_TIMER_ISR() TC_HANDLER(MF_TIMER_PULSE) #endif -#if TEMP_TIMER_NUM == RTC_TIMER_NUM +#if MF_TIMER_TEMP == MF_TIMER_RTC #define HAL_TEMP_TIMER_ISR() void RTC_Handler() #else - #define HAL_TEMP_TIMER_ISR() TC_HANDLER(TEMP_TIMER_NUM) + #define HAL_TEMP_TIMER_ISR() TC_HANDLER(MF_TIMER_TEMP) #endif // -------------------------------------------------------------------------- @@ -95,7 +95,7 @@ typedef struct { // Public Variables // -------------------------------------------------------------------------- -extern const tTimerConfig TimerConfig[]; +extern const tTimerConfig timer_config[]; // -------------------------------------------------------------------------- // Public functions @@ -104,20 +104,20 @@ extern const tTimerConfig TimerConfig[]; void HAL_timer_start(const uint8_t timer_num, const uint32_t frequency); FORCE_INLINE static void HAL_timer_set_compare(const uint8_t timer_num, const hal_timer_t compare) { - // Should never be called with timer RTC_TIMER_NUM - Tc * const tc = TimerConfig[timer_num].pTc; + // Should never be called with timer MF_TIMER_RTC + Tc * const tc = timer_config[timer_num].pTc; tc->COUNT32.CC[0].reg = compare; } FORCE_INLINE static hal_timer_t HAL_timer_get_compare(const uint8_t timer_num) { - // Should never be called with timer RTC_TIMER_NUM - Tc * const tc = TimerConfig[timer_num].pTc; + // Should never be called with timer MF_TIMER_RTC + Tc * const tc = timer_config[timer_num].pTc; return (hal_timer_t)tc->COUNT32.CC[0].reg; } FORCE_INLINE static hal_timer_t HAL_timer_get_count(const uint8_t timer_num) { - // Should never be called with timer RTC_TIMER_NUM - Tc * const tc = TimerConfig[timer_num].pTc; + // Should never be called with timer MF_TIMER_RTC + Tc * const tc = timer_config[timer_num].pTc; tc->COUNT32.CTRLBSET.reg = TC_CTRLBCLR_CMD_READSYNC; SYNC(tc->COUNT32.SYNCBUSY.bit.CTRLB || tc->COUNT32.SYNCBUSY.bit.COUNT); return tc->COUNT32.COUNT.reg; @@ -128,13 +128,13 @@ void HAL_timer_disable_interrupt(const uint8_t timer_num); bool HAL_timer_interrupt_enabled(const uint8_t timer_num); FORCE_INLINE static void HAL_timer_isr_prologue(const uint8_t timer_num) { - if (timer_num == RTC_TIMER_NUM) { - Rtc * const rtc = TimerConfig[timer_num].pRtc; + if (timer_num == MF_TIMER_RTC) { + Rtc * const rtc = timer_config[timer_num].pRtc; // Clear interrupt flag rtc->MODE0.INTFLAG.reg = RTC_MODE0_INTFLAG_CMP0; } else { - Tc * const tc = TimerConfig[timer_num].pTc; + Tc * const tc = timer_config[timer_num].pTc; // Clear interrupt flag tc->COUNT32.INTFLAG.reg = TC_INTFLAG_OVF; } diff --git a/Marlin/src/HAL/STM32/HAL.cpp b/Marlin/src/HAL/STM32/HAL.cpp index d13be1a21a3a..324a78316a2d 100644 --- a/Marlin/src/HAL/STM32/HAL.cpp +++ b/Marlin/src/HAL/STM32/HAL.cpp @@ -20,7 +20,9 @@ * along with this program. If not, see . * */ -#if defined(ARDUINO_ARCH_STM32) && !defined(STM32GENERIC) +#include "../platforms.h" + +#ifdef HAL_STM32 #include "HAL.h" #include "usb_serial.h" @@ -29,7 +31,7 @@ #include "../shared/Delay.h" #ifdef USBCON - DefaultSerial MSerial(false, SerialUSB); + DefaultSerial1 MSerial0(false, SerialUSB); #endif #if ENABLED(SRAM_EEPROM_EMULATION) @@ -51,18 +53,18 @@ // Public Variables // ------------------------ -uint16_t HAL_adc_result; +uint16_t MarlinHAL::adc_result; // ------------------------ // Public functions // ------------------------ -TERN_(POSTMORTEM_DEBUGGING, extern void install_min_serial()); +#if ENABLED(POSTMORTEM_DEBUGGING) + extern void install_min_serial(); +#endif // HAL initialization task -void HAL_init() { - FastIO_init(); - +void MarlinHAL::init() { // Ensure F_CPU is a constant expression. // If the compiler breaks here, it means that delay code that should compute at compile time will not work. // So better safe than sorry here. @@ -91,15 +93,19 @@ void HAL_init() { USB_Hook_init(); #endif - TERN_(POSTMORTEM_DEBUGGING, install_min_serial()); // Install the min serial handler + TERN_(POSTMORTEM_DEBUGGING, install_min_serial()); // Install the min serial handler + + TERN_(HAS_SD_HOST_DRIVE, MSC_SD_init()); // Enable USB SD card access - #if HAS_SD_HOST_DRIVE - MSC_SD_init(); // Enable USB SD card access + #if PIN_EXISTS(USB_CONNECT) + OUT_WRITE(USB_CONNECT_PIN, !USB_CONNECT_INVERTING); // USB clear connection + delay(1000); // Give OS time to notice + WRITE(USB_CONNECT_PIN, USB_CONNECT_INVERTING); #endif } // HAL idle task -void HAL_idletask() { +void MarlinHAL::idletask() { #if HAS_SHARED_MEDIA // Stm32duino currently doesn't have a "loop/idle" method CDC_resume_receive(); @@ -107,9 +113,9 @@ void HAL_idletask() { #endif } -void HAL_clear_reset_source() { __HAL_RCC_CLEAR_RESET_FLAGS(); } +void MarlinHAL::reboot() { NVIC_SystemReset(); } -uint8_t HAL_get_reset_source() { +uint8_t MarlinHAL::get_reset_source() { return #ifdef RCC_FLAG_IWDGRST // Some sources may not exist... RESET != __HAL_RCC_GET_FLAG(RCC_FLAG_IWDGRST) ? RST_WATCHDOG : @@ -133,22 +139,14 @@ uint8_t HAL_get_reset_source() { ; } -void _delay_ms(const int delay_ms) { delay(delay_ms); } +void MarlinHAL::clear_reset_source() { __HAL_RCC_CLEAR_RESET_FLAGS(); } extern "C" { extern unsigned int _ebss; // end of bss section } -// ------------------------ -// ADC -// ------------------------ - -// TODO: Make sure this doesn't cause any delay -void HAL_adc_start_conversion(const uint8_t adc_pin) { HAL_adc_result = analogRead(adc_pin); } -uint16_t HAL_adc_get_result() { return HAL_adc_result; } - -// Reset the system (to initiate a firmware flash) -void flashFirmware(const int16_t) { NVIC_SystemReset(); } +// Reset the system to initiate a firmware flash +WEAK void flashFirmware(const int16_t) { hal.reboot(); } // Maple Compatibility volatile uint32_t systick_uptime_millis = 0; @@ -159,4 +157,4 @@ void HAL_SYSTICK_Callback() { if (systick_user_callback) systick_user_callback(); } -#endif // ARDUINO_ARCH_STM32 && !STM32GENERIC +#endif // HAL_STM32 diff --git a/Marlin/src/HAL/STM32/HAL.h b/Marlin/src/HAL/STM32/HAL.h index be0cc309626c..f5e8c1187da7 100644 --- a/Marlin/src/HAL/STM32/HAL.h +++ b/Marlin/src/HAL/STM32/HAL.h @@ -37,54 +37,71 @@ #include +// +// Default graphical display delays +// +#define CPU_ST7920_DELAY_1 300 +#define CPU_ST7920_DELAY_2 40 +#define CPU_ST7920_DELAY_3 340 + +// ------------------------ +// Serial ports +// ------------------------ #ifdef USBCON #include #include "../../core/serial_hook.h" - typedef ForwardSerial0Type< decltype(SerialUSB) > DefaultSerial; - extern DefaultSerial MSerial; + typedef ForwardSerial1Class< decltype(SerialUSB) > DefaultSerial1; + extern DefaultSerial1 MSerial0; #endif -// ------------------------ -// Defines -// ------------------------ #define _MSERIAL(X) MSerial##X #define MSERIAL(X) _MSERIAL(X) #if SERIAL_PORT == -1 - #define MYSERIAL0 MSerial + #define MYSERIAL1 MSerial0 #elif WITHIN(SERIAL_PORT, 1, 6) - #define MYSERIAL0 MSERIAL(SERIAL_PORT) + #define MYSERIAL1 MSERIAL(SERIAL_PORT) #else - #error "SERIAL_PORT must be -1 or from 1 to 6. Please update your configuration." + #error "SERIAL_PORT must be from 1 to 6. You can also use -1 if the board supports Native USB." #endif #ifdef SERIAL_PORT_2 #if SERIAL_PORT_2 == -1 - #define MYSERIAL1 MSerial + #define MYSERIAL2 MSerial0 #elif WITHIN(SERIAL_PORT_2, 1, 6) - #define MYSERIAL1 MSERIAL(SERIAL_PORT_2) + #define MYSERIAL2 MSERIAL(SERIAL_PORT_2) + #else + #error "SERIAL_PORT_2 must be from 1 to 6. You can also use -1 if the board supports Native USB." + #endif +#endif + +#ifdef SERIAL_PORT_3 + #if SERIAL_PORT_3 == -1 + #define MYSERIAL3 MSerial0 + #elif WITHIN(SERIAL_PORT_3, 1, 6) + #define MYSERIAL3 MSERIAL(SERIAL_PORT_3) #else - #error "SERIAL_PORT_2 must be -1 or from 1 to 6. Please update your configuration." + #error "SERIAL_PORT_3 must be from 1 to 6. You can also use -1 if the board supports Native USB." #endif #endif #ifdef MMU2_SERIAL_PORT #if MMU2_SERIAL_PORT == -1 - #define MMU2_SERIAL MSerial + #define MMU2_SERIAL MSerial0 #elif WITHIN(MMU2_SERIAL_PORT, 1, 6) #define MMU2_SERIAL MSERIAL(MMU2_SERIAL_PORT) #else - #error "MMU2_SERIAL_PORT must be -1 or from 1 to 6. Please update your configuration." + #error "MMU2_SERIAL_PORT must be from 1 to 6. You can also use -1 if the board supports Native USB." #endif #endif #ifdef LCD_SERIAL_PORT #if LCD_SERIAL_PORT == -1 - #define LCD_SERIAL MSerial + #define LCD_SERIAL MSerial0 #elif WITHIN(LCD_SERIAL_PORT, 1, 6) #define LCD_SERIAL MSERIAL(LCD_SERIAL_PORT) #else - #error "LCD_SERIAL_PORT must be -1 or from 1 to 6. Please update your configuration." + #error "LCD_SERIAL_PORT must be from 1 to 6. You can also use -1 if the board supports Native USB." #endif #if HAS_DGUS_LCD #define SERIAL_GET_TX_BUFFER_FREE() LCD_SERIAL.availableForWrite() @@ -98,60 +115,79 @@ #define analogInputToDigitalPin(p) (p) #endif -#define CRITICAL_SECTION_START() uint32_t primask = __get_PRIMASK(); __disable_irq() -#define CRITICAL_SECTION_END() if (!primask) __enable_irq() -#define ISRS_ENABLED() (!__get_PRIMASK()) -#define ENABLE_ISRS() __enable_irq() -#define DISABLE_ISRS() __disable_irq() +// +// Interrupts +// +#define CRITICAL_SECTION_START() const bool irqon = !__get_PRIMASK(); __disable_irq() +#define CRITICAL_SECTION_END() if (irqon) __enable_irq() #define cli() __disable_irq() #define sei() __enable_irq() -// On AVR this is in math.h? -#define square(x) ((x)*(x)) - // ------------------------ // Types // ------------------------ -typedef int16_t pin_t; +#ifdef STM32G0B1xx + typedef int32_t pin_t; +#else + typedef int16_t pin_t; +#endif -#define HAL_SERVO_LIB libServo +class libServo; +typedef libServo hal_servo_t; #define PAUSE_SERVO_OUTPUT() libServo::pause_all_servos() #define RESUME_SERVO_OUTPUT() libServo::resume_all_servos() // ------------------------ -// Public Variables +// ADC // ------------------------ -// result of last ADC conversion -extern uint16_t HAL_adc_result; +#ifdef ADC_RESOLUTION + #define HAL_ADC_RESOLUTION ADC_RESOLUTION +#else + #define HAL_ADC_RESOLUTION 12 +#endif -// ------------------------ -// Public functions -// ------------------------ +#define HAL_ADC_VREF 3.3 -// Memory related -#define __bss_end __bss_end__ +// +// Pin Mapping for M42, M43, M226 +// +#define GET_PIN_MAP_PIN(index) index +#define GET_PIN_MAP_INDEX(pin) pin +#define PARSED_PIN_INDEX(code, dval) parser.intval(code, dval) -// Enable hooks into setup for HAL -void HAL_init(); -#define HAL_IDLETASK 1 -void HAL_idletask(); +#ifdef STM32F1xx + #define JTAG_DISABLE() AFIO_DBGAFR_CONFIG(AFIO_MAPR_SWJ_CFG_JTAGDISABLE) + #define JTAGSWD_DISABLE() AFIO_DBGAFR_CONFIG(AFIO_MAPR_SWJ_CFG_DISABLE) + #define JTAGSWD_RESET() AFIO_DBGAFR_CONFIG(AFIO_MAPR_SWJ_CFG_RESET); // Reset: FULL SWD+JTAG +#endif + +#define PLATFORM_M997_SUPPORT +void flashFirmware(const int16_t); + +// Maple Compatibility +typedef void (*systickCallback_t)(void); +void systick_attach_callback(systickCallback_t cb); +void HAL_SYSTICK_Callback(); -// Clear reset reason -void HAL_clear_reset_source(); +extern volatile uint32_t systick_uptime_millis; -// Reset reason -uint8_t HAL_get_reset_source(); +#define HAL_CAN_SET_PWM_FREQ // This HAL supports PWM Frequency adjustment -inline void HAL_reboot() {} // reboot the board or restart the bootloader +// ------------------------ +// Class Utilities +// ------------------------ -void _delay_ms(const int delay); +// Memory related +#define __bss_end __bss_end__ extern "C" char* _sbrk(int incr); #pragma GCC diagnostic push -#pragma GCC diagnostic ignored "-Wunused-function" +#if GCC_VERSION <= 50000 + #pragma GCC diagnostic ignored "-Wunused-function" +#endif static inline int freeMemory() { volatile char top; @@ -160,56 +196,71 @@ static inline int freeMemory() { #pragma GCC diagnostic pop -// -// ADC -// +// ------------------------ +// MarlinHAL Class +// ------------------------ -#define HAL_ANALOG_SELECT(pin) pinMode(pin, INPUT) +class MarlinHAL { +public: -#define HAL_ADC_VREF 3.3 -#define HAL_ADC_RESOLUTION ADC_RESOLUTION // 12 -#define HAL_START_ADC(pin) HAL_adc_start_conversion(pin) -#define HAL_READ_ADC() HAL_adc_result -#define HAL_ADC_READY() true + // Earliest possible init, before setup() + MarlinHAL() {} -inline void HAL_adc_init() { analogReadResolution(HAL_ADC_RESOLUTION); } + static void init(); // Called early in setup() + static void init_board() {} // Called less early in setup() + static void reboot(); // Restart the firmware from 0x0 -void HAL_adc_start_conversion(const uint8_t adc_pin); + // Interrupts + static bool isr_state() { return !__get_PRIMASK(); } + static void isr_on() { sei(); } + static void isr_off() { cli(); } -uint16_t HAL_adc_get_result(); + static void delay_ms(const int ms) { delay(ms); } -#define GET_PIN_MAP_PIN(index) index -#define GET_PIN_MAP_INDEX(pin) pin -#define PARSED_PIN_INDEX(code, dval) parser.intval(code, dval) + // Tasks, called from idle() + static void idletask(); -#ifdef STM32F1xx - #define JTAG_DISABLE() AFIO_DBGAFR_CONFIG(AFIO_MAPR_SWJ_CFG_JTAGDISABLE) - #define JTAGSWD_DISABLE() AFIO_DBGAFR_CONFIG(AFIO_MAPR_SWJ_CFG_DISABLE) -#endif + // Reset + static uint8_t get_reset_source(); + static void clear_reset_source(); -#define PLATFORM_M997_SUPPORT -void flashFirmware(const int16_t); + // Free SRAM + static int freeMemory() { return ::freeMemory(); } -// Maple Compatibility -typedef void (*systickCallback_t)(void); -void systick_attach_callback(systickCallback_t cb); -void HAL_SYSTICK_Callback(); + // + // ADC Methods + // -extern volatile uint32_t systick_uptime_millis; + static uint16_t adc_result; -#define HAL_CAN_SET_PWM_FREQ // This HAL supports PWM Frequency adjustment + // Called by Temperature::init once at startup + static void adc_init() { + analogReadResolution(HAL_ADC_RESOLUTION); + } -/** - * set_pwm_frequency - * Set the frequency of the timer corresponding to the provided pin - * All Timer PWM pins run at the same frequency - */ -void set_pwm_frequency(const pin_t pin, int f_desired); + // Called by Temperature::init for each sensor at startup + static void adc_enable(const pin_t pin) { pinMode(pin, INPUT); } -/** - * set_pwm_duty - * Set the PWM duty cycle of the provided pin to the provided value - * Optionally allows inverting the duty cycle [default = false] - * Optionally allows changing the maximum size of the provided value to enable finer PWM duty control [default = 255] - */ -void set_pwm_duty(const pin_t pin, const uint16_t v, const uint16_t v_size=255, const bool invert=false); + // Begin ADC sampling on the given channel + static void adc_start(const pin_t pin) { adc_result = analogRead(pin); } + + // Is the ADC ready for reading? + static bool adc_ready() { return true; } + + // The current value of the ADC register + static uint16_t adc_value() { return adc_result; } + + /** + * Set the PWM duty cycle for the pin to the given value. + * Optionally invert the duty cycle [default = false] + * Optionally change the maximum size of the provided value to enable finer PWM duty control [default = 255] + */ + static void set_pwm_duty(const pin_t pin, const uint16_t v, const uint16_t v_size=255, const bool invert=false); + + /** + * Set the frequency of the timer for the given pin. + * All Timer PWM pins run at the same frequency. + */ + static void set_pwm_frequency(const pin_t pin, const uint16_t f_desired); + +}; diff --git a/Marlin/src/HAL/STM32/HAL_MinSerial.cpp b/Marlin/src/HAL/STM32/HAL_MinSerial.cpp index 823bb6e8f5e7..b6e86b72daa9 100644 --- a/Marlin/src/HAL/STM32/HAL_MinSerial.cpp +++ b/Marlin/src/HAL/STM32/HAL_MinSerial.cpp @@ -20,7 +20,9 @@ * along with this program. If not, see . * */ -#if defined(ARDUINO_ARCH_STM32) && !defined(STM32GENERIC) +#include "../platforms.h" + +#ifdef HAL_STM32 #include "../../inc/MarlinConfigPre.h" @@ -71,8 +73,8 @@ static void TXBegin() { volatile uint32_t ICER[32]; }; - NVICMin * nvicBase = (NVICMin*)0xE000E100; - nvicBase->ICER[nvicIndex / 32] |= _BV32(nvicIndex % 32); + NVICMin *nvicBase = (NVICMin*)0xE000E100; + SBI32(nvicBase->ICER[nvicIndex >> 5], nvicIndex & 0x1F); // We NEED memory barriers to ensure Interrupts are actually disabled! // ( https://dzone.com/articles/nvic-disabling-interrupts-on-arm-cortex-m-and-the ) @@ -123,7 +125,7 @@ static void TX(char c) { } regs->DR = c; #else - // Let's hope a mystical guru will fix this, one day by writting interrupt-free USB CDC ACM code (or, at least, by polling the registers since interrupt will be queued but will never trigger) + // Let's hope a mystical guru will fix this, one day by writing interrupt-free USB CDC ACM code (or, at least, by polling the registers since interrupt will be queued but will never trigger) // For now, it's completely lost to oblivion. #endif } @@ -133,7 +135,7 @@ void install_min_serial() { HAL_min_serial_out = &TX; } -#if DISABLED(DYNAMIC_VECTORTABLE) && DISABLED(STM32F0xx) // Cortex M0 can't jump to a symbol that's too far from the current function, so we work around this in exception_arm.cpp +#if NONE(DYNAMIC_VECTORTABLE, STM32F0xx, STM32G0xx) // Cortex M0 can't jump to a symbol that's too far from the current function, so we work around this in exception_arm.cpp extern "C" { __attribute__((naked)) void JumpHandler_ASM() { __asm__ __volatile__ ( @@ -149,4 +151,4 @@ extern "C" { #endif #endif // POSTMORTEM_DEBUGGING -#endif // ARDUINO_ARCH_STM32 +#endif // HAL_STM32 diff --git a/Marlin/src/HAL/STM32/HAL_SPI.cpp b/Marlin/src/HAL/STM32/HAL_SPI.cpp index c9f23e6fa312..40d320d5e822 100644 --- a/Marlin/src/HAL/STM32/HAL_SPI.cpp +++ b/Marlin/src/HAL/STM32/HAL_SPI.cpp @@ -20,7 +20,9 @@ * along with this program. If not, see . * */ -#if defined(ARDUINO_ARCH_STM32) && !defined(STM32GENERIC) +#include "../platforms.h" + +#ifdef HAL_STM32 #include "../../inc/MarlinConfig.h" @@ -45,7 +47,9 @@ static SPISettings spiConfig; #include "../shared/Delay.h" void spiBegin(void) { - OUT_WRITE(SD_SS_PIN, HIGH); + #if PIN_EXISTS(SD_SS) + OUT_WRITE(SD_SS_PIN, HIGH); + #endif OUT_WRITE(SD_SCK_PIN, HIGH); SET_INPUT(SD_MISO_PIN); OUT_WRITE(SD_MOSI_PIN, HIGH); @@ -98,9 +102,9 @@ static SPISettings spiConfig; // Soft SPI receive byte uint8_t spiRec() { - DISABLE_ISRS(); // No interrupts during byte receive + hal.isr_off(); // No interrupts during byte receive const uint8_t data = HAL_SPI_STM32_SpiTransfer_Mode_3(0xFF); - ENABLE_ISRS(); // Enable interrupts + hal.isr_on(); // Enable interrupts return data; } @@ -112,9 +116,9 @@ static SPISettings spiConfig; // Soft SPI send byte void spiSend(uint8_t data) { - DISABLE_ISRS(); // No interrupts during byte send + hal.isr_off(); // No interrupts during byte send HAL_SPI_STM32_SpiTransfer_Mode_3(data); // Don't care what is received - ENABLE_ISRS(); // Enable interrupts + hal.isr_on(); // Enable interrupts } // Soft SPI send block @@ -163,11 +167,9 @@ static SPISettings spiConfig; } spiConfig = SPISettings(clock, MSBFIRST, SPI_MODE0); - #if ENABLED(CUSTOM_SPI_PINS) - SPI.setMISO(SD_MISO_PIN); - SPI.setMOSI(SD_MOSI_PIN); - SPI.setSCLK(SD_SCK_PIN); - #endif + SPI.setMISO(SD_MISO_PIN); + SPI.setMOSI(SD_MOSI_PIN); + SPI.setSCLK(SD_SCK_PIN); SPI.begin(); } @@ -193,7 +195,7 @@ static SPISettings spiConfig; * * @details Uses DMA */ - void spiRead(uint8_t* buf, uint16_t nbyte) { + void spiRead(uint8_t *buf, uint16_t nbyte) { if (nbyte == 0) return; memset(buf, 0xFF, nbyte); SPI.transfer(buf, nbyte); @@ -218,7 +220,7 @@ static SPISettings spiConfig; * * @details Use DMA */ - void spiSendBlock(uint8_t token, const uint8_t* buf) { + void spiSendBlock(uint8_t token, const uint8_t *buf) { uint8_t rxBuf[512]; SPI.transfer(token); SPI.transfer((uint8_t*)buf, &rxBuf, 512); @@ -226,4 +228,4 @@ static SPISettings spiConfig; #endif // SOFTWARE_SPI -#endif // ARDUINO_ARCH_STM32 && !STM32GENERIC +#endif // HAL_STM32 diff --git a/Marlin/src/HAL/STM32/MarlinSPI.cpp b/Marlin/src/HAL/STM32/MarlinSPI.cpp index 5086b4178461..f7c603d77e5c 100644 --- a/Marlin/src/HAL/STM32/MarlinSPI.cpp +++ b/Marlin/src/HAL/STM32/MarlinSPI.cpp @@ -19,7 +19,10 @@ * along with this program. If not, see . * */ -#if defined(ARDUINO_ARCH_STM32) && !defined(STM32GENERIC) + +#include "../platforms.h" + +#if defined(HAL_STM32) && !defined(STM32H7xx) #include "MarlinSPI.h" @@ -111,16 +114,19 @@ byte MarlinSPI::transfer(uint8_t _data) { return rxData; } +__STATIC_INLINE void LL_SPI_EnableDMAReq_RX(SPI_TypeDef *SPIx) { SET_BIT(SPIx->CR2, SPI_CR2_RXDMAEN); } +__STATIC_INLINE void LL_SPI_EnableDMAReq_TX(SPI_TypeDef *SPIx) { SET_BIT(SPIx->CR2, SPI_CR2_TXDMAEN); } + uint8_t MarlinSPI::dmaTransfer(const void *transmitBuf, void *receiveBuf, uint16_t length) { const uint8_t ff = 0xFF; - //if ((hspi->Instance->CR1 & SPI_CR1_SPE) != SPI_CR1_SPE) //only enable if disabled + //if (!LL_SPI_IsEnabled(_spi.handle)) // only enable if disabled __HAL_SPI_ENABLE(&_spi.handle); if (receiveBuf) { setupDma(_spi.handle, _dmaRx, DMA_PERIPH_TO_MEMORY, true); HAL_DMA_Start(&_dmaRx, (uint32_t)&(_spi.handle.Instance->DR), (uint32_t)receiveBuf, length); - SET_BIT(_spi.handle.Instance->CR2, SPI_CR2_RXDMAEN); /* Enable Rx DMA Request */ + LL_SPI_EnableDMAReq_RX(_spi.handle.Instance); // Enable Rx DMA Request } // check for 2 lines transfer @@ -133,7 +139,7 @@ uint8_t MarlinSPI::dmaTransfer(const void *transmitBuf, void *receiveBuf, uint16 if (transmitBuf) { setupDma(_spi.handle, _dmaTx, DMA_MEMORY_TO_PERIPH, mincTransmit); HAL_DMA_Start(&_dmaTx, (uint32_t)transmitBuf, (uint32_t)&(_spi.handle.Instance->DR), length); - SET_BIT(_spi.handle.Instance->CR2, SPI_CR2_TXDMAEN); /* Enable Tx DMA Request */ + LL_SPI_EnableDMAReq_TX(_spi.handle.Instance); // Enable Tx DMA Request } if (transmitBuf) { @@ -157,7 +163,7 @@ uint8_t MarlinSPI::dmaSend(const void * transmitBuf, uint16_t length, bool minc) setupDma(_spi.handle, _dmaTx, DMA_MEMORY_TO_PERIPH, minc); HAL_DMA_Start(&_dmaTx, (uint32_t)transmitBuf, (uint32_t)&(_spi.handle.Instance->DR), length); __HAL_SPI_ENABLE(&_spi.handle); - SET_BIT(_spi.handle.Instance->CR2, SPI_CR2_TXDMAEN); /* Enable Tx DMA Request */ + LL_SPI_EnableDMAReq_TX(_spi.handle.Instance); // Enable Tx DMA Request HAL_DMA_PollForTransfer(&_dmaTx, HAL_DMA_FULL_TRANSFER, HAL_MAX_DELAY); HAL_DMA_Abort(&_dmaTx); // DeInit objects @@ -165,4 +171,4 @@ uint8_t MarlinSPI::dmaSend(const void * transmitBuf, uint16_t length, bool minc) return 1; } -#endif // ARDUINO_ARCH_STM32 && !STM32GENERIC +#endif // HAL_STM32 && !STM32H7xx diff --git a/Marlin/src/HAL/STM32/MarlinSerial.cpp b/Marlin/src/HAL/STM32/MarlinSerial.cpp index cfb13f5bb561..37a8f40fd0b1 100644 --- a/Marlin/src/HAL/STM32/MarlinSerial.cpp +++ b/Marlin/src/HAL/STM32/MarlinSerial.cpp @@ -2,6 +2,9 @@ * Marlin 3D Printer Firmware * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * * This program is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation, either version 3 of the License, or @@ -16,7 +19,10 @@ * along with this program. If not, see . * */ -#if defined(ARDUINO_ARCH_STM32) && !defined(STM32GENERIC) + +#include "../platforms.h" + +#ifdef HAL_STM32 #include "../../inc/MarlinConfig.h" #include "MarlinSerial.h" @@ -28,7 +34,6 @@ #ifndef USART4 #define USART4 UART4 #endif - #ifndef USART5 #define USART5 UART5 #endif @@ -38,22 +43,38 @@ MSerialT MSerial ## ser_num (true, USART ## ser_num, &_rx_complete_irq_ ## ser_num); \ void _rx_complete_irq_ ## ser_num (serial_t * obj) { MSerial ## ser_num ._rx_complete_irq(obj); } -#define DECLARE_SERIAL_PORT_EXP(ser_num) DECLARE_SERIAL_PORT(ser_num) - -#if defined(SERIAL_PORT) && SERIAL_PORT >= 0 - DECLARE_SERIAL_PORT_EXP(SERIAL_PORT) +#if USING_HW_SERIAL1 + DECLARE_SERIAL_PORT(1) #endif - -#if defined(SERIAL_PORT_2) && SERIAL_PORT_2 >= 0 - DECLARE_SERIAL_PORT_EXP(SERIAL_PORT_2) +#if USING_HW_SERIAL2 + DECLARE_SERIAL_PORT(2) #endif - -#if defined(MMU2_SERIAL_PORT) && MMU2_SERIAL_PORT >= 0 - DECLARE_SERIAL_PORT_EXP(MMU2_SERIAL_PORT) +#if USING_HW_SERIAL3 + DECLARE_SERIAL_PORT(3) #endif - -#if defined(LCD_SERIAL_PORT) && LCD_SERIAL_PORT >= 0 - DECLARE_SERIAL_PORT_EXP(LCD_SERIAL_PORT) +#if USING_HW_SERIAL4 + DECLARE_SERIAL_PORT(4) +#endif +#if USING_HW_SERIAL5 + DECLARE_SERIAL_PORT(5) +#endif +#if USING_HW_SERIAL6 + DECLARE_SERIAL_PORT(6) +#endif +#if USING_HW_SERIAL7 + DECLARE_SERIAL_PORT(7) +#endif +#if USING_HW_SERIAL8 + DECLARE_SERIAL_PORT(8) +#endif +#if USING_HW_SERIAL9 + DECLARE_SERIAL_PORT(9) +#endif +#if USING_HW_SERIAL10 + DECLARE_SERIAL_PORT(10) +#endif +#if USING_HW_SERIALLP1 + DECLARE_SERIAL_PORT(LP1) #endif void MarlinSerial::begin(unsigned long baud, uint8_t config) { @@ -86,4 +107,4 @@ void MarlinSerial::_rx_complete_irq(serial_t *obj) { } } -#endif // ARDUINO_ARCH_STM32 && !STM32GENERIC +#endif // HAL_STM32 diff --git a/Marlin/src/HAL/STM32/MarlinSerial.h b/Marlin/src/HAL/STM32/MarlinSerial.h index 8cc4f0dd4cb8..bf861fb8a79a 100644 --- a/Marlin/src/HAL/STM32/MarlinSerial.h +++ b/Marlin/src/HAL/STM32/MarlinSerial.h @@ -2,6 +2,9 @@ * Marlin 3D Printer Firmware * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * * This program is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation, either version 3 of the License, or @@ -29,20 +32,20 @@ typedef void (*usart_rx_callback_t)(serial_t * obj); struct MarlinSerial : public HardwareSerial { - MarlinSerial(void* peripheral, usart_rx_callback_t rx_callback) : + MarlinSerial(void *peripheral, usart_rx_callback_t rx_callback) : HardwareSerial(peripheral), _rx_callback(rx_callback) { } void begin(unsigned long baud, uint8_t config); inline void begin(unsigned long baud) { begin(baud, SERIAL_8N1); } - void _rx_complete_irq(serial_t* obj); + void _rx_complete_irq(serial_t *obj); protected: usart_rx_callback_t _rx_callback; }; -typedef Serial0Type MSerialT; +typedef Serial1Class MSerialT; extern MSerialT MSerial1; extern MSerialT MSerial2; extern MSerialT MSerial3; diff --git a/Marlin/src/HAL/STM32/Sd2Card_sdio_stm32duino.cpp b/Marlin/src/HAL/STM32/Sd2Card_sdio_stm32duino.cpp deleted file mode 100644 index fc9b960c1c49..000000000000 --- a/Marlin/src/HAL/STM32/Sd2Card_sdio_stm32duino.cpp +++ /dev/null @@ -1,328 +0,0 @@ -/** - * Marlin 3D Printer Firmware - * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] - * - * Based on Sprinter and grbl. - * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm - * - * This program is free software: you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation, either version 3 of the License, or - * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with this program. If not, see . - * - */ -#if defined(ARDUINO_ARCH_STM32) && !defined(STM32GENERIC) - -#include "../../inc/MarlinConfig.h" - -#if ENABLED(SDIO_SUPPORT) - -#include -#include - -#if NONE(STM32F103xE, STM32F103xG, STM32F4xx, STM32F7xx) - #error "ERROR - Only STM32F103xE, STM32F103xG, STM32F4xx or STM32F7xx CPUs supported" -#endif - -#if HAS_SD_HOST_DRIVE - - // use USB drivers - - extern "C" { int8_t SD_MSC_Read(uint8_t lun, uint8_t *buf, uint32_t blk_addr, uint16_t blk_len); - int8_t SD_MSC_Write(uint8_t lun, uint8_t *buf, uint32_t blk_addr, uint16_t blk_len); - extern SD_HandleTypeDef hsd; - } - - bool SDIO_Init() { - return hsd.State == HAL_SD_STATE_READY; // return pass/fail status - } - - bool SDIO_ReadBlock(uint32_t block, uint8_t *src) { - int8_t status = SD_MSC_Read(0, (uint8_t*)src, block, 1); // read one 512 byte block - return (bool) status; - } - - bool SDIO_WriteBlock(uint32_t block, const uint8_t *src) { - int8_t status = SD_MSC_Write(0, (uint8_t*)src, block, 1); // write one 512 byte block - return (bool) status; - } - -#else // !USBD_USE_CDC_COMPOSITE - - // use local drivers - #if defined(STM32F103xE) || defined(STM32F103xG) - #include - #include - #elif defined(STM32F4xx) - #include - #include - #include - #include - #elif defined(STM32F7xx) - #include - #include - #include - #include - #else - #error "ERROR - Only STM32F103xE, STM32F103xG, STM32F4xx or STM32F7xx CPUs supported" - #endif - - SD_HandleTypeDef hsd; // create SDIO structure - - /* - SDIO_INIT_CLK_DIV is 118 - SDIO clock frequency is 48MHz / (TRANSFER_CLOCK_DIV + 2) - SDIO init clock frequency should not exceed 400KHz = 48MHz / (118 + 2) - - Default TRANSFER_CLOCK_DIV is 2 (118 / 40) - Default SDIO clock frequency is 48MHz / (2 + 2) = 12 MHz - This might be too fast for stable SDIO operations - - MKS Robin board seems to have stable SDIO with BusWide 1bit and ClockDiv 8 i.e. 4.8MHz SDIO clock frequency - Additional testing is required as there are clearly some 4bit initialization problems - */ - - #ifndef USBD_OK - #define USBD_OK 0 - #endif - - // Target Clock, configurable. Default is 18MHz, from STM32F1 - #ifndef SDIO_CLOCK - #define SDIO_CLOCK 18000000 /* 18 MHz */ - #endif - - // SDIO retries, configurable. Default is 3, from STM32F1 - #ifndef SDIO_READ_RETRIES - #define SDIO_READ_RETRIES 3 - #endif - - // SDIO Max Clock (naming from STM Manual, don't change) - #define SDIOCLK 48000000 - - static uint32_t clock_to_divider(uint32_t clk) { - // limit the SDIO master clock to 8/3 of PCLK2. See STM32 Manuals - // Also limited to no more than 48Mhz (SDIOCLK). - const uint32_t pclk2 = HAL_RCC_GetPCLK2Freq(); - clk = min(clk, (uint32_t)(pclk2 * 8 / 3)); - clk = min(clk, (uint32_t)SDIOCLK); - // Round up divider, so we don't run the card over the speed supported, - // and subtract by 2, because STM32 will add 2, as written in the manual: - // SDIO_CK frequency = SDIOCLK / [CLKDIV + 2] - return pclk2 / clk + (pclk2 % clk != 0) - 2; - } - - void go_to_transfer_speed() { - SD_InitTypeDef Init; - - /* Default SDIO peripheral configuration for SD card initialization */ - Init.ClockEdge = hsd.Init.ClockEdge; - Init.ClockBypass = hsd.Init.ClockBypass; - Init.ClockPowerSave = hsd.Init.ClockPowerSave; - Init.BusWide = hsd.Init.BusWide; - Init.HardwareFlowControl = hsd.Init.HardwareFlowControl; - Init.ClockDiv = clock_to_divider(SDIO_CLOCK); - - /* Initialize SDIO peripheral interface with default configuration */ - SDIO_Init(hsd.Instance, Init); - } - - void SD_LowLevel_Init(void) { - uint32_t tempreg; - - __HAL_RCC_SDIO_CLK_ENABLE(); - __HAL_RCC_GPIOC_CLK_ENABLE(); //enable GPIO clocks - __HAL_RCC_GPIOD_CLK_ENABLE(); //enable GPIO clocks - - GPIO_InitTypeDef GPIO_InitStruct; - - GPIO_InitStruct.Mode = GPIO_MODE_AF_PP; - GPIO_InitStruct.Pull = 1; //GPIO_NOPULL; - GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_HIGH; - - #if DISABLED(STM32F1xx) - GPIO_InitStruct.Alternate = GPIO_AF12_SDIO; - #endif - - GPIO_InitStruct.Pin = GPIO_PIN_8 | GPIO_PIN_12; // D0 & SCK - HAL_GPIO_Init(GPIOC, &GPIO_InitStruct); - - #if PINS_EXIST(SDIO_D1, SDIO_D2, SDIO_D3) // define D1-D3 only if have a four bit wide SDIO bus - GPIO_InitStruct.Pin = GPIO_PIN_9 | GPIO_PIN_10 | GPIO_PIN_11; // D1-D3 - HAL_GPIO_Init(GPIOC, &GPIO_InitStruct); - #endif - - // Configure PD.02 CMD line - GPIO_InitStruct.Pin = GPIO_PIN_2; - HAL_GPIO_Init(GPIOD, &GPIO_InitStruct); - - #if DISABLED(STM32F1xx) - // TODO: use __HAL_RCC_SDIO_RELEASE_RESET() and __HAL_RCC_SDIO_CLK_ENABLE(); - RCC->APB2RSTR &= ~RCC_APB2RSTR_SDIORST_Msk; // take SDIO out of reset - RCC->APB2ENR |= RCC_APB2RSTR_SDIORST_Msk; // enable SDIO clock - // Enable the DMA2 Clock - #endif - - //Initialize the SDIO (with initial <400Khz Clock) - tempreg = 0; //Reset value - tempreg |= SDIO_CLKCR_CLKEN; // Clock enabled - tempreg |= SDIO_INIT_CLK_DIV; // Clock Divider. Clock = 48000 / (118 + 2) = 400Khz - // Keep the rest at 0 => HW_Flow Disabled, Rising Clock Edge, Disable CLK ByPass, Bus Width = 0, Power save Disable - SDIO->CLKCR = tempreg; - - // Power up the SDIO - SDIO_PowerState_ON(SDIO); - } - - void HAL_SD_MspInit(SD_HandleTypeDef *hsd) { // application specific init - UNUSED(hsd); // Prevent unused argument(s) compilation warning - __HAL_RCC_SDIO_CLK_ENABLE(); // turn on SDIO clock - } - - bool SDIO_Init() { - uint8_t retryCnt = SDIO_READ_RETRIES; - - bool status; - hsd.Instance = SDIO; - hsd.State = HAL_SD_STATE_RESET; - - SD_LowLevel_Init(); - - uint8_t retry_Cnt = retryCnt; - for (;;) { - TERN_(USE_WATCHDOG, HAL_watchdog_refresh()); - status = (bool) HAL_SD_Init(&hsd); - if (!status) break; - if (!--retry_Cnt) return false; // return failing status if retries are exhausted - } - - go_to_transfer_speed(); - - #if PINS_EXIST(SDIO_D1, SDIO_D2, SDIO_D3) // go to 4 bit wide mode if pins are defined - retry_Cnt = retryCnt; - for (;;) { - TERN_(USE_WATCHDOG, HAL_watchdog_refresh()); - if (!HAL_SD_ConfigWideBusOperation(&hsd, SDIO_BUS_WIDE_4B)) break; // some cards are only 1 bit wide so a pass here is not required - if (!--retry_Cnt) break; - } - if (!retry_Cnt) { // wide bus failed, go back to one bit wide mode - hsd.State = (HAL_SD_StateTypeDef) 0; // HAL_SD_STATE_RESET - SD_LowLevel_Init(); - retry_Cnt = retryCnt; - for (;;) { - TERN_(USE_WATCHDOG, HAL_watchdog_refresh()); - status = (bool) HAL_SD_Init(&hsd); - if (!status) break; - if (!--retry_Cnt) return false; // return failing status if retries are exhausted - } - } - #endif - - return true; - } - /* - void init_SDIO_pins(void) { - GPIO_InitTypeDef GPIO_InitStruct = {0}; - - // SDIO GPIO Configuration - // PC8 ------> SDIO_D0 - // PC12 ------> SDIO_CK - // PD2 ------> SDIO_CMD - - GPIO_InitStruct.Pin = GPIO_PIN_8; - GPIO_InitStruct.Mode = GPIO_MODE_AF_PP; - GPIO_InitStruct.Pull = GPIO_NOPULL; - GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_VERY_HIGH; - GPIO_InitStruct.Alternate = GPIO_AF12_SDIO; - HAL_GPIO_Init(GPIOC, &GPIO_InitStruct); - - GPIO_InitStruct.Pin = GPIO_PIN_12; - GPIO_InitStruct.Mode = GPIO_MODE_AF_PP; - GPIO_InitStruct.Pull = GPIO_NOPULL; - GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_VERY_HIGH; - GPIO_InitStruct.Alternate = GPIO_AF12_SDIO; - HAL_GPIO_Init(GPIOC, &GPIO_InitStruct); - - GPIO_InitStruct.Pin = GPIO_PIN_2; - GPIO_InitStruct.Mode = GPIO_MODE_AF_PP; - GPIO_InitStruct.Pull = GPIO_NOPULL; - GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_VERY_HIGH; - GPIO_InitStruct.Alternate = GPIO_AF12_SDIO; - HAL_GPIO_Init(GPIOD, &GPIO_InitStruct); - } - */ - //bool SDIO_init() { return (bool) (SD_SDIO_Init() ? 1 : 0);} - //bool SDIO_Init_C() { return (bool) (SD_SDIO_Init() ? 1 : 0);} - - bool SDIO_ReadBlock(uint32_t block, uint8_t *dst) { - hsd.Instance = SDIO; - uint8_t retryCnt = SDIO_READ_RETRIES; - - bool status; - for (;;) { - TERN_(USE_WATCHDOG, HAL_watchdog_refresh()); - status = (bool) HAL_SD_ReadBlocks(&hsd, (uint8_t*)dst, block, 1, 1000); // read one 512 byte block with 500mS timeout - status |= (bool) HAL_SD_GetCardState(&hsd); // make sure all is OK - if (!status) break; // return passing status - if (!--retryCnt) break; // return failing status if retries are exhausted - } - return status; - - /* - return (bool) ((status_read | status_card) ? 1 : 0); - - if (SDIO_GetCardState() != SDIO_CARD_TRANSFER) return false; - if (blockAddress >= SdCard.LogBlockNbr) return false; - if ((0x03 & (uint32_t)data)) return false; // misaligned data - - if (SdCard.CardType != CARD_SDHC_SDXC) { blockAddress *= 512U; } - - if (!SDIO_CmdReadSingleBlock(blockAddress)) { - SDIO_CLEAR_FLAG(SDIO_ICR_CMD_FLAGS); - dma_disable(SDIO_DMA_DEV, SDIO_DMA_CHANNEL); - return false; - } - - while (!SDIO_GET_FLAG(SDIO_STA_DATAEND | SDIO_STA_TRX_ERROR_FLAGS)) {} - - dma_disable(SDIO_DMA_DEV, SDIO_DMA_CHANNEL); - - if (SDIO->STA & SDIO_STA_RXDAVL) { - while (SDIO->STA & SDIO_STA_RXDAVL) (void)SDIO->FIFO; - SDIO_CLEAR_FLAG(SDIO_ICR_CMD_FLAGS | SDIO_ICR_DATA_FLAGS); - return false; - } - - if (SDIO_GET_FLAG(SDIO_STA_TRX_ERROR_FLAGS)) { - SDIO_CLEAR_FLAG(SDIO_ICR_CMD_FLAGS | SDIO_ICR_DATA_FLAGS); - return false; - } - SDIO_CLEAR_FLAG(SDIO_ICR_CMD_FLAGS | SDIO_ICR_DATA_FLAGS); - */ - - return true; - } - - bool SDIO_WriteBlock(uint32_t block, const uint8_t *src) { - hsd.Instance = SDIO; - uint8_t retryCnt = SDIO_READ_RETRIES; - bool status; - for (;;) { - status = (bool) HAL_SD_WriteBlocks(&hsd, (uint8_t*)src, block, 1, 500); // write one 512 byte block with 500mS timeout - status |= (bool) HAL_SD_GetCardState(&hsd); // make sure all is OK - if (!status) break; // return passing status - if (!--retryCnt) break; // return failing status if retries are exhausted - } - return status; - } - -#endif // !USBD_USE_CDC_COMPOSITE -#endif // SDIO_SUPPORT -#endif // ARDUINO_ARCH_STM32 && !STM32GENERIC diff --git a/Marlin/src/HAL/STM32/Servo.cpp b/Marlin/src/HAL/STM32/Servo.cpp index 1cf117a05694..a00186e0e79e 100644 --- a/Marlin/src/HAL/STM32/Servo.cpp +++ b/Marlin/src/HAL/STM32/Servo.cpp @@ -20,7 +20,9 @@ * along with this program. If not, see . * */ -#if defined(ARDUINO_ARCH_STM32) && !defined(STM32GENERIC) +#include "../platforms.h" + +#ifdef HAL_STM32 #include "../../inc/MarlinConfig.h" @@ -37,7 +39,7 @@ static_assert(COUNT(servoDelay) == NUM_SERVOS, "SERVO_DELAY must be an array NUM // This allows all timer interrupt priorities to be managed from a single location in the HAL. static uint32_t servo_interrupt_priority = NVIC_EncodePriority(NVIC_GetPriorityGrouping(), TIM_IRQ_PRIO, TIM_IRQ_SUBPRIO); -// This must be called after the STM32 Servo class has intialized the timer. +// This must be called after the STM32 Servo class has initialized the timer. // It may only be needed after the first call to attach(), but it is possible // that is is necessary after every detach() call. To be safe this is currently // called after every call to attach(). @@ -107,4 +109,4 @@ void libServo::setInterruptPriority(uint32_t preemptPriority, uint32_t subPriori } #endif // HAS_SERVOS -#endif // ARDUINO_ARCH_STM32 && !STM32GENERIC +#endif // HAL_STM32 diff --git a/Marlin/src/HAL/STM32/eeprom_bl24cxx.cpp b/Marlin/src/HAL/STM32/eeprom_bl24cxx.cpp new file mode 100644 index 000000000000..f30b3dedb203 --- /dev/null +++ b/Marlin/src/HAL/STM32/eeprom_bl24cxx.cpp @@ -0,0 +1,85 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + * + */ + +#include "../platforms.h" + +#ifdef HAL_STM32 + +/** + * PersistentStore for Arduino-style EEPROM interface + * with simple implementations supplied by Marlin. + */ + +#include "../../inc/MarlinConfig.h" + +#if ENABLED(IIC_BL24CXX_EEPROM) + +#include "../shared/eeprom_if.h" +#include "../shared/eeprom_api.h" + +// +// PersistentStore +// + +#ifndef MARLIN_EEPROM_SIZE + #error "MARLIN_EEPROM_SIZE is required for IIC_BL24CXX_EEPROM." +#endif + +size_t PersistentStore::capacity() { return MARLIN_EEPROM_SIZE; } + +bool PersistentStore::access_start() { eeprom_init(); return true; } +bool PersistentStore::access_finish() { return true; } + +bool PersistentStore::write_data(int &pos, const uint8_t *value, size_t size, uint16_t *crc) { + uint16_t written = 0; + while (size--) { + uint8_t v = *value; + uint8_t * const p = (uint8_t * const)pos; + if (v != eeprom_read_byte(p)) { // EEPROM has only ~100,000 write cycles, so only write bytes that have changed! + eeprom_write_byte(p, v); + if (++written & 0x7F) delay(2); else safe_delay(2); // Avoid triggering watchdog during long EEPROM writes + if (eeprom_read_byte(p) != v) { + SERIAL_ECHO_MSG(STR_ERR_EEPROM_WRITE); + return true; + } + } + crc16(crc, &v, 1); + pos++; + value++; + } + return false; +} + +bool PersistentStore::read_data(int &pos, uint8_t *value, size_t size, uint16_t *crc, const bool writing/*=true*/) { + do { + uint8_t * const p = (uint8_t * const)pos; + uint8_t c = eeprom_read_byte(p); + if (writing) *value = c; + crc16(crc, &c, 1); + pos++; + value++; + } while (--size); + return false; +} + +#endif // IIC_BL24CXX_EEPROM +#endif // HAL_STM32 diff --git a/Marlin/src/HAL/STM32/eeprom_flash.cpp b/Marlin/src/HAL/STM32/eeprom_flash.cpp index 5862090f1ba4..7c8cc8dd21e1 100644 --- a/Marlin/src/HAL/STM32/eeprom_flash.cpp +++ b/Marlin/src/HAL/STM32/eeprom_flash.cpp @@ -20,7 +20,9 @@ * along with this program. If not, see . * */ -#if defined(ARDUINO_ARCH_STM32) && !defined(STM32GENERIC) +#include "../platforms.h" + +#ifdef HAL_STM32 #include "../../inc/MarlinConfig.h" @@ -28,6 +30,10 @@ #include "../shared/eeprom_api.h" +// Better: "utility/stm32_eeprom.h", but only after updating stm32duino to 2.0.0 +// Use EEPROM.h for compatibility, for now. +#include + /** * The STM32 HAL supports chips that deal with "pages" and some with "sectors" and some that * even have multiple "banks" of flash. @@ -100,6 +106,8 @@ size_t PersistentStore::capacity() { return MARLIN_EEPROM_SIZE; } bool PersistentStore::access_start() { + EEPROM.begin(); // Avoid STM32 EEPROM.h warning (do nothing) + #if ENABLED(FLASH_EEPROM_LEVELING) if (current_slot == -1 || eeprom_data_written) { @@ -117,7 +125,7 @@ bool PersistentStore::access_start() { address += sizeof(uint32_t); } if (current_slot == -1) { - // We didn't find anything, so we'll just intialize to empty + // We didn't find anything, so we'll just initialize to empty for (int i = 0; i < MARLIN_EEPROM_SIZE; i++) ram_eeprom[i] = EMPTY_UINT8; current_slot = EEPROM_SLOTS; } @@ -125,7 +133,7 @@ bool PersistentStore::access_start() { // load current settings uint8_t *eeprom_data = (uint8_t *)SLOT_ADDRESS(current_slot); for (int i = 0; i < MARLIN_EEPROM_SIZE; i++) ram_eeprom[i] = eeprom_data[i]; - DEBUG_ECHOLNPAIR("EEPROM loaded from slot ", current_slot, "."); + DEBUG_ECHOLNPGM("EEPROM loaded from slot ", current_slot, "."); } eeprom_data_written = false; } @@ -166,14 +174,14 @@ bool PersistentStore::access_finish() { UNLOCK_FLASH(); TERN_(HAS_PAUSE_SERVO_OUTPUT, PAUSE_SERVO_OUTPUT()); - DISABLE_ISRS(); + hal.isr_off(); status = HAL_FLASHEx_Erase(&EraseInitStruct, &SectorError); - ENABLE_ISRS(); + hal.isr_on(); TERN_(HAS_PAUSE_SERVO_OUTPUT, RESUME_SERVO_OUTPUT()); if (status != HAL_OK) { - DEBUG_ECHOLNPAIR("HAL_FLASHEx_Erase=", status); - DEBUG_ECHOLNPAIR("GetError=", HAL_FLASH_GetError()); - DEBUG_ECHOLNPAIR("SectorError=", SectorError); + DEBUG_ECHOLNPGM("HAL_FLASHEx_Erase=", status); + DEBUG_ECHOLNPGM("GetError=", HAL_FLASH_GetError()); + DEBUG_ECHOLNPGM("SectorError=", SectorError); LOCK_FLASH(); return false; } @@ -196,9 +204,9 @@ bool PersistentStore::access_finish() { offset += sizeof(uint32_t); } else { - DEBUG_ECHOLNPAIR("HAL_FLASH_Program=", status); - DEBUG_ECHOLNPAIR("GetError=", HAL_FLASH_GetError()); - DEBUG_ECHOLNPAIR("address=", address); + DEBUG_ECHOLNPGM("HAL_FLASH_Program=", status); + DEBUG_ECHOLNPGM("GetError=", HAL_FLASH_GetError()); + DEBUG_ECHOLNPGM("address=", address); success = false; break; } @@ -208,7 +216,7 @@ bool PersistentStore::access_finish() { if (success) { eeprom_data_written = false; - DEBUG_ECHOLNPAIR("EEPROM saved to slot ", current_slot, "."); + DEBUG_ECHOLNPGM("EEPROM saved to slot ", current_slot, "."); } return success; @@ -221,9 +229,9 @@ bool PersistentStore::access_finish() { // output. Servo output still glitches with interrupts disabled, but recovers after the // erase. TERN_(HAS_PAUSE_SERVO_OUTPUT, PAUSE_SERVO_OUTPUT()); - DISABLE_ISRS(); + hal.isr_off(); eeprom_buffer_flush(); - ENABLE_ISRS(); + hal.isr_on(); TERN_(HAS_PAUSE_SERVO_OUTPUT, RESUME_SERVO_OUTPUT()); eeprom_data_written = false; @@ -266,4 +274,4 @@ bool PersistentStore::read_data(int &pos, uint8_t *value, size_t size, uint16_t } #endif // FLASH_EEPROM_EMULATION -#endif // ARDUINO_ARCH_STM32 && !STM32GENERIC +#endif // HAL_STM32 diff --git a/Marlin/src/HAL/STM32/eeprom_if_iic.cpp b/Marlin/src/HAL/STM32/eeprom_if_iic.cpp new file mode 100644 index 000000000000..ad8712c0c075 --- /dev/null +++ b/Marlin/src/HAL/STM32/eeprom_if_iic.cpp @@ -0,0 +1,56 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + * + */ + +#include "../platforms.h" + +#ifdef HAL_STM32 + +/** + * Platform-independent Arduino functions for I2C EEPROM. + * Enable USE_SHARED_EEPROM if not supplied by the framework. + */ + +#include "../../inc/MarlinConfig.h" + +#if ENABLED(IIC_BL24CXX_EEPROM) + +#include "../../libs/BL24CXX.h" +#include "../shared/eeprom_if.h" + +void eeprom_init() { BL24CXX::init(); } + +// ------------------------ +// Public functions +// ------------------------ + +void eeprom_write_byte(uint8_t *pos, uint8_t value) { + const unsigned eeprom_address = (unsigned)pos; + return BL24CXX::writeOneByte(eeprom_address, value); +} + +uint8_t eeprom_read_byte(uint8_t *pos) { + const unsigned eeprom_address = (unsigned)pos; + return BL24CXX::readOneByte(eeprom_address); +} + +#endif // IIC_BL24CXX_EEPROM +#endif // HAL_STM32 diff --git a/Marlin/src/HAL/STM32/eeprom_sdcard.cpp b/Marlin/src/HAL/STM32/eeprom_sdcard.cpp index f811468fb484..473b656f9a3c 100644 --- a/Marlin/src/HAL/STM32/eeprom_sdcard.cpp +++ b/Marlin/src/HAL/STM32/eeprom_sdcard.cpp @@ -19,7 +19,10 @@ * along with this program. If not, see . * */ -#if defined(ARDUINO_ARCH_STM32) && !defined(STM32GENERIC) + +#include "../platforms.h" + +#ifdef HAL_STM32 /** * Implementation of EEPROM settings in SD Card @@ -88,4 +91,4 @@ bool PersistentStore::read_data(int &pos, uint8_t *value, const size_t size, uin } #endif // SDCARD_EEPROM_EMULATION -#endif // ARDUINO_ARCH_STM32 && !STM32GENERIC +#endif // HAL_STM32 diff --git a/Marlin/src/HAL/STM32/eeprom_sram.cpp b/Marlin/src/HAL/STM32/eeprom_sram.cpp index 135bcabde9aa..687e7f55c226 100644 --- a/Marlin/src/HAL/STM32/eeprom_sram.cpp +++ b/Marlin/src/HAL/STM32/eeprom_sram.cpp @@ -20,7 +20,9 @@ * along with this program. If not, see . * */ -#if defined(ARDUINO_ARCH_STM32) && !defined(STM32GENERIC) +#include "../platforms.h" + +#ifdef HAL_STM32 #include "../../inc/MarlinConfig.h" @@ -65,4 +67,4 @@ bool PersistentStore::read_data(int &pos, uint8_t *value, size_t size, uint16_t } #endif // SRAM_EEPROM_EMULATION -#endif // ARDUINO_ARCH_STM32 && !STM32GENERIC +#endif // HAL_STM32 diff --git a/Marlin/src/HAL/STM32/eeprom_wired.cpp b/Marlin/src/HAL/STM32/eeprom_wired.cpp index ad54c12c471f..cf0468151e5e 100644 --- a/Marlin/src/HAL/STM32/eeprom_wired.cpp +++ b/Marlin/src/HAL/STM32/eeprom_wired.cpp @@ -20,7 +20,9 @@ * along with this program. If not, see . * */ -#if defined(ARDUINO_ARCH_STM32) && !defined(STM32GENERIC) +#include "../platforms.h" + +#ifdef HAL_STM32 #include "../../inc/MarlinConfig.h" @@ -43,25 +45,22 @@ bool PersistentStore::access_start() { eeprom_init(); return true; } bool PersistentStore::access_finish() { return true; } bool PersistentStore::write_data(int &pos, const uint8_t *value, size_t size, uint16_t *crc) { + uint16_t written = 0; while (size--) { uint8_t v = *value; - - // EEPROM has only ~100,000 write cycles, - // so only write bytes that have changed! uint8_t * const p = (uint8_t * const)pos; - if (v != eeprom_read_byte(p)) { + if (v != eeprom_read_byte(p)) { // EEPROM has only ~100,000 write cycles, so only write bytes that have changed! eeprom_write_byte(p, v); + if (++written & 0x7F) delay(2); else safe_delay(2); // Avoid triggering watchdog during long EEPROM writes if (eeprom_read_byte(p) != v) { SERIAL_ECHO_MSG(STR_ERR_EEPROM_WRITE); return true; } } - crc16(crc, &v, 1); pos++; value++; - }; - + } return false; } @@ -78,4 +77,4 @@ bool PersistentStore::read_data(int &pos, uint8_t *value, size_t size, uint16_t } #endif // USE_WIRED_EEPROM -#endif // ARDUINO_ARCH_STM32 && !STM32GENERIC +#endif // HAL_STM32 diff --git a/Marlin/src/HAL/STM32/endstop_interrupts.h b/Marlin/src/HAL/STM32/endstop_interrupts.h index fdff8cc644cd..90870881fe66 100644 --- a/Marlin/src/HAL/STM32/endstop_interrupts.h +++ b/Marlin/src/HAL/STM32/endstop_interrupts.h @@ -46,4 +46,10 @@ void setup_endstop_interrupts() { TERN_(HAS_Z4_MAX, _ATTACH(Z4_MAX_PIN)); TERN_(HAS_Z4_MIN, _ATTACH(Z4_MIN_PIN)); TERN_(HAS_Z_MIN_PROBE_PIN, _ATTACH(Z_MIN_PROBE_PIN)); + TERN_(HAS_I_MAX, _ATTACH(I_MAX_PIN)); + TERN_(HAS_I_MIN, _ATTACH(I_MIN_PIN)); + TERN_(HAS_J_MAX, _ATTACH(J_MAX_PIN)); + TERN_(HAS_J_MIN, _ATTACH(J_MIN_PIN)); + TERN_(HAS_K_MAX, _ATTACH(K_MAX_PIN)); + TERN_(HAS_K_MIN, _ATTACH(K_MIN_PIN)); } diff --git a/Marlin/src/HAL/STM32/fast_pwm.cpp b/Marlin/src/HAL/STM32/fast_pwm.cpp index 42eecb5e1a96..a0d8ecc612c2 100644 --- a/Marlin/src/HAL/STM32/fast_pwm.cpp +++ b/Marlin/src/HAL/STM32/fast_pwm.cpp @@ -19,41 +19,70 @@ * along with this program. If not, see . * */ -#if defined(ARDUINO_ARCH_STM32) && !defined(STM32GENERIC) -#include "../../inc/MarlinConfigPre.h" +#include "../platforms.h" -#if NEEDS_HARDWARE_PWM +#ifdef HAL_STM32 -#include "HAL.h" -#include "timers.h" +#include "../../inc/MarlinConfig.h" -void set_pwm_frequency(const pin_t pin, int f_desired) { - if (!PWM_PIN(pin)) return; // Don't proceed if no hardware timer +// Array to support sticky frequency sets per timer +static uint16_t timer_freq[TIMER_NUM]; - PinName pin_name = digitalPinToPinName(pin); - TIM_TypeDef *Instance = (TIM_TypeDef *)pinmap_peripheral(pin_name, PinMap_PWM); // Get HAL timer instance +void MarlinHAL::set_pwm_duty(const pin_t pin, const uint16_t v, const uint16_t v_size/*=255*/, const bool invert/*=false*/) { + const uint16_t duty = invert ? v_size - v : v; + if (PWM_PIN(pin)) { + const PinName pin_name = digitalPinToPinName(pin); + TIM_TypeDef * const Instance = (TIM_TypeDef *)pinmap_peripheral(pin_name, PinMap_PWM); - LOOP_S_L_N(i, 0, NUM_HARDWARE_TIMERS) // Protect used timers - if (timer_instance[i] && timer_instance[i]->getHandle()->Instance == Instance) - return; + const timer_index_t index = get_timer_index(Instance); + const bool needs_freq = (HardwareTimer_Handle[index] == nullptr); + if (needs_freq) // A new instance must be set to the default frequency of PWM_FREQUENCY + HardwareTimer_Handle[index]->__this = new HardwareTimer((TIM_TypeDef *)pinmap_peripheral(pin_name, PinMap_PWM)); - pwm_start(pin_name, f_desired, 0, RESOLUTION_8B_COMPARE_FORMAT); -} + HardwareTimer * const HT = (HardwareTimer *)(HardwareTimer_Handle[index]->__this); + const uint32_t channel = STM_PIN_CHANNEL(pinmap_function(pin_name, PinMap_PWM)); + const TimerModes_t previousMode = HT->getMode(channel); + if (previousMode != TIMER_OUTPUT_COMPARE_PWM1) + HT->setMode(channel, TIMER_OUTPUT_COMPARE_PWM1, pin); + + if (needs_freq && timer_freq[index] == 0) // If the timer is unconfigured and no freq is set then default PWM_FREQUENCY + set_pwm_frequency(pin_name, PWM_FREQUENCY); // Set the frequency and save the value to the assigned index no. -void set_pwm_duty(const pin_t pin, const uint16_t v, const uint16_t v_size/*=255*/, const bool invert/*=false*/) { - PinName pin_name = digitalPinToPinName(pin); - TIM_TypeDef *Instance = (TIM_TypeDef *)pinmap_peripheral(pin_name, PinMap_PWM); - uint16_t adj_val = Instance->ARR * v / v_size; - if (invert) adj_val = Instance->ARR - adj_val; - - switch (get_pwm_channel(pin_name)) { - case TIM_CHANNEL_1: LL_TIM_OC_SetCompareCH1(Instance, adj_val); break; - case TIM_CHANNEL_2: LL_TIM_OC_SetCompareCH2(Instance, adj_val); break; - case TIM_CHANNEL_3: LL_TIM_OC_SetCompareCH3(Instance, adj_val); break; - case TIM_CHANNEL_4: LL_TIM_OC_SetCompareCH4(Instance, adj_val); break; + // Note the resolution is sticky here, the input can be upto 16 bits and that would require RESOLUTION_16B_COMPARE_FORMAT (16) + // If such a need were to manifest then we would need to calc the resolution based on the v_size parameter and add code for it. + HT->setCaptureCompare(channel, duty, RESOLUTION_8B_COMPARE_FORMAT); // Set the duty, the calc is done in the library :) + pinmap_pinout(pin_name, PinMap_PWM); // Make sure the pin output state is set. + if (previousMode != TIMER_OUTPUT_COMPARE_PWM1) HT->resume(); } + else { + pinMode(pin, OUTPUT); + digitalWrite(pin, duty < v_size / 2 ? LOW : HIGH); + } +} + +void MarlinHAL::set_pwm_frequency(const pin_t pin, const uint16_t f_desired) { + if (!PWM_PIN(pin)) return; // Don't proceed if no hardware timer + const PinName pin_name = digitalPinToPinName(pin); + TIM_TypeDef * const Instance = (TIM_TypeDef *)pinmap_peripheral(pin_name, PinMap_PWM); // Get HAL timer instance + const timer_index_t index = get_timer_index(Instance); + + // Protect used timers. + #ifdef STEP_TIMER + if (index == TIMER_INDEX(STEP_TIMER)) return; + #endif + #ifdef TEMP_TIMER + if (index == TIMER_INDEX(TEMP_TIMER)) return; + #endif + #if defined(PULSE_TIMER) && MF_TIMER_PULSE != MF_TIMER_STEP + if (index == TIMER_INDEX(PULSE_TIMER)) return; + #endif + + if (HardwareTimer_Handle[index] == nullptr) // If frequency is set before duty we need to create a handle here. + HardwareTimer_Handle[index]->__this = new HardwareTimer((TIM_TypeDef *)pinmap_peripheral(pin_name, PinMap_PWM)); + HardwareTimer * const HT = (HardwareTimer *)(HardwareTimer_Handle[index]->__this); + HT->setOverflow(f_desired, HERTZ_FORMAT); + timer_freq[index] = f_desired; // Save the last frequency so duty will not set the default for this timer number. } -#endif // NEEDS_HARDWARE_PWM -#endif // ARDUINO_ARCH_STM32 && !STM32GENERIC +#endif // HAL_STM32 diff --git a/Marlin/src/HAL/STM32/fastio.cpp b/Marlin/src/HAL/STM32/fastio.cpp index 0d55579d8886..b34555b8c841 100644 --- a/Marlin/src/HAL/STM32/fastio.cpp +++ b/Marlin/src/HAL/STM32/fastio.cpp @@ -20,15 +20,17 @@ * along with this program. If not, see . * */ -#if defined(ARDUINO_ARCH_STM32) && !defined(STM32GENERIC) +#include "../platforms.h" + +#ifdef HAL_STM32 #include "../../inc/MarlinConfig.h" -GPIO_TypeDef* FastIOPortMap[LastPort + 1]; +GPIO_TypeDef* FastIOPortMap[LastPort + 1] = { 0 }; void FastIO_init() { LOOP_L_N(i, NUM_DIGITAL_PINS) FastIOPortMap[STM_PORT(digitalPin[i])] = get_GPIO_Port(STM_PORT(digitalPin[i])); } -#endif +#endif // HAL_STM32 diff --git a/Marlin/src/HAL/STM32/fastio.h b/Marlin/src/HAL/STM32/fastio.h index 17751c44dd8d..4a489544716f 100644 --- a/Marlin/src/HAL/STM32/fastio.h +++ b/Marlin/src/HAL/STM32/fastio.h @@ -38,6 +38,7 @@ extern GPIO_TypeDef * FastIOPortMap[]; // ------------------------ void FastIO_init(); // Must be called before using fast io macros +#define FASTIO_INIT() FastIO_init() // ------------------------ // Defines diff --git a/Marlin/src/HAL/STM32/inc/Conditionals_adv.h b/Marlin/src/HAL/STM32/inc/Conditionals_adv.h index 9c9a7014c7b4..451c94f25d1f 100644 --- a/Marlin/src/HAL/STM32/inc/Conditionals_adv.h +++ b/Marlin/src/HAL/STM32/inc/Conditionals_adv.h @@ -21,7 +21,7 @@ */ #pragma once -#if defined(USBD_USE_CDC_MSC) && DISABLED(NO_SD_HOST_DRIVE) +#if BOTH(SDSUPPORT, USBD_USE_CDC_MSC) && DISABLED(NO_SD_HOST_DRIVE) #define HAS_SD_HOST_DRIVE 1 #endif @@ -30,3 +30,6 @@ #undef F_CPU #define F_CPU BOARD_F_CPU #endif + +// The Sensitive Pins array is not optimizable +#define RUNTIME_ONLY_ANALOG_TO_DIGITAL diff --git a/Marlin/src/HAL/STM32/inc/SanityCheck.h b/Marlin/src/HAL/STM32/inc/SanityCheck.h index 7ee606af7f37..0f1a2acaa41c 100644 --- a/Marlin/src/HAL/STM32/inc/SanityCheck.h +++ b/Marlin/src/HAL/STM32/inc/SanityCheck.h @@ -24,7 +24,7 @@ /** * Test STM32-specific configuration values for errors at compile-time. */ -//#if ENABLED(SPINDLE_LASER_PWM) && !(SPINDLE_LASER_PWM_PIN == 4 || SPINDLE_LASER_PWM_PIN == 6 || SPINDLE_LASER_PWM_PIN == 11) +//#if ENABLED(SPINDLE_LASER_USE_PWM) && !(SPINDLE_LASER_PWM_PIN == 4 || SPINDLE_LASER_PWM_PIN == 6 || SPINDLE_LASER_PWM_PIN == 11) // #error "SPINDLE_LASER_PWM_PIN must use SERVO0, SERVO1 or SERVO3 connector" //#endif @@ -52,6 +52,6 @@ #error "SERIAL_STATS_DROPPED_RX is not supported on STM32." #endif -#if ANY(TFT_COLOR_UI, TFT_LVGL_UI, TFT_CLASSIC_UI) && NOT_TARGET(STM32F4xx, STM32F1xx) - #error "TFT_COLOR_UI, TFT_LVGL_UI and TFT_CLASSIC_UI are currently only supported on STM32F4 and STM32F1 hardware." +#if ANY(TFT_COLOR_UI, TFT_LVGL_UI, TFT_CLASSIC_UI) && NOT_TARGET(STM32H7xx, STM32F4xx, STM32F1xx) + #error "TFT_COLOR_UI, TFT_LVGL_UI and TFT_CLASSIC_UI are currently only supported on STM32H7, STM32F4 and STM32F1 hardware." #endif diff --git a/Marlin/src/HAL/STM32/msc_sd.cpp b/Marlin/src/HAL/STM32/msc_sd.cpp index 63ce7808f183..4f85af0d446a 100644 --- a/Marlin/src/HAL/STM32/msc_sd.cpp +++ b/Marlin/src/HAL/STM32/msc_sd.cpp @@ -13,71 +13,88 @@ * along with this program. If not, see . * */ +#include "../platforms.h" + +#ifdef HAL_STM32 + #include "../../inc/MarlinConfigPre.h" -#if defined(ARDUINO_ARCH_STM32) && !defined(STM32GENERIC) && HAS_SD_HOST_DRIVE +#if HAS_SD_HOST_DRIVE -#include "msc_sd.h" #include "../shared/Marduino.h" +#include "msc_sd.h" #include "usbd_core.h" + +#include "../../sd/cardreader.h" + #include #include #define BLOCK_SIZE 512 #define PRODUCT_ID 0x29 -#include "../../sd/cardreader.h" - class Sd2CardUSBMscHandler : public USBMscHandler { public: + DiskIODriver* diskIODriver() { + #if ENABLED(MULTI_VOLUME) + #if SHARED_VOLUME_IS(SD_ONBOARD) + return &card.media_driver_sdcard; + #elif SHARED_VOLUME_IS(USB_FLASH_DRIVE) + return &card.media_driver_usbFlash; + #endif + #else + return card.diskIODriver(); + #endif + } + bool GetCapacity(uint32_t *pBlockNum, uint16_t *pBlockSize) { - *pBlockNum = card.getSd2Card().cardSize(); + *pBlockNum = diskIODriver()->cardSize(); *pBlockSize = BLOCK_SIZE; return true; } bool Write(uint8_t *pBuf, uint32_t blkAddr, uint16_t blkLen) { - auto sd2card = card.getSd2Card(); + auto sd2card = diskIODriver(); // single block if (blkLen == 1) { watchdog_refresh(); - sd2card.writeBlock(blkAddr, pBuf); + sd2card->writeBlock(blkAddr, pBuf); return true; } - // multi block optmization - sd2card.writeStart(blkAddr, blkLen); + // multi block optimization + sd2card->writeStart(blkAddr, blkLen); while (blkLen--) { watchdog_refresh(); - sd2card.writeData(pBuf); + sd2card->writeData(pBuf); pBuf += BLOCK_SIZE; } - sd2card.writeStop(); + sd2card->writeStop(); return true; } bool Read(uint8_t *pBuf, uint32_t blkAddr, uint16_t blkLen) { - auto sd2card = card.getSd2Card(); + auto sd2card = diskIODriver(); // single block if (blkLen == 1) { watchdog_refresh(); - sd2card.readBlock(blkAddr, pBuf); + sd2card->readBlock(blkAddr, pBuf); return true; } - // multi block optmization - sd2card.readStart(blkAddr); + // multi block optimization + sd2card->readStart(blkAddr); while (blkLen--) { watchdog_refresh(); - sd2card.readData(pBuf); + sd2card->readData(pBuf); pBuf += BLOCK_SIZE; } - sd2card.readStop(); + sd2card->readStop(); return true; } bool IsReady() { - return card.isMounted(); + return diskIODriver()->isReady(); } }; @@ -105,8 +122,9 @@ USBMscHandler *pSingleMscHandler = &usbMscHandler; void MSC_SD_init() { USBDevice.end(); delay(200); - USBDevice.begin(); USBDevice.registerMscHandlers(1, &pSingleMscHandler, Marlin_STORAGE_Inquirydata); + USBDevice.begin(); } -#endif // __STM32F1__ && HAS_SD_HOST_DRIVE +#endif // HAS_SD_HOST_DRIVE +#endif // HAL_STM32 diff --git a/Marlin/src/HAL/STM32/pinsDebug.h b/Marlin/src/HAL/STM32/pinsDebug.h index 048f788e3d08..a7f022a0b62d 100644 --- a/Marlin/src/HAL/STM32/pinsDebug.h +++ b/Marlin/src/HAL/STM32/pinsDebug.h @@ -2,6 +2,9 @@ * Marlin 3D Printer Firmware * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * * This program is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation, either version 3 of the License, or @@ -109,9 +112,9 @@ const XrefInfo pin_xref[] PROGMEM = { #define VALID_PIN(ANUM) ((ANUM) >= 0 && (ANUM) < NUMBER_PINS_TOTAL) #define digitalRead_mod(Ard_num) extDigitalRead(Ard_num) // must use Arduino pin numbers when doing reads #define PRINT_PIN(Q) +#define PRINT_PIN_ANALOG(p) do{ sprintf_P(buffer, PSTR(" (A%2d) "), DIGITAL_PIN_TO_ANALOG_PIN(pin)); SERIAL_ECHO(buffer); }while(0) #define PRINT_PORT(ANUM) port_print(ANUM) #define DIGITAL_PIN_TO_ANALOG_PIN(ANUM) -1 // will report analog pin number in the print port routine -#define GET_PIN_MAP_PIN_M43(Index) pin_xref[Index].Ard_num // x is a variable used to search pin_array #define GET_ARRAY_IS_DIGITAL(x) ((bool) pin_array[x].is_digital) @@ -119,6 +122,11 @@ const XrefInfo pin_xref[] PROGMEM = { #define PRINT_ARRAY_NAME(x) do{ sprintf_P(buffer, PSTR("%-" STRINGIFY(MAX_NAME_LENGTH) "s"), pin_array[x].name); SERIAL_ECHO(buffer); }while(0) #define MULTI_NAME_PAD 33 // space needed to be pretty if not first name assigned to a pin +// +// Pin Mapping for M43 +// +#define GET_PIN_MAP_PIN_M43(Index) pin_xref[Index].Ard_num + #ifndef M43_NEVER_TOUCH #define _M43_NEVER_TOUCH(Index) (Index >= 9 && Index <= 12) // SERIAL/USB pins: PA9(TX) PA10(RX) PA11(USB_DM) PA12(USB_DP) #ifdef KILL_PIN @@ -236,7 +244,7 @@ void pwm_details(const pin_t Ard_num) { if (over_7) pin_number -= 8; uint8_t alt_func = (alt_all >> (4 * pin_number)) & 0x0F; - SERIAL_ECHOPAIR("Alt Function: ", alt_func); + SERIAL_ECHOPGM("Alt Function: ", alt_func); if (alt_func < 10) SERIAL_CHAR(' '); SERIAL_ECHOPGM(" - "); switch (alt_func) { diff --git a/Marlin/src/HAL/STM32/sdio.cpp b/Marlin/src/HAL/STM32/sdio.cpp new file mode 100644 index 000000000000..0af5f9040e54 --- /dev/null +++ b/Marlin/src/HAL/STM32/sdio.cpp @@ -0,0 +1,320 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + * + */ + +#include "../platforms.h" + +#ifdef HAL_STM32 + +#include "../../inc/MarlinConfig.h" + +#if ENABLED(SDIO_SUPPORT) + +#include "sdio.h" + +#include +#include + +// use local drivers +#if defined(STM32F103xE) || defined(STM32F103xG) + #include + #include +#elif defined(STM32F4xx) + #include + #include + #include + #include +#elif defined(STM32F7xx) + #include + #include + #include + #include +#else + #error "SDIO only supported with STM32F103xE, STM32F103xG, STM32F4xx, or STM32F7xx." +#endif + +SD_HandleTypeDef hsd; // create SDIO structure +// F4 supports one DMA for RX and another for TX, but Marlin will never +// do read and write at same time, so we use the same DMA for both. +DMA_HandleTypeDef hdma_sdio; + +/* + SDIO_INIT_CLK_DIV is 118 + SDIO clock frequency is 48MHz / (TRANSFER_CLOCK_DIV + 2) + SDIO init clock frequency should not exceed 400kHz = 48MHz / (118 + 2) + + Default TRANSFER_CLOCK_DIV is 2 (118 / 40) + Default SDIO clock frequency is 48MHz / (2 + 2) = 12 MHz + This might be too fast for stable SDIO operations + + MKS Robin board seems to have stable SDIO with BusWide 1bit and ClockDiv 8 i.e. 4.8MHz SDIO clock frequency + Additional testing is required as there are clearly some 4bit initialization problems +*/ + +#ifndef USBD_OK + #define USBD_OK 0 +#endif + +// Target Clock, configurable. Default is 18MHz, from STM32F1 +#ifndef SDIO_CLOCK + #define SDIO_CLOCK 18000000 // 18 MHz +#endif + +// SDIO retries, configurable. Default is 3, from STM32F1 +#ifndef SDIO_READ_RETRIES + #define SDIO_READ_RETRIES 3 +#endif + +// SDIO Max Clock (naming from STM Manual, don't change) +#define SDIOCLK 48000000 + +static uint32_t clock_to_divider(uint32_t clk) { + // limit the SDIO master clock to 8/3 of PCLK2. See STM32 Manuals + // Also limited to no more than 48Mhz (SDIOCLK). + const uint32_t pclk2 = HAL_RCC_GetPCLK2Freq(); + clk = min(clk, (uint32_t)(pclk2 * 8 / 3)); + clk = min(clk, (uint32_t)SDIOCLK); + // Round up divider, so we don't run the card over the speed supported, + // and subtract by 2, because STM32 will add 2, as written in the manual: + // SDIO_CK frequency = SDIOCLK / [CLKDIV + 2] + return pclk2 / clk + (pclk2 % clk != 0) - 2; +} + +void go_to_transfer_speed() { + /* Default SDIO peripheral configuration for SD card initialization */ + hsd.Init.ClockEdge = hsd.Init.ClockEdge; + hsd.Init.ClockBypass = hsd.Init.ClockBypass; + hsd.Init.ClockPowerSave = hsd.Init.ClockPowerSave; + hsd.Init.BusWide = hsd.Init.BusWide; + hsd.Init.HardwareFlowControl = hsd.Init.HardwareFlowControl; + hsd.Init.ClockDiv = clock_to_divider(SDIO_CLOCK); + + /* Initialize SDIO peripheral interface with default configuration */ + SDIO_Init(hsd.Instance, hsd.Init); +} + +void SD_LowLevel_Init(void) { + uint32_t tempreg; + + __HAL_RCC_GPIOC_CLK_ENABLE(); //enable GPIO clocks + __HAL_RCC_GPIOD_CLK_ENABLE(); //enable GPIO clocks + + GPIO_InitTypeDef GPIO_InitStruct; + + GPIO_InitStruct.Mode = GPIO_MODE_AF_PP; + GPIO_InitStruct.Pull = 1; //GPIO_NOPULL; + GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_HIGH; + + #if DISABLED(STM32F1xx) + GPIO_InitStruct.Alternate = GPIO_AF12_SDIO; + #endif + + GPIO_InitStruct.Pin = GPIO_PIN_8 | GPIO_PIN_12; // D0 & SCK + HAL_GPIO_Init(GPIOC, &GPIO_InitStruct); + + #if PINS_EXIST(SDIO_D1, SDIO_D2, SDIO_D3) // define D1-D3 only if have a four bit wide SDIO bus + GPIO_InitStruct.Pin = GPIO_PIN_9 | GPIO_PIN_10 | GPIO_PIN_11; // D1-D3 + HAL_GPIO_Init(GPIOC, &GPIO_InitStruct); + #endif + + // Configure PD.02 CMD line + GPIO_InitStruct.Pin = GPIO_PIN_2; + HAL_GPIO_Init(GPIOD, &GPIO_InitStruct); + + // Setup DMA + #if defined(STM32F1xx) + hdma_sdio.Init.Mode = DMA_NORMAL; + hdma_sdio.Instance = DMA2_Channel4; + HAL_NVIC_EnableIRQ(DMA2_Channel4_5_IRQn); + #elif defined(STM32F4xx) + hdma_sdio.Init.Mode = DMA_PFCTRL; + hdma_sdio.Instance = DMA2_Stream3; + hdma_sdio.Init.Channel = DMA_CHANNEL_4; + hdma_sdio.Init.FIFOMode = DMA_FIFOMODE_ENABLE; + hdma_sdio.Init.FIFOThreshold = DMA_FIFO_THRESHOLD_FULL; + hdma_sdio.Init.MemBurst = DMA_MBURST_INC4; + hdma_sdio.Init.PeriphBurst = DMA_PBURST_INC4; + HAL_NVIC_EnableIRQ(DMA2_Stream3_IRQn); + #endif + HAL_NVIC_EnableIRQ(SDIO_IRQn); + hdma_sdio.Init.PeriphInc = DMA_PINC_DISABLE; + hdma_sdio.Init.MemInc = DMA_MINC_ENABLE; + hdma_sdio.Init.PeriphDataAlignment = DMA_PDATAALIGN_WORD; + hdma_sdio.Init.MemDataAlignment = DMA_MDATAALIGN_WORD; + hdma_sdio.Init.Priority = DMA_PRIORITY_LOW; + __HAL_LINKDMA(&hsd, hdmarx, hdma_sdio); + __HAL_LINKDMA(&hsd, hdmatx, hdma_sdio); + + #if defined(STM32F1xx) + __HAL_RCC_SDIO_CLK_ENABLE(); + __HAL_RCC_DMA2_CLK_ENABLE(); + #else + __HAL_RCC_SDIO_FORCE_RESET(); + delay(2); + __HAL_RCC_SDIO_RELEASE_RESET(); + delay(2); + __HAL_RCC_SDIO_CLK_ENABLE(); + + __HAL_RCC_DMA2_FORCE_RESET(); + delay(2); + __HAL_RCC_DMA2_RELEASE_RESET(); + delay(2); + __HAL_RCC_DMA2_CLK_ENABLE(); + #endif + + //Initialize the SDIO (with initial <400Khz Clock) + tempreg = 0; //Reset value + tempreg |= SDIO_CLKCR_CLKEN; // Clock enabled + tempreg |= SDIO_INIT_CLK_DIV; // Clock Divider. Clock = 48000 / (118 + 2) = 400Khz + // Keep the rest at 0 => HW_Flow Disabled, Rising Clock Edge, Disable CLK ByPass, Bus Width = 0, Power save Disable + SDIO->CLKCR = tempreg; + + // Power up the SDIO + SDIO_PowerState_ON(SDIO); + hsd.Instance = SDIO; +} + +void HAL_SD_MspInit(SD_HandleTypeDef *hsd) { // application specific init + UNUSED(hsd); // Prevent unused argument(s) compilation warning + __HAL_RCC_SDIO_CLK_ENABLE(); // turn on SDIO clock +} + +bool SDIO_Init() { + uint8_t retryCnt = SDIO_READ_RETRIES; + + bool status; + hsd.Instance = SDIO; + hsd.State = HAL_SD_STATE_RESET; + + SD_LowLevel_Init(); + + uint8_t retry_Cnt = retryCnt; + for (;;) { + TERN_(USE_WATCHDOG, HAL_watchdog_refresh()); + status = (bool) HAL_SD_Init(&hsd); + if (!status) break; + if (!--retry_Cnt) return false; // return failing status if retries are exhausted + } + + go_to_transfer_speed(); + + #if PINS_EXIST(SDIO_D1, SDIO_D2, SDIO_D3) // go to 4 bit wide mode if pins are defined + retry_Cnt = retryCnt; + for (;;) { + TERN_(USE_WATCHDOG, HAL_watchdog_refresh()); + if (!HAL_SD_ConfigWideBusOperation(&hsd, SDIO_BUS_WIDE_4B)) break; // some cards are only 1 bit wide so a pass here is not required + if (!--retry_Cnt) break; + } + if (!retry_Cnt) { // wide bus failed, go back to one bit wide mode + hsd.State = (HAL_SD_StateTypeDef) 0; // HAL_SD_STATE_RESET + SD_LowLevel_Init(); + retry_Cnt = retryCnt; + for (;;) { + TERN_(USE_WATCHDOG, HAL_watchdog_refresh()); + status = (bool) HAL_SD_Init(&hsd); + if (!status) break; + if (!--retry_Cnt) return false; // return failing status if retries are exhausted + } + go_to_transfer_speed(); + } + #endif + + return true; +} + +static bool SDIO_ReadWriteBlock_DMA(uint32_t block, const uint8_t *src, uint8_t *dst) { + if (HAL_SD_GetCardState(&hsd) != HAL_SD_CARD_TRANSFER) return false; + + TERN_(USE_WATCHDOG, HAL_watchdog_refresh()); + + HAL_StatusTypeDef ret; + if (src) { + hdma_sdio.Init.Direction = DMA_MEMORY_TO_PERIPH; + HAL_DMA_Init(&hdma_sdio); + ret = HAL_SD_WriteBlocks_DMA(&hsd, (uint8_t *)src, block, 1); + } + else { + hdma_sdio.Init.Direction = DMA_PERIPH_TO_MEMORY; + HAL_DMA_Init(&hdma_sdio); + ret = HAL_SD_ReadBlocks_DMA(&hsd, (uint8_t *)dst, block, 1); + } + + if (ret != HAL_OK) { + HAL_DMA_Abort_IT(&hdma_sdio); + HAL_DMA_DeInit(&hdma_sdio); + return false; + } + + millis_t timeout = millis() + 500; + // Wait the transfer + while (hsd.State != HAL_SD_STATE_READY) { + if (ELAPSED(millis(), timeout)) { + HAL_DMA_Abort_IT(&hdma_sdio); + HAL_DMA_DeInit(&hdma_sdio); + return false; + } + } + + while (__HAL_DMA_GET_FLAG(&hdma_sdio, __HAL_DMA_GET_TC_FLAG_INDEX(&hdma_sdio)) != 0 + || __HAL_DMA_GET_FLAG(&hdma_sdio, __HAL_DMA_GET_TE_FLAG_INDEX(&hdma_sdio)) != 0) { /* nada */ } + + HAL_DMA_Abort_IT(&hdma_sdio); + HAL_DMA_DeInit(&hdma_sdio); + + timeout = millis() + 500; + while (HAL_SD_GetCardState(&hsd) != HAL_SD_CARD_TRANSFER) if (ELAPSED(millis(), timeout)) return false; + + return true; +} + +bool SDIO_ReadBlock(uint32_t block, uint8_t *dst) { + uint8_t retries = SDIO_READ_RETRIES; + while (retries--) if (SDIO_ReadWriteBlock_DMA(block, nullptr, dst)) return true; + return false; +} + +bool SDIO_WriteBlock(uint32_t block, const uint8_t *src) { + uint8_t retries = SDIO_READ_RETRIES; + while (retries--) if (SDIO_ReadWriteBlock_DMA(block, src, nullptr)) return true; + return false; +} + +bool SDIO_IsReady() { + return hsd.State == HAL_SD_STATE_READY; +} + +uint32_t SDIO_GetCardSize() { + return (uint32_t)(hsd.SdCard.BlockNbr) * (hsd.SdCard.BlockSize); +} + +#if defined(STM32F1xx) + #define DMA_IRQ_HANDLER DMA2_Channel4_5_IRQHandler +#elif defined(STM32F4xx) + #define DMA_IRQ_HANDLER DMA2_Stream3_IRQHandler +#else + #error "Unknown STM32 architecture." +#endif + +extern "C" void SDIO_IRQHandler(void) { HAL_SD_IRQHandler(&hsd); } +extern "C" void DMA_IRQ_HANDLER(void) { HAL_DMA_IRQHandler(&hdma_sdio); } + +#endif // SDIO_SUPPORT +#endif // HAL_STM32 diff --git a/Marlin/src/HAL/STM32/sdio.h b/Marlin/src/HAL/STM32/sdio.h new file mode 100644 index 000000000000..cf5539c3c76d --- /dev/null +++ b/Marlin/src/HAL/STM32/sdio.h @@ -0,0 +1,29 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2021 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + * + */ +#pragma once + +#define SDIO_D0_PIN PC8 +#define SDIO_D1_PIN PC9 +#define SDIO_D2_PIN PC10 +#define SDIO_D3_PIN PC11 +#define SDIO_CK_PIN PC12 +#define SDIO_CMD_PIN PD2 diff --git a/Marlin/src/HAL/STM32/spi_pins.h b/Marlin/src/HAL/STM32/spi_pins.h index e2052c5c7704..7f341a8c253f 100644 --- a/Marlin/src/HAL/STM32/spi_pins.h +++ b/Marlin/src/HAL/STM32/spi_pins.h @@ -2,6 +2,9 @@ * Marlin 3D Printer Firmware * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * * This program is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation, either version 3 of the License, or diff --git a/Marlin/src/HAL/STM32/tft/gt911.cpp b/Marlin/src/HAL/STM32/tft/gt911.cpp new file mode 100644 index 000000000000..92e14edb2057 --- /dev/null +++ b/Marlin/src/HAL/STM32/tft/gt911.cpp @@ -0,0 +1,204 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2021 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + * + */ +#include "../../platforms.h" + +#ifdef HAL_STM32 + +#include "../../../inc/MarlinConfig.h" + +#if ENABLED(TFT_TOUCH_DEVICE_GT911) + +#include "gt911.h" +#include "pinconfig.h" + +SW_IIC::SW_IIC(uint16_t sda, uint16_t scl) { + scl_pin = scl; + sda_pin = sda; +} + +// Software I2C hardware io init +void SW_IIC::init() { + OUT_WRITE(scl_pin, HIGH); + OUT_WRITE(sda_pin, HIGH); +} + +// Software I2C start signal +void SW_IIC::start() { + write_sda(HIGH); // SDA = 1 + write_scl(HIGH); // SCL = 1 + iic_delay(2); + write_sda(LOW); // SDA = 0 + iic_delay(1); + write_scl(LOW); // SCL = 0 // keep SCL low, avoid false stop caused by level jump caused by SDA switching IN/OUT +} + +// Software I2C stop signal +void SW_IIC::stop() { + write_scl(LOW); // SCL = 0 + iic_delay(2); + write_sda(LOW); // SDA = 0 + iic_delay(2); + write_scl(HIGH); // SCL = 1 + iic_delay(2); + write_sda(HIGH); // SDA = 1 +} + +// Software I2C sends ACK or NACK signal +void SW_IIC::send_ack(bool ack) { + write_sda(ack ? LOW : HIGH); // SDA = !ack + iic_delay(2); + write_scl(HIGH); // SCL = 1 + iic_delay(2); + write_scl(LOW); // SCL = 0 +} + +// Software I2C read ACK or NACK signal +bool SW_IIC::read_ack() { + bool error = 0; + set_sda_in(); + + iic_delay(2); + + write_scl(HIGH); // SCL = 1 + error = read_sda(); + + iic_delay(2); + + write_scl(LOW); // SCL = 0 + + set_sda_out(); + return error; +} + +void SW_IIC::send_byte(uint8_t txd) { + LOOP_L_N(i, 8) { + write_sda(txd & 0x80); // write data bit + txd <<= 1; + iic_delay(1); + write_scl(HIGH); // SCL = 1 + iic_delay(2); + write_scl(LOW); // SCL = 0 + iic_delay(1); + } + + read_ack(); // wait ack +} + +uint8_t SW_IIC::read_byte(bool ack) { + uint8_t data = 0; + + set_sda_in(); + LOOP_L_N(i, 8) { + write_scl(HIGH); // SCL = 1 + iic_delay(1); + data <<= 1; + if (read_sda()) data++; + write_scl(LOW); // SCL = 0 + iic_delay(2); + } + set_sda_out(); + + send_ack(ack); + + return data; +} + +GT911_REG_MAP GT911::reg; +SW_IIC GT911::sw_iic = SW_IIC(GT911_SW_I2C_SDA_PIN, GT911_SW_I2C_SCL_PIN); + +void GT911::write_reg(uint16_t reg, uint8_t reg_len, uint8_t* w_data, uint8_t w_len) { + sw_iic.start(); + sw_iic.send_byte(gt911_slave_address); // Set IIC Slave address + LOOP_L_N(i, reg_len) { // Set reg address + uint8_t r = (reg >> (8 * (reg_len - 1 - i))) & 0xFF; + sw_iic.send_byte(r); + } + + LOOP_L_N(i, w_len) { // Write data to reg + sw_iic.send_byte(w_data[i]); + } + sw_iic.stop(); +} + +void GT911::read_reg(uint16_t reg, uint8_t reg_len, uint8_t* r_data, uint8_t r_len) { + sw_iic.start(); + sw_iic.send_byte(gt911_slave_address); // Set IIC Slave address + LOOP_L_N(i, reg_len) { // Set reg address + uint8_t r = (reg >> (8 * (reg_len - 1 - i))) & 0xFF; + sw_iic.send_byte(r); + } + + sw_iic.start(); + sw_iic.send_byte(gt911_slave_address + 1); // Set read mode + + LOOP_L_N(i, r_len) { + r_data[i] = sw_iic.read_byte(1); // Read data from reg + } + sw_iic.stop(); +} + +void GT911::Init() { + OUT_WRITE(GT911_RST_PIN, LOW); + OUT_WRITE(GT911_INT_PIN, LOW); + delay(20); + WRITE(GT911_RST_PIN, HIGH); + SET_INPUT(GT911_INT_PIN); + + sw_iic.init(); + + uint8_t clear_reg = 0x0000; + write_reg(0x814E, 2, &clear_reg, 2); // Reset to 0 for start +} + +bool GT911::getFirstTouchPoint(int16_t *x, int16_t *y) { + read_reg(0x814E, 2, ®.REG.status, 1); + + if (reg.REG.status & 0x80) { + uint8_t clear_reg = 0x00; + write_reg(0x814E, 2, &clear_reg, 1); // Reset to 0 for start + read_reg(0x8150, 2, reg.map + 2, 8 * (reg.REG.status & 0x0F)); + + // First touch point + *x = ((reg.REG.point[0].xh & 0x0F) << 8) | reg.REG.point[0].xl; + *y = ((reg.REG.point[0].yh & 0x0F) << 8) | reg.REG.point[0].yl; + return true; + } + return false; +} + +bool GT911::getPoint(int16_t *x, int16_t *y) { + static bool touched = 0; + static int16_t read_x = 0, read_y = 0; + static millis_t next_time = 0; + + if (ELAPSED(millis(), next_time)) { + touched = getFirstTouchPoint(&read_x, &read_y); + next_time = millis() + 20; + } + + *x = read_x; + *y = read_y; + return touched; +} + +#endif // TFT_TOUCH_DEVICE_GT911 +#endif // HAL_STM32 diff --git a/Marlin/src/HAL/STM32/tft/gt911.h b/Marlin/src/HAL/STM32/tft/gt911.h new file mode 100644 index 000000000000..752a554d98ed --- /dev/null +++ b/Marlin/src/HAL/STM32/tft/gt911.h @@ -0,0 +1,120 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2021 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + * + */ +#pragma once + +#include "../../../inc/MarlinConfig.h" + +#define GT911_SLAVE_ADDRESS 0xBA + +#if !PIN_EXISTS(GT911_RST) + #error "GT911_RST_PIN is not defined." +#elif !PIN_EXISTS(GT911_INT) + #error "GT911_INT_PIN is not defined." +#elif !PIN_EXISTS(GT911_SW_I2C_SCL) + #error "GT911_SW_I2C_SCL_PIN is not defined." +#elif !PIN_EXISTS(GT911_SW_I2C_SDA) + #error "GT911_SW_I2C_SDA_PIN is not defined." +#endif + +class SW_IIC { + private: + uint16_t scl_pin; + uint16_t sda_pin; + void write_scl(bool level) + { + WRITE(scl_pin, level); + } + void write_sda(bool level) + { + WRITE(sda_pin, level); + } + bool read_sda() + { + return READ(sda_pin); + } + void set_sda_out() + { + SET_OUTPUT(sda_pin); + } + void set_sda_in() + { + SET_INPUT_PULLUP(sda_pin); + } + static void iic_delay(uint8_t t) + { + delayMicroseconds(t); + } + + public: + SW_IIC(uint16_t sda, uint16_t scl); + // setSCL/SDA have to be called before begin() + void setSCL(uint16_t scl) + { + scl_pin = scl; + }; + void setSDA(uint16_t sda) + { + sda_pin = sda; + }; + void init(); // Initialize the IO port of IIC + void start(); // Send IIC start signal + void stop(); // Send IIC stop signal + void send_byte(uint8_t txd); // IIC sends a byte + uint8_t read_byte(bool ack); // IIC reads a byte + void send_ack(bool ack); // IIC sends ACK or NACK signal + bool read_ack(); +}; + +typedef struct __attribute__((__packed__)) { + uint8_t xl; + uint8_t xh; + uint8_t yl; + uint8_t yh; + uint8_t sizel; + uint8_t sizeh; + uint8_t reserved; + uint8_t track_id; +} GT911_POINT; + +typedef union __attribute__((__packed__)) { + uint8_t map[42]; + struct { + uint8_t status; // 0x814E + uint8_t track_id; // 0x814F + + GT911_POINT point[5]; // [0]:0x8150 - 0x8157 / [1]:0x8158 - 0x815F / [2]:0x8160 - 0x8167 / [3]:0x8168 - 0x816F / [4]:0x8170 - 0x8177 + } REG; +} GT911_REG_MAP; + +class GT911 { + private: + static const uint8_t gt911_slave_address = GT911_SLAVE_ADDRESS; + static GT911_REG_MAP reg; + static SW_IIC sw_iic; + static void write_reg(uint16_t reg, uint8_t reg_len, uint8_t* w_data, uint8_t w_len); + static void read_reg(uint16_t reg, uint8_t reg_len, uint8_t* r_data, uint8_t r_len); + + public: + static void Init(); + static bool getFirstTouchPoint(int16_t *x, int16_t *y); + static bool getPoint(int16_t *x, int16_t *y); +}; diff --git a/Marlin/src/HAL/STM32/tft/tft_fsmc.cpp b/Marlin/src/HAL/STM32/tft/tft_fsmc.cpp index 87ca2dbbe18a..e68b3c126939 100644 --- a/Marlin/src/HAL/STM32/tft/tft_fsmc.cpp +++ b/Marlin/src/HAL/STM32/tft/tft_fsmc.cpp @@ -19,7 +19,10 @@ * along with this program. If not, see . * */ -#if defined(ARDUINO_ARCH_STM32) && !defined(STM32GENERIC) + +#include "../../platforms.h" + +#ifdef HAL_STM32 #include "../../../inc/MarlinConfig.h" @@ -34,16 +37,6 @@ LCD_CONTROLLER_TypeDef *TFT_FSMC::LCD; void TFT_FSMC::Init() { uint32_t controllerAddress; - - #if PIN_EXISTS(TFT_RESET) - OUT_WRITE(TFT_RESET_PIN, HIGH); - HAL_Delay(100); - #endif - - #if PIN_EXISTS(TFT_BACKLIGHT) - OUT_WRITE(TFT_BACKLIGHT_PIN, HIGH); - #endif - FSMC_NORSRAM_TimingTypeDef Timing, ExtTiming; uint32_t NSBank = (uint32_t)pinmap_peripheral(digitalPinToPinName(TFT_CS_PIN), PinMap_FSMC_CS); @@ -178,4 +171,4 @@ void TFT_FSMC::TransmitDMA(uint32_t MemoryIncrease, uint16_t *Data, uint16_t Cou } #endif // HAS_FSMC_TFT -#endif // ARDUINO_ARCH_STM32 && !STM32GENERIC +#endif // HAL_STM32 diff --git a/Marlin/src/HAL/STM32/tft/tft_ltdc.cpp b/Marlin/src/HAL/STM32/tft/tft_ltdc.cpp new file mode 100644 index 000000000000..66cfd65995dd --- /dev/null +++ b/Marlin/src/HAL/STM32/tft/tft_ltdc.cpp @@ -0,0 +1,389 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2021 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + * + */ +#include "../../platforms.h" + +#ifdef HAL_STM32 + +#include "../../../inc/MarlinConfig.h" + +#if HAS_LTDC_TFT + +#include "tft_ltdc.h" +#include "pinconfig.h" + +#define FRAME_BUFFER_ADDRESS 0XC0000000 // SDRAM address + +#define SDRAM_TIMEOUT ((uint32_t)0xFFFF) +#define REFRESH_COUNT ((uint32_t)0x02A5) // SDRAM refresh counter + +#define SDRAM_MODEREG_BURST_LENGTH_1 ((uint16_t)0x0000) +#define SDRAM_MODEREG_BURST_LENGTH_2 ((uint16_t)0x0001) +#define SDRAM_MODEREG_BURST_LENGTH_4 ((uint16_t)0x0002) +#define SDRAM_MODEREG_BURST_LENGTH_8 ((uint16_t)0x0004) +#define SDRAM_MODEREG_BURST_TYPE_SEQUENTIAL ((uint16_t)0x0000) +#define SDRAM_MODEREG_BURST_TYPE_INTERLEAVED ((uint16_t)0x0008) +#define SDRAM_MODEREG_CAS_LATENCY_2 ((uint16_t)0x0020) +#define SDRAM_MODEREG_CAS_LATENCY_3 ((uint16_t)0x0030) +#define SDRAM_MODEREG_OPERATING_MODE_STANDARD ((uint16_t)0x0000) +#define SDRAM_MODEREG_WRITEBURST_MODE_PROGRAMMED ((uint16_t)0x0000) +#define SDRAM_MODEREG_WRITEBURST_MODE_SINGLE ((uint16_t)0x0200) + +void SDRAM_Initialization_Sequence(SDRAM_HandleTypeDef *hsdram, FMC_SDRAM_CommandTypeDef *Command) { + + __IO uint32_t tmpmrd =0; + /* Step 1: Configure a clock configuration enable command */ + Command->CommandMode = FMC_SDRAM_CMD_CLK_ENABLE; + Command->CommandTarget = FMC_SDRAM_CMD_TARGET_BANK1; + Command->AutoRefreshNumber = 1; + Command->ModeRegisterDefinition = 0; + /* Send the command */ + HAL_SDRAM_SendCommand(hsdram, Command, SDRAM_TIMEOUT); + + /* Step 2: Insert 100 us minimum delay */ + /* Inserted delay is equal to 1 ms due to systick time base unit (ms) */ + HAL_Delay(1); + + /* Step 3: Configure a PALL (precharge all) command */ + Command->CommandMode = FMC_SDRAM_CMD_PALL; + Command->CommandTarget = FMC_SDRAM_CMD_TARGET_BANK1; + Command->AutoRefreshNumber = 1; + Command->ModeRegisterDefinition = 0; + /* Send the command */ + HAL_SDRAM_SendCommand(hsdram, Command, SDRAM_TIMEOUT); + + /* Step 4 : Configure a Auto-Refresh command */ + Command->CommandMode = FMC_SDRAM_CMD_AUTOREFRESH_MODE; + Command->CommandTarget = FMC_SDRAM_CMD_TARGET_BANK1; + Command->AutoRefreshNumber = 8; + Command->ModeRegisterDefinition = 0; + /* Send the command */ + HAL_SDRAM_SendCommand(hsdram, Command, SDRAM_TIMEOUT); + + /* Step 5: Program the external memory mode register */ + tmpmrd = (uint32_t)(SDRAM_MODEREG_BURST_LENGTH_1 | + SDRAM_MODEREG_BURST_TYPE_SEQUENTIAL | + SDRAM_MODEREG_CAS_LATENCY_2 | + SDRAM_MODEREG_OPERATING_MODE_STANDARD | + SDRAM_MODEREG_WRITEBURST_MODE_SINGLE); + + Command->CommandMode = FMC_SDRAM_CMD_LOAD_MODE; + Command->CommandTarget = FMC_SDRAM_CMD_TARGET_BANK1; + Command->AutoRefreshNumber = 1; + Command->ModeRegisterDefinition = tmpmrd; + /* Send the command */ + HAL_SDRAM_SendCommand(hsdram, Command, SDRAM_TIMEOUT); + + /* Step 6: Set the refresh rate counter */ + /* Set the device refresh rate */ + HAL_SDRAM_ProgramRefreshRate(hsdram, REFRESH_COUNT); +} + +void SDRAM_Config() { + + __HAL_RCC_SYSCFG_CLK_ENABLE(); + __HAL_RCC_FMC_CLK_ENABLE(); + + SDRAM_HandleTypeDef hsdram; + FMC_SDRAM_TimingTypeDef SDRAM_Timing; + FMC_SDRAM_CommandTypeDef command; + + /* Configure the SDRAM device */ + hsdram.Instance = FMC_SDRAM_DEVICE; + hsdram.Init.SDBank = FMC_SDRAM_BANK1; + hsdram.Init.ColumnBitsNumber = FMC_SDRAM_COLUMN_BITS_NUM_9; + hsdram.Init.RowBitsNumber = FMC_SDRAM_ROW_BITS_NUM_13; + hsdram.Init.MemoryDataWidth = FMC_SDRAM_MEM_BUS_WIDTH_16; + hsdram.Init.InternalBankNumber = FMC_SDRAM_INTERN_BANKS_NUM_4; + hsdram.Init.CASLatency = FMC_SDRAM_CAS_LATENCY_2; + hsdram.Init.WriteProtection = FMC_SDRAM_WRITE_PROTECTION_DISABLE; + hsdram.Init.SDClockPeriod = FMC_SDRAM_CLOCK_PERIOD_2; + hsdram.Init.ReadBurst = FMC_SDRAM_RBURST_ENABLE; + hsdram.Init.ReadPipeDelay = FMC_SDRAM_RPIPE_DELAY_0; + + /* Timing configuration for 100Mhz as SDRAM clock frequency (System clock is up to 200Mhz) */ + SDRAM_Timing.LoadToActiveDelay = 2; + SDRAM_Timing.ExitSelfRefreshDelay = 8; + SDRAM_Timing.SelfRefreshTime = 6; + SDRAM_Timing.RowCycleDelay = 6; + SDRAM_Timing.WriteRecoveryTime = 2; + SDRAM_Timing.RPDelay = 2; + SDRAM_Timing.RCDDelay = 2; + + /* Initialize the SDRAM controller */ + if (HAL_SDRAM_Init(&hsdram, &SDRAM_Timing) != HAL_OK) + { + /* Initialization Error */ + } + + /* Program the SDRAM external device */ + SDRAM_Initialization_Sequence(&hsdram, &command); +} + +void LTDC_Config() { + + __HAL_RCC_LTDC_CLK_ENABLE(); + __HAL_RCC_DMA2D_CLK_ENABLE(); + + RCC_PeriphCLKInitTypeDef PeriphClkInitStruct; + + /* The PLL3R is configured to provide the LTDC PCLK clock */ + /* PLL3_VCO Input = HSE_VALUE / PLL3M = 25Mhz / 5 = 5 Mhz */ + /* PLL3_VCO Output = PLL3_VCO Input * PLL3N = 5Mhz * 160 = 800 Mhz */ + /* PLLLCDCLK = PLL3_VCO Output/PLL3R = 800Mhz / 16 = 50Mhz */ + /* LTDC clock frequency = PLLLCDCLK = 50 Mhz */ + PeriphClkInitStruct.PeriphClockSelection = RCC_PERIPHCLK_LTDC; + PeriphClkInitStruct.PLL3.PLL3M = 5; + PeriphClkInitStruct.PLL3.PLL3N = 160; + PeriphClkInitStruct.PLL3.PLL3FRACN = 0; + PeriphClkInitStruct.PLL3.PLL3P = 2; + PeriphClkInitStruct.PLL3.PLL3Q = 2; + PeriphClkInitStruct.PLL3.PLL3R = (800 / LTDC_LCD_CLK); + PeriphClkInitStruct.PLL3.PLL3VCOSEL = RCC_PLL3VCOWIDE; + PeriphClkInitStruct.PLL3.PLL3RGE = RCC_PLL3VCIRANGE_2; + HAL_RCCEx_PeriphCLKConfig(&PeriphClkInitStruct); + + LTDC_HandleTypeDef hltdc_F; + LTDC_LayerCfgTypeDef pLayerCfg; + + /* LTDC Initialization -------------------------------------------------------*/ + + /* Polarity configuration */ + /* Initialize the horizontal synchronization polarity as active low */ + hltdc_F.Init.HSPolarity = LTDC_HSPOLARITY_AL; + /* Initialize the vertical synchronization polarity as active low */ + hltdc_F.Init.VSPolarity = LTDC_VSPOLARITY_AL; + /* Initialize the data enable polarity as active low */ + hltdc_F.Init.DEPolarity = LTDC_DEPOLARITY_AL; + /* Initialize the pixel clock polarity as input pixel clock */ + hltdc_F.Init.PCPolarity = LTDC_PCPOLARITY_IPC; + + /* Timing configuration */ + hltdc_F.Init.HorizontalSync = (LTDC_LCD_HSYNC - 1); + hltdc_F.Init.VerticalSync = (LTDC_LCD_VSYNC - 1); + hltdc_F.Init.AccumulatedHBP = (LTDC_LCD_HSYNC + LTDC_LCD_HBP - 1); + hltdc_F.Init.AccumulatedVBP = (LTDC_LCD_VSYNC + LTDC_LCD_VBP - 1); + hltdc_F.Init.AccumulatedActiveH = (TFT_HEIGHT + LTDC_LCD_VSYNC + LTDC_LCD_VBP - 1); + hltdc_F.Init.AccumulatedActiveW = (TFT_WIDTH + LTDC_LCD_HSYNC + LTDC_LCD_HBP - 1); + hltdc_F.Init.TotalHeigh = (TFT_HEIGHT + LTDC_LCD_VSYNC + LTDC_LCD_VBP + LTDC_LCD_VFP - 1); + hltdc_F.Init.TotalWidth = (TFT_WIDTH + LTDC_LCD_HSYNC + LTDC_LCD_HBP + LTDC_LCD_HFP - 1); + + /* Configure R,G,B component values for LCD background color : all black background */ + hltdc_F.Init.Backcolor.Blue = 0; + hltdc_F.Init.Backcolor.Green = 0; + hltdc_F.Init.Backcolor.Red = 0; + + hltdc_F.Instance = LTDC; + + /* Layer0 Configuration ------------------------------------------------------*/ + + /* Windowing configuration */ + pLayerCfg.WindowX0 = 0; + pLayerCfg.WindowX1 = TFT_WIDTH; + pLayerCfg.WindowY0 = 0; + pLayerCfg.WindowY1 = TFT_HEIGHT; + + /* Pixel Format configuration*/ + pLayerCfg.PixelFormat = LTDC_PIXEL_FORMAT_RGB565; + + /* Start Address configuration : frame buffer is located at SDRAM memory */ + pLayerCfg.FBStartAdress = (uint32_t)(FRAME_BUFFER_ADDRESS); + + /* Alpha constant (255 == totally opaque) */ + pLayerCfg.Alpha = 255; + + /* Default Color configuration (configure A,R,G,B component values) : no background color */ + pLayerCfg.Alpha0 = 0; /* fully transparent */ + pLayerCfg.Backcolor.Blue = 0; + pLayerCfg.Backcolor.Green = 0; + pLayerCfg.Backcolor.Red = 0; + + /* Configure blending factors */ + pLayerCfg.BlendingFactor1 = LTDC_BLENDING_FACTOR1_CA; + pLayerCfg.BlendingFactor2 = LTDC_BLENDING_FACTOR2_CA; + + /* Configure the number of lines and number of pixels per line */ + pLayerCfg.ImageWidth = TFT_WIDTH; + pLayerCfg.ImageHeight = TFT_HEIGHT; + + /* Configure the LTDC */ + if (HAL_LTDC_Init(&hltdc_F) != HAL_OK) + { + /* Initialization Error */ + } + + /* Configure the Layer*/ + if (HAL_LTDC_ConfigLayer(&hltdc_F, &pLayerCfg, 0) != HAL_OK) + { + /* Initialization Error */ + } +} + +uint16_t TFT_LTDC::x_min = 0; +uint16_t TFT_LTDC::x_max = 0; +uint16_t TFT_LTDC::y_min = 0; +uint16_t TFT_LTDC::y_max = 0; +uint16_t TFT_LTDC::x_cur = 0; +uint16_t TFT_LTDC::y_cur = 0; +uint8_t TFT_LTDC::reg = 0; +volatile uint16_t* TFT_LTDC::framebuffer = (volatile uint16_t* )FRAME_BUFFER_ADDRESS; + +void TFT_LTDC::Init() { + + // SDRAM pins init + for (uint16_t i = 0; PinMap_SDRAM[i].pin != NC; i++) + pinmap_pinout(PinMap_SDRAM[i].pin, PinMap_SDRAM); + + // SDRAM peripheral config + SDRAM_Config(); + + // LTDC pins init + for (uint16_t i = 0; PinMap_LTDC[i].pin != NC; i++) + pinmap_pinout(PinMap_LTDC[i].pin, PinMap_LTDC); + + // LTDC peripheral config + LTDC_Config(); +} + +uint32_t TFT_LTDC::GetID() { + return 0xABAB; +} + +uint32_t TFT_LTDC::ReadID(tft_data_t Reg) { + return 0xABAB; +} + +bool TFT_LTDC::isBusy() { + return false; +} + +uint16_t TFT_LTDC::ReadPoint(uint16_t x, uint16_t y) { + return framebuffer[(TFT_WIDTH * y) + x]; +} + +void TFT_LTDC::DrawPoint(uint16_t x, uint16_t y, uint16_t color) { + framebuffer[(TFT_WIDTH * y) + x] = color; +} + +void TFT_LTDC::DrawRect(uint16_t sx, uint16_t sy, uint16_t ex, uint16_t ey, uint16_t color) { + + if (sx == ex || sy == ey) return; + + uint16_t offline = TFT_WIDTH - (ex - sx); + uint32_t addr = (uint32_t)&framebuffer[(TFT_WIDTH * sy) + sx]; + + CBI(DMA2D->CR, 0); + DMA2D->CR = 3 << 16; + DMA2D->OPFCCR = 0X02; + DMA2D->OOR = offline; + DMA2D->OMAR = addr; + DMA2D->NLR = (ey - sy) | ((ex - sx) << 16); + DMA2D->OCOLR = color; + SBI(DMA2D->CR, 0); + + uint32_t timeout = 0; + while (!TEST(DMA2D->ISR, 1)) { + timeout++; + if (timeout > 0x1FFFFF) break; + } + SBI(DMA2D->IFCR, 1); +} + +void TFT_LTDC::DrawImage(uint16_t sx, uint16_t sy, uint16_t ex, uint16_t ey, uint16_t *colors) { + + if (sx == ex || sy == ey) return; + + uint16_t offline = TFT_WIDTH - (ex - sx); + uint32_t addr = (uint32_t)&framebuffer[(TFT_WIDTH * sy) + sx]; + + CBI(DMA2D->CR, 0); + DMA2D->CR = 0 << 16; + DMA2D->FGPFCCR = 0X02; + DMA2D->FGOR = 0; + DMA2D->OOR = offline; + DMA2D->FGMAR = (uint32_t)colors; + DMA2D->OMAR = addr; + DMA2D->NLR = (ey - sy) | ((ex - sx) << 16); + SBI(DMA2D->CR, 0); + + uint32_t timeout = 0; + while (!TEST(DMA2D->ISR, 1)) { + timeout++; + if (timeout > 0x1FFFFF) break; + } + SBI(DMA2D->IFCR, 1); +} + +void TFT_LTDC::WriteData(uint16_t data) { + switch (reg) { + case 0x01: x_cur = x_min = data; return; + case 0x02: x_max = data; return; + case 0x03: y_cur = y_min = data; return; + case 0x04: y_max = data; return; + } + Transmit(data); +} + +void TFT_LTDC::Transmit(tft_data_t Data) { + DrawPoint(x_cur, y_cur, Data); + x_cur++; + if (x_cur > x_max) { + x_cur = x_min; + y_cur++; + if (y_cur > y_max) y_cur = y_min; + } +} + +void TFT_LTDC::WriteReg(uint16_t Reg) { + reg = Reg; +} + +void TFT_LTDC::TransmitDMA(uint32_t MemoryIncrease, uint16_t *Data, uint16_t Count) { + + while (x_cur != x_min && Count) { + Transmit(*Data); + if (MemoryIncrease == DMA_PINC_ENABLE) Data++; + Count--; + } + + uint16_t width = x_max - x_min + 1; + uint16_t height = Count / width; + uint16_t x_end_cnt = Count - (width * height); + + if (height) { + if (MemoryIncrease == DMA_PINC_ENABLE) { + DrawImage(x_min, y_cur, x_min + width, y_cur + height, Data); + Data += width * height; + } else { + DrawRect(x_min, y_cur, x_min + width, y_cur + height, *Data); + } + y_cur += height; + } + + while (x_end_cnt) { + Transmit(*Data); + if (MemoryIncrease == DMA_PINC_ENABLE) Data++; + x_end_cnt--; + } +} + +#endif // HAS_LTDC_TFT +#endif // HAL_STM32 diff --git a/Marlin/src/HAL/STM32/tft/tft_ltdc.h b/Marlin/src/HAL/STM32/tft/tft_ltdc.h new file mode 100644 index 000000000000..7b63d6929b31 --- /dev/null +++ b/Marlin/src/HAL/STM32/tft/tft_ltdc.h @@ -0,0 +1,155 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2021 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + * + */ +#pragma once + +#include "../../../inc/MarlinConfig.h" + +#ifdef STM32H7xx + #include "stm32h7xx_hal.h" +#else + #error "LTDC TFT is currently only supported on STM32H7 hardware." +#endif + +#define DATASIZE_8BIT SPI_DATASIZE_8BIT +#define DATASIZE_16BIT SPI_DATASIZE_16BIT +#define TFT_IO_DRIVER TFT_LTDC + +#define TFT_DATASIZE DATASIZE_16BIT +typedef uint16_t tft_data_t; + +class TFT_LTDC { + private: + static volatile uint16_t *framebuffer; + static uint16_t x_min, x_max, y_min, y_max, x_cur, y_cur; + static uint8_t reg; + + static uint32_t ReadID(tft_data_t Reg); + + static uint16_t ReadPoint(uint16_t x, uint16_t y); + static void DrawPoint(uint16_t x, uint16_t y, uint16_t color); + static void DrawRect(uint16_t sx, uint16_t sy, uint16_t ex, uint16_t ey, uint16_t color); + static void DrawImage(uint16_t sx, uint16_t sy, uint16_t ex, uint16_t ey, uint16_t *colors); + static void Transmit(tft_data_t Data); + static void TransmitDMA(uint32_t MemoryIncrease, uint16_t *Data, uint16_t Count); + + public: + static void Init(); + static uint32_t GetID(); + static bool isBusy(); + static void Abort() { /*__HAL_DMA_DISABLE(&DMAtx);*/ } + + static void DataTransferBegin(uint16_t DataWidth = TFT_DATASIZE) {} + static void DataTransferEnd() {}; + + static void WriteData(uint16_t Data); + static void WriteReg(uint16_t Reg); + + static void WriteSequence(uint16_t *Data, uint16_t Count) { TransmitDMA(DMA_PINC_ENABLE, Data, Count); } + static void WriteMultiple(uint16_t Color, uint16_t Count) { static uint16_t Data; Data = Color; TransmitDMA(DMA_PINC_DISABLE, &Data, Count); } + static void WriteMultiple(uint16_t Color, uint32_t Count) { + static uint16_t Data; Data = Color; + while (Count > 0) { + TransmitDMA(DMA_MINC_DISABLE, &Data, Count > 0xFFFF ? 0xFFFF : Count); + Count = Count > 0xFFFF ? Count - 0xFFFF : 0; + } + } +}; + +const PinMap PinMap_LTDC[] = { + {PF_10, LTDC, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF14_LTDC)}, // LCD_DE + {PG_7, LTDC, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF14_LTDC)}, // LCD_CLK + {PI_9, LTDC, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF14_LTDC)}, // LCD_VSYNC + {PI_10, LTDC, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF14_LTDC)}, // LCD_HSYNC + + {PG_6, LTDC, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF14_LTDC)}, // LCD_R7 + {PH_12, LTDC, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF14_LTDC)}, // LCD_R6 + {PH_11, LTDC, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF14_LTDC)}, // LCD_R5 + {PH_10, LTDC, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF14_LTDC)}, // LCD_R4 + {PH_9, LTDC, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF14_LTDC)}, // LCD_R3 + + {PI_2, LTDC, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF14_LTDC)}, // LCD_G7 + {PI_1, LTDC, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF14_LTDC)}, // LCD_G6 + {PI_0, LTDC, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF14_LTDC)}, // LCD_G5 + {PH_15, LTDC, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF14_LTDC)}, // LCD_G4 + {PH_14, LTDC, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF14_LTDC)}, // LCD_G3 + {PH_13, LTDC, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF14_LTDC)}, // LCD_G2 + + {PI_7, LTDC, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF14_LTDC)}, // LCD_B7 + {PI_6, LTDC, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF14_LTDC)}, // LCD_B6 + {PI_5, LTDC, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF14_LTDC)}, // LCD_B5 + {PI_4, LTDC, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF14_LTDC)}, // LCD_B4 + {PG_11, LTDC, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF14_LTDC)}, // LCD_B3 + {NC, NP, 0} +}; + +const PinMap PinMap_SDRAM[] = { + {PC_0, FMC_Bank1_R, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF12_FMC)}, // FMC_SDNWE + {PC_2, FMC_Bank1_R, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF12_FMC)}, // FMC_SDNE0 + {PC_3, FMC_Bank1_R, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF12_FMC)}, // FMC_SDCKE0 + {PE_0, FMC_Bank1_R, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF12_FMC)}, // FMC_NBL0 + {PE_1, FMC_Bank1_R, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF12_FMC)}, // FMC_NBL1 + {PF_11, FMC_Bank1_R, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF12_FMC)}, // FMC_SDNRAS + {PG_8, FMC_Bank1_R, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF12_FMC)}, // FMC_SDCLK + {PG_15, FMC_Bank1_R, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF12_FMC)}, // FMC_SDNCAS + {PG_4, FMC_Bank1_R, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF12_FMC)}, // FMC_BA0 + {PG_5, FMC_Bank1_R, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF12_FMC)}, // FMC_BA1 + {PD_14, FMC_Bank1_R, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF12_FMC)}, // FMC_D0 + {PD_15, FMC_Bank1_R, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF12_FMC)}, // FMC_D1 + {PD_0, FMC_Bank1_R, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF12_FMC)}, // FMC_D2 + {PD_1, FMC_Bank1_R, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF12_FMC)}, // FMC_D3 + {PE_7, FMC_Bank1_R, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF12_FMC)}, // FMC_D4 + {PE_8, FMC_Bank1_R, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF12_FMC)}, // FMC_D5 + {PE_9, FMC_Bank1_R, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF12_FMC)}, // FMC_D6 + {PE_10, FMC_Bank1_R, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF12_FMC)}, // FMC_D7 + {PE_11, FMC_Bank1_R, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF12_FMC)}, // FMC_D8 + {PE_12, FMC_Bank1_R, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF12_FMC)}, // FMC_D9 + {PE_13, FMC_Bank1_R, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF12_FMC)}, // FMC_D10 + {PE_14, FMC_Bank1_R, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF12_FMC)}, // FMC_D11 + {PE_15, FMC_Bank1_R, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF12_FMC)}, // FMC_D12 + {PD_8, FMC_Bank1_R, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF12_FMC)}, // FMC_D13 + {PD_9, FMC_Bank1_R, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF12_FMC)}, // FMC_D14 + {PD_10, FMC_Bank1_R, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF12_FMC)}, // FMC_D15 + {PF_0, FMC_Bank1_R, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF12_FMC)}, // FMC_A0 + {PF_1, FMC_Bank1_R, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF12_FMC)}, // FMC_A1 + {PF_2, FMC_Bank1_R, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF12_FMC)}, // FMC_A2 + {PF_3, FMC_Bank1_R, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF12_FMC)}, // FMC_A3 + {PF_4, FMC_Bank1_R, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF12_FMC)}, // FMC_A4 + {PF_5, FMC_Bank1_R, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF12_FMC)}, // FMC_A5 + {PF_12, FMC_Bank1_R, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF12_FMC)}, // FMC_A6 + {PF_13, FMC_Bank1_R, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF12_FMC)}, // FMC_A7 + {PF_14, FMC_Bank1_R, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF12_FMC)}, // FMC_A8 + {PF_15, FMC_Bank1_R, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF12_FMC)}, // FMC_A9 + {PG_0, FMC_Bank1_R, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF12_FMC)}, // FMC_A10 + {PG_1, FMC_Bank1_R, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF12_FMC)}, // FMC_A11 + {PG_2, FMC_Bank1_R, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF12_FMC)}, // FMC_A12 + {NC, NP, 0} +}; + +const PinMap PinMap_QUADSPI[] = { + {PB_2, QUADSPI, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF9_QUADSPI)}, // QUADSPI_CLK + {PB_10, QUADSPI, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF9_QUADSPI)}, // QUADSPI_BK1_NCS + {PF_6, QUADSPI, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF9_QUADSPI)}, // QUADSPI_BK1_IO3 + {PF_7, QUADSPI, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF9_QUADSPI)}, // QUADSPI_BK1_IO2 + {PF_8, QUADSPI, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF10_QUADSPI)}, // QUADSPI_BK1_IO0 + {PF_9, QUADSPI, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF10_QUADSPI)}, // QUADSPI_BK1_IO1 + {NC, NP, 0} +}; diff --git a/Marlin/src/HAL/STM32/tft/tft_spi.cpp b/Marlin/src/HAL/STM32/tft/tft_spi.cpp index 32af67d158c4..2e18c8a64c06 100644 --- a/Marlin/src/HAL/STM32/tft/tft_spi.cpp +++ b/Marlin/src/HAL/STM32/tft/tft_spi.cpp @@ -19,7 +19,10 @@ * along with this program. If not, see . * */ -#if defined(ARDUINO_ARCH_STM32) && !defined(STM32GENERIC) + +#include "../../platforms.h" + +#ifdef HAL_STM32 #include "../../../inc/MarlinConfig.h" @@ -125,12 +128,20 @@ void TFT_SPI::DataTransferBegin(uint16_t DataSize) { WRITE(TFT_CS_PIN, LOW); } +#ifdef TFT_DEFAULT_DRIVER + #include "../../../lcd/tft_io/tft_ids.h" +#endif + uint32_t TFT_SPI::GetID() { uint32_t id; id = ReadID(LCD_READ_ID); - - if ((id & 0xFFFF) == 0 || (id & 0xFFFF) == 0xFFFF) + if ((id & 0xFFFF) == 0 || (id & 0xFFFF) == 0xFFFF) { id = ReadID(LCD_READ_ID4); + #ifdef TFT_DEFAULT_DRIVER + if ((id & 0xFFFF) == 0 || (id & 0xFFFF) == 0xFFFF) + id = TFT_DEFAULT_DRIVER; + #endif + } return id; } @@ -150,11 +161,11 @@ uint32_t TFT_SPI::ReadID(uint16_t Reg) { for (i = 0; i < 4; i++) { #if TFT_MISO_PIN != TFT_MOSI_PIN //if (hspi->Init.Direction == SPI_DIRECTION_2LINES) { - while ((SPIx.Instance->SR & SPI_FLAG_TXE) != SPI_FLAG_TXE) {} + while (!__HAL_SPI_GET_FLAG(&SPIx, SPI_FLAG_TXE)) {} SPIx.Instance->DR = 0; //} #endif - while ((SPIx.Instance->SR & SPI_FLAG_RXNE) != SPI_FLAG_RXNE) {} + while (!__HAL_SPI_GET_FLAG(&SPIx, SPI_FLAG_RXNE)) {} Data = (Data << 8) | SPIx.Instance->DR; } @@ -184,8 +195,8 @@ bool TFT_SPI::isBusy() { void TFT_SPI::Abort() { // Wait for any running spi - while ((SPIx.Instance->SR & SPI_FLAG_TXE) != SPI_FLAG_TXE) {} - while ((SPIx.Instance->SR & SPI_FLAG_BSY) == SPI_FLAG_BSY) {} + while (!__HAL_SPI_GET_FLAG(&SPIx, SPI_FLAG_TXE)) {} + while ( __HAL_SPI_GET_FLAG(&SPIx, SPI_FLAG_BSY)) {} // First, abort any running dma HAL_DMA_Abort(&DMAtx); // DeInit objects @@ -203,8 +214,8 @@ void TFT_SPI::Transmit(uint16_t Data) { SPIx.Instance->DR = Data; - while ((SPIx.Instance->SR & SPI_FLAG_TXE) != SPI_FLAG_TXE) {} - while ((SPIx.Instance->SR & SPI_FLAG_BSY) == SPI_FLAG_BSY) {} + while (!__HAL_SPI_GET_FLAG(&SPIx, SPI_FLAG_TXE)) {} + while ( __HAL_SPI_GET_FLAG(&SPIx, SPI_FLAG_BSY)) {} if (TFT_MISO_PIN != TFT_MOSI_PIN) __HAL_SPI_CLEAR_OVRFLAG(&SPIx); // Clear overrun flag in 2 Lines communication mode because received is not read @@ -231,5 +242,29 @@ void TFT_SPI::TransmitDMA(uint32_t MemoryIncrease, uint16_t *Data, uint16_t Coun Abort(); } +#if ENABLED(USE_SPI_DMA_TC) + + void TFT_SPI::TransmitDMA_IT(uint32_t MemoryIncrease, uint16_t *Data, uint16_t Count) { + + DMAtx.Init.MemInc = MemoryIncrease; + HAL_DMA_Init(&DMAtx); + + if (TFT_MISO_PIN == TFT_MOSI_PIN) + SPI_1LINE_TX(&SPIx); + + DataTransferBegin(); + + HAL_NVIC_SetPriority(DMA2_Stream3_IRQn, 5, 0); + HAL_NVIC_EnableIRQ(DMA2_Stream3_IRQn); + HAL_DMA_Start_IT(&DMAtx, (uint32_t)Data, (uint32_t)&(SPIx.Instance->DR), Count); + __HAL_SPI_ENABLE(&SPIx); + + SET_BIT(SPIx.Instance->CR2, SPI_CR2_TXDMAEN); // Enable Tx DMA Request + } + + extern "C" void DMA2_Stream3_IRQHandler(void) { HAL_DMA_IRQHandler(&TFT_SPI::DMAtx); } + +#endif + #endif // HAS_SPI_TFT -#endif // ARDUINO_ARCH_STM32 && !STM32GENERIC +#endif // HAL_STM32 diff --git a/Marlin/src/HAL/STM32/tft/tft_spi.h b/Marlin/src/HAL/STM32/tft/tft_spi.h index 667b5f366b7e..de051e229459 100644 --- a/Marlin/src/HAL/STM32/tft/tft_spi.h +++ b/Marlin/src/HAL/STM32/tft/tft_spi.h @@ -36,20 +36,25 @@ #define LCD_READ_ID4 0xD3 // Read display identification information (0xD3 on ILI9341) #endif -#define DATASIZE_8BIT SPI_DATASIZE_8BIT -#define DATASIZE_16BIT SPI_DATASIZE_16BIT -#define TFT_IO_DRIVER TFT_SPI +#define DATASIZE_8BIT SPI_DATASIZE_8BIT +#define DATASIZE_16BIT SPI_DATASIZE_16BIT +#define TFT_IO_DRIVER TFT_SPI class TFT_SPI { private: static SPI_HandleTypeDef SPIx; - static DMA_HandleTypeDef DMAtx; + static uint32_t ReadID(uint16_t Reg); static void Transmit(uint16_t Data); static void TransmitDMA(uint32_t MemoryIncrease, uint16_t *Data, uint16_t Count); + #if ENABLED(USE_SPI_DMA_TC) + static void TransmitDMA_IT(uint32_t MemoryIncrease, uint16_t *Data, uint16_t Count); + #endif public: + static DMA_HandleTypeDef DMAtx; + static void Init(); static uint32_t GetID(); static bool isBusy(); @@ -63,6 +68,11 @@ class TFT_SPI { static void WriteReg(uint16_t Reg) { WRITE(TFT_A0_PIN, LOW); Transmit(Reg); WRITE(TFT_A0_PIN, HIGH); } static void WriteSequence(uint16_t *Data, uint16_t Count) { TransmitDMA(DMA_MINC_ENABLE, Data, Count); } + + #if ENABLED(USE_SPI_DMA_TC) + static void WriteSequenceIT(uint16_t *Data, uint16_t Count) { TransmitDMA_IT(DMA_MINC_ENABLE, Data, Count); } + #endif + static void WriteMultiple(uint16_t Color, uint16_t Count) { static uint16_t Data; Data = Color; TransmitDMA(DMA_MINC_DISABLE, &Data, Count); } static void WriteMultiple(uint16_t Color, uint32_t Count) { static uint16_t Data; Data = Color; diff --git a/Marlin/src/HAL/STM32/tft/xpt2046.cpp b/Marlin/src/HAL/STM32/tft/xpt2046.cpp index 04294e669c55..cf4a8f18e9ac 100644 --- a/Marlin/src/HAL/STM32/tft/xpt2046.cpp +++ b/Marlin/src/HAL/STM32/tft/xpt2046.cpp @@ -19,11 +19,14 @@ * along with this program. If not, see . * */ -#if defined(ARDUINO_ARCH_STM32) && !defined(STM32GENERIC) + +#include "../../platforms.h" + +#ifdef HAL_STM32 #include "../../../inc/MarlinConfig.h" -#if HAS_TFT_XPT2046 || HAS_TOUCH_BUTTONS +#if HAS_TFT_XPT2046 || HAS_RES_TOUCH_BUTTONS #include "xpt2046.h" #include "pinconfig.h" @@ -167,4 +170,4 @@ uint16_t XPT2046::SoftwareIO(uint16_t data) { } #endif // HAS_TFT_XPT2046 -#endif // ARDUINO_ARCH_STM32 && !STM32GENERIC +#endif // HAL_STM32 diff --git a/Marlin/src/HAL/STM32/tft/xpt2046.h b/Marlin/src/HAL/STM32/tft/xpt2046.h index 5b8acf4b87af..71de6b00251b 100644 --- a/Marlin/src/HAL/STM32/tft/xpt2046.h +++ b/Marlin/src/HAL/STM32/tft/xpt2046.h @@ -56,7 +56,7 @@ enum XPTCoordinate : uint8_t { XPT2046_Z2 = 0x40 | XPT2046_CONTROL | XPT2046_DFR_MODE, }; -#if !defined(XPT2046_Z1_THRESHOLD) +#ifndef XPT2046_Z1_THRESHOLD #define XPT2046_Z1_THRESHOLD 10 #endif @@ -69,8 +69,8 @@ class XPT2046 { static uint16_t getRawData(const XPTCoordinate coordinate); static bool isTouched(); - static inline void DataTransferBegin() { if (SPIx.Instance) { HAL_SPI_Init(&SPIx); } WRITE(TOUCH_CS_PIN, LOW); }; - static inline void DataTransferEnd() { WRITE(TOUCH_CS_PIN, HIGH); }; + static void DataTransferBegin() { if (SPIx.Instance) { HAL_SPI_Init(&SPIx); } WRITE(TOUCH_CS_PIN, LOW); }; + static void DataTransferEnd() { WRITE(TOUCH_CS_PIN, HIGH); }; static uint16_t HardwareIO(uint16_t data); static uint16_t SoftwareIO(uint16_t data); static uint16_t IO(uint16_t data = 0) { return SPIx.Instance ? HardwareIO(data) : SoftwareIO(data); } diff --git a/Marlin/src/HAL/STM32/timers.cpp b/Marlin/src/HAL/STM32/timers.cpp index e8e18a47d48b..a1e3372bbb2e 100644 --- a/Marlin/src/HAL/STM32/timers.cpp +++ b/Marlin/src/HAL/STM32/timers.cpp @@ -19,7 +19,9 @@ * along with this program. If not, see . * */ -#if defined(ARDUINO_ARCH_STM32) && !defined(STM32GENERIC) +#include "../platforms.h" + +#ifdef HAL_STM32 #include "../../inc/MarlinConfig.h" @@ -65,17 +67,17 @@ #endif #endif -#ifdef STM32F0xx +#if defined(STM32F0xx) || defined(STM32G0xx) #define MCU_STEP_TIMER 16 #define MCU_TEMP_TIMER 17 #elif defined(STM32F1xx) #define MCU_STEP_TIMER 4 #define MCU_TEMP_TIMER 2 #elif defined(STM32F401xC) || defined(STM32F401xE) - #define MCU_STEP_TIMER 9 + #define MCU_STEP_TIMER 9 // STM32F401 has no TIM6, TIM7, or TIM8 #define MCU_TEMP_TIMER 10 -#elif defined(STM32F4xx) || defined(STM32F7xx) - #define MCU_STEP_TIMER 6 // STM32F401 has no TIM6, TIM7, or TIM8 +#elif defined(STM32F4xx) || defined(STM32F7xx) || defined(STM32H7xx) + #define MCU_STEP_TIMER 6 #define MCU_TEMP_TIMER 14 // TIM7 is consumed by Software Serial if used. #endif @@ -95,9 +97,15 @@ #define STEP_TIMER_DEV _TIMER_DEV(STEP_TIMER) #define TEMP_TIMER_DEV _TIMER_DEV(TEMP_TIMER) -// ------------------------ +// -------------------------------------------------------------------------- +// Local defines +// -------------------------------------------------------------------------- + +#define NUM_HARDWARE_TIMERS 2 + +// -------------------------------------------------------------------------- // Private Variables -// ------------------------ +// -------------------------------------------------------------------------- HardwareTimer *timer_instance[NUM_HARDWARE_TIMERS] = { nullptr }; @@ -108,7 +116,7 @@ HardwareTimer *timer_instance[NUM_HARDWARE_TIMERS] = { nullptr }; uint32_t GetStepperTimerClkFreq() { // Timer input clocks vary between devices, and in some cases between timers on the same device. // Retrieve at runtime to ensure device compatibility. Cache result to avoid repeated overhead. - static uint32_t clkfreq = timer_instance[STEP_TIMER_NUM]->getTimerClkFreq(); + static uint32_t clkfreq = timer_instance[MF_TIMER_STEP]->getTimerClkFreq(); return clkfreq; } @@ -116,7 +124,7 @@ uint32_t GetStepperTimerClkFreq() { void HAL_timer_start(const uint8_t timer_num, const uint32_t frequency) { if (!HAL_timer_initialized(timer_num)) { switch (timer_num) { - case STEP_TIMER_NUM: // STEPPER TIMER - use a 32bit timer if possible + case MF_TIMER_STEP: // STEPPER TIMER - use a 32bit timer if possible timer_instance[timer_num] = new HardwareTimer(STEP_TIMER_DEV); /* Set the prescaler to the final desired value. * This will change the effective ISR callback frequency but when @@ -135,7 +143,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); break; - case TEMP_TIMER_NUM: // TEMP TIMER - any available 16bit timer + case MF_TIMER_TEMP: // TEMP TIMER - any available 16bit timer timer_instance[timer_num] = new HardwareTimer(TEMP_TIMER_DEV); // The prescale factor is computed automatically for HERTZ_FORMAT timer_instance[timer_num]->setOverflow(frequency, HERTZ_FORMAT); @@ -155,10 +163,10 @@ void HAL_timer_start(const uint8_t timer_num, const uint32_t frequency) { // These calls can be removed and replaced with // timer_instance[timer_num]->setInterruptPriority switch (timer_num) { - case STEP_TIMER_NUM: + case MF_TIMER_STEP: timer_instance[timer_num]->setInterruptPriority(STEP_TIMER_IRQ_PRIO, 0); break; - case TEMP_TIMER_NUM: + case MF_TIMER_TEMP: timer_instance[timer_num]->setInterruptPriority(TEMP_TIMER_IRQ_PRIO, 0); break; } @@ -168,10 +176,10 @@ void HAL_timer_start(const uint8_t timer_num, const uint32_t frequency) { void HAL_timer_enable_interrupt(const uint8_t timer_num) { if (HAL_timer_initialized(timer_num) && !timer_instance[timer_num]->hasInterrupt()) { switch (timer_num) { - case STEP_TIMER_NUM: + case MF_TIMER_STEP: timer_instance[timer_num]->attachInterrupt(Step_Handler); break; - case TEMP_TIMER_NUM: + case MF_TIMER_TEMP: timer_instance[timer_num]->attachInterrupt(Temp_Handler); break; } @@ -319,4 +327,4 @@ static constexpr bool verify_no_timer_conflicts() { // when hovering over it, making it easy to identify the conflicting timers. static_assert(verify_no_timer_conflicts(), "One or more timer conflict detected. Examine \"timers_in_use\" to help identify conflict."); -#endif // ARDUINO_ARCH_STM32 && !STM32GENERIC +#endif // HAL_STM32 diff --git a/Marlin/src/HAL/STM32/timers.h b/Marlin/src/HAL/STM32/timers.h index 464982430358..6828998198af 100644 --- a/Marlin/src/HAL/STM32/timers.h +++ b/Marlin/src/HAL/STM32/timers.h @@ -21,15 +21,12 @@ */ #pragma once -#include #include "../../inc/MarlinConfig.h" // ------------------------ // Defines // ------------------------ -#define FORCE_INLINE __attribute__((always_inline)) inline - // STM32 timers may be 16 or 32 bit. Limiting HAL_TIMER_TYPE_MAX to 16 bits // avoids issues with STM32F0 MCUs, which seem to pause timers if UINT32_MAX // is written to the register. STM32F4 timers do not manifest this issue, @@ -43,17 +40,13 @@ #define hal_timer_t uint32_t #define HAL_TIMER_TYPE_MAX UINT16_MAX -#define NUM_HARDWARE_TIMERS 2 +// Marlin timer_instance[] content (unrelated to timer selection) +#define MF_TIMER_STEP 0 // Timer Index for Stepper +#define MF_TIMER_TEMP 1 // Timer Index for Temperature +#define MF_TIMER_PULSE MF_TIMER_STEP -#ifndef STEP_TIMER_NUM - #define STEP_TIMER_NUM 0 // Timer Index for Stepper -#endif -#ifndef PULSE_TIMER_NUM - #define PULSE_TIMER_NUM STEP_TIMER_NUM -#endif -#ifndef TEMP_TIMER_NUM - #define TEMP_TIMER_NUM 1 // Timer Index for Temperature -#endif +#define TIMER_INDEX_(T) TIMER##T##_INDEX // TIMER#_INDEX enums (timer_index_t) depend on TIM#_BASE defines. +#define TIMER_INDEX(T) TIMER_INDEX_(T) // Convert Timer ID to HardwareTimer_Handle index. #define TEMP_TIMER_FREQUENCY 1000 // Temperature::isr() is expected to be called at around 1kHz @@ -67,12 +60,12 @@ extern uint32_t GetStepperTimerClkFreq(); #define PULSE_TIMER_PRESCALE STEPPER_TIMER_PRESCALE #define PULSE_TIMER_TICKS_PER_US STEPPER_TIMER_TICKS_PER_US -#define ENABLE_STEPPER_DRIVER_INTERRUPT() HAL_timer_enable_interrupt(STEP_TIMER_NUM) -#define DISABLE_STEPPER_DRIVER_INTERRUPT() HAL_timer_disable_interrupt(STEP_TIMER_NUM) -#define STEPPER_ISR_ENABLED() HAL_timer_interrupt_enabled(STEP_TIMER_NUM) +#define ENABLE_STEPPER_DRIVER_INTERRUPT() HAL_timer_enable_interrupt(MF_TIMER_STEP) +#define DISABLE_STEPPER_DRIVER_INTERRUPT() HAL_timer_disable_interrupt(MF_TIMER_STEP) +#define STEPPER_ISR_ENABLED() HAL_timer_interrupt_enabled(MF_TIMER_STEP) -#define ENABLE_TEMPERATURE_INTERRUPT() HAL_timer_enable_interrupt(TEMP_TIMER_NUM) -#define DISABLE_TEMPERATURE_INTERRUPT() HAL_timer_disable_interrupt(TEMP_TIMER_NUM) +#define ENABLE_TEMPERATURE_INTERRUPT() HAL_timer_enable_interrupt(MF_TIMER_TEMP) +#define DISABLE_TEMPERATURE_INTERRUPT() HAL_timer_disable_interrupt(MF_TIMER_TEMP) extern void Step_Handler(); extern void Temp_Handler(); @@ -123,5 +116,5 @@ FORCE_INLINE static void HAL_timer_set_compare(const uint8_t timer_num, const ha } } -#define HAL_timer_isr_prologue(TIMER_NUM) -#define HAL_timer_isr_epilogue(TIMER_NUM) +#define HAL_timer_isr_prologue(T) NOOP +#define HAL_timer_isr_epilogue(T) NOOP diff --git a/Marlin/src/HAL/STM32/usb_host.cpp b/Marlin/src/HAL/STM32/usb_host.cpp index ed743361e681..d77f0b28e964 100644 --- a/Marlin/src/HAL/STM32/usb_host.cpp +++ b/Marlin/src/HAL/STM32/usb_host.cpp @@ -20,7 +20,9 @@ * */ -#if defined(ARDUINO_ARCH_STM32) && !defined(STM32GENERIC) +#include "../platforms.h" + +#ifdef HAL_STM32 #include "../../inc/MarlinConfig.h" @@ -87,9 +89,9 @@ void USBHost::setUsbTaskState(uint8_t state) { capacity = info.capacity.block_nbr / 2000; block_size = info.capacity.block_size; block_count = info.capacity.block_nbr; - // SERIAL_ECHOLNPAIR("info.capacity.block_nbr : %ld\n", info.capacity.block_nbr); - // SERIAL_ECHOLNPAIR("info.capacity.block_size: %d\n", info.capacity.block_size); - // SERIAL_ECHOLNPAIR("capacity : %d MB\n", capacity); + //SERIAL_ECHOLNPGM("info.capacity.block_nbr : %ld\n", info.capacity.block_nbr); + //SERIAL_ECHOLNPGM("info.capacity.block_size: %d\n", info.capacity.block_size); + //SERIAL_ECHOLNPGM("capacity : %d MB\n", capacity); } }; @@ -110,8 +112,8 @@ uint8_t BulkStorage::Read(uint8_t lun, uint32_t addr, uint16_t bsize, uint8_t bl } uint8_t BulkStorage::Write(uint8_t lun, uint32_t addr, uint16_t bsize, uint8_t blocks, const uint8_t * buf) { - return USBH_MSC_Write(&hUsbHost, lun, addr, const_cast (buf), blocks) != USBH_OK; + return USBH_MSC_Write(&hUsbHost, lun, addr, const_cast(buf), blocks) != USBH_OK; } #endif // USE_OTG_USB_HOST && USBHOST -#endif // ARDUINO_ARCH_STM32 && !STM32GENERIC +#endif // HAL_STM32 diff --git a/Marlin/src/HAL/STM32/usb_serial.cpp b/Marlin/src/HAL/STM32/usb_serial.cpp index 705d649ff58c..b607275db5bb 100644 --- a/Marlin/src/HAL/STM32/usb_serial.cpp +++ b/Marlin/src/HAL/STM32/usb_serial.cpp @@ -2,6 +2,9 @@ * Marlin 3D Printer Firmware * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * * This program is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation, either version 3 of the License, or @@ -16,7 +19,10 @@ * along with this program. If not, see . * */ -#if defined(ARDUINO_ARCH_STM32) && !defined(STM32GENERIC) + +#include "../platforms.h" + +#ifdef HAL_STM32 #include "../../inc/MarlinConfigPre.h" @@ -51,4 +57,4 @@ void USB_Hook_init() { } #endif // EMERGENCY_PARSER && USBD_USE_CDC -#endif // ARDUINO_ARCH_STM32 && !STM32GENERIC +#endif // HAL_STM32 diff --git a/Marlin/src/HAL/STM32/usb_serial.h b/Marlin/src/HAL/STM32/usb_serial.h index ca61b9ed239f..3edb6fd6185c 100644 --- a/Marlin/src/HAL/STM32/usb_serial.h +++ b/Marlin/src/HAL/STM32/usb_serial.h @@ -2,6 +2,9 @@ * Marlin 3D Printer Firmware * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * * This program is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation, either version 3 of the License, or diff --git a/Marlin/src/HAL/STM32/watchdog.cpp b/Marlin/src/HAL/STM32/watchdog.cpp index aad0a79a0cb2..1eccdec4985d 100644 --- a/Marlin/src/HAL/STM32/watchdog.cpp +++ b/Marlin/src/HAL/STM32/watchdog.cpp @@ -19,7 +19,10 @@ * along with this program. If not, see . * */ -#if defined(ARDUINO_ARCH_STM32) && !defined(STM32GENERIC) + +#include "../platforms.h" + +#ifdef HAL_STM32 #include "../../inc/MarlinConfigPre.h" @@ -46,4 +49,4 @@ void HAL_watchdog_refresh() { } #endif // USE_WATCHDOG -#endif // ARDUINO_ARCH_STM32 && !STM32GENERIC +#endif // HAL_STM32 diff --git a/Marlin/src/HAL/STM32F1/HAL.cpp b/Marlin/src/HAL/STM32F1/HAL.cpp index 020c623b77a9..1d58b106adfd 100644 --- a/Marlin/src/HAL/STM32F1/HAL.cpp +++ b/Marlin/src/HAL/STM32F1/HAL.cpp @@ -79,12 +79,12 @@ #define SCB_AIRCR_PRIGROUP_Msk (7UL << SCB_AIRCR_PRIGROUP_Pos) /*!< SCB AIRCR: PRIGROUP Mask */ // ------------------------ -// Public Variables +// Serial ports // ------------------------ #if defined(SERIAL_USB) && !HAS_SD_HOST_DRIVE USBSerial SerialUSB; - DefaultSerial MSerial(true, SerialUSB); + DefaultSerial1 MSerial0(true, SerialUSB); #if ENABLED(EMERGENCY_PARSER) #include "../libmaple/usb/stm32f1/usb_reg_map.h" @@ -107,147 +107,42 @@ len = usb_cdcacm_peek(buf, total); for (uint32 i = 0; i < len; i++) - emergency_parser.update(MSerial.emergency_state, buf[i + total - len]); + emergency_parser.update(MSerial0.emergency_state, buf[i + total - len]); } #endif #endif -uint16_t HAL_adc_result; - // ------------------------ -// Private Variables +// ADC // ------------------------ -STM32ADC adc(ADC1); -const uint8_t adc_pins[] = { - #if HAS_TEMP_ADC_0 - TEMP_0_PIN, - #endif - #if HAS_TEMP_ADC_PROBE - TEMP_PROBE_PIN, - #endif - #if HAS_HEATED_BED - TEMP_BED_PIN, - #endif - #if HAS_TEMP_CHAMBER - TEMP_CHAMBER_PIN, - #endif - #if HAS_TEMP_ADC_1 - TEMP_1_PIN, - #endif - #if HAS_TEMP_ADC_2 - TEMP_2_PIN, - #endif - #if HAS_TEMP_ADC_3 - TEMP_3_PIN, - #endif - #if HAS_TEMP_ADC_4 - TEMP_4_PIN, - #endif - #if HAS_TEMP_ADC_5 - TEMP_5_PIN, - #endif - #if HAS_TEMP_ADC_6 - TEMP_6_PIN, - #endif - #if HAS_TEMP_ADC_7 - TEMP_7_PIN, - #endif - #if ENABLED(FILAMENT_WIDTH_SENSOR) - FILWIDTH_PIN, - #endif - #if HAS_ADC_BUTTONS - ADC_KEYPAD_PIN, - #endif - #if HAS_JOY_ADC_X - JOY_X_PIN, - #endif - #if HAS_JOY_ADC_Y - JOY_Y_PIN, - #endif - #if HAS_JOY_ADC_Z - JOY_Z_PIN, - #endif - #if ENABLED(POWER_MONITOR_CURRENT) - POWER_MONITOR_CURRENT_PIN, - #endif - #if ENABLED(POWER_MONITOR_VOLTAGE) - POWER_MONITOR_VOLTAGE_PIN, - #endif -}; +// Watch out for recursion here! Our pin_t is signed, so pass through to Arduino -> analogRead(uint8_t) -enum TempPinIndex : char { - #if HAS_TEMP_ADC_0 - TEMP_0, - #endif - #if HAS_TEMP_ADC_PROBE - TEMP_PROBE, - #endif - #if HAS_HEATED_BED - TEMP_BED, - #endif - #if HAS_TEMP_CHAMBER - TEMP_CHAMBER, - #endif - #if HAS_TEMP_ADC_1 - TEMP_1, - #endif - #if HAS_TEMP_ADC_2 - TEMP_2, - #endif - #if HAS_TEMP_ADC_3 - TEMP_3, - #endif - #if HAS_TEMP_ADC_4 - TEMP_4, - #endif - #if HAS_TEMP_ADC_5 - TEMP_5, - #endif - #if HAS_TEMP_ADC_6 - TEMP_6, - #endif - #if HAS_TEMP_ADC_7 - TEMP_7, - #endif - #if ENABLED(FILAMENT_WIDTH_SENSOR) - FILWIDTH, - #endif - #if HAS_ADC_BUTTONS - ADC_KEY, - #endif - #if HAS_JOY_ADC_X - JOY_X, - #endif - #if HAS_JOY_ADC_Y - JOY_Y, - #endif - #if HAS_JOY_ADC_Z - JOY_Z, - #endif - #if ENABLED(POWER_MONITOR_CURRENT) - POWERMON_CURRENT, - #endif - #if ENABLED(POWER_MONITOR_VOLTAGE) - POWERMON_VOLTS, - #endif - ADC_PIN_COUNT -}; +uint16_t analogRead(const pin_t pin) { + const bool is_analog = _GET_MODE(pin) == GPIO_INPUT_ANALOG; + return is_analog ? analogRead(uint8_t(pin)) : 0; +} + +// Wrapper to maple unprotected analogWrite +void analogWrite(const pin_t pin, int pwm_val8) { + if (PWM_PIN(pin)) analogWrite(uint8_t(pin), pwm_val8); +} -uint16_t HAL_adc_results[ADC_PIN_COUNT]; +uint16_t MarlinHAL::adc_result; // ------------------------ // Private functions // ------------------------ + static void NVIC_SetPriorityGrouping(uint32_t PriorityGroup) { uint32_t reg_value; - uint32_t PriorityGroupTmp = (PriorityGroup & (uint32_t)0x07); /* only values 0..7 are used */ + uint32_t PriorityGroupTmp = (PriorityGroup & (uint32_t)0x07); // only values 0..7 are used - reg_value = SCB->AIRCR; /* read old register configuration */ - reg_value &= ~(SCB_AIRCR_VECTKEY_Msk | SCB_AIRCR_PRIGROUP_Msk); /* clear bits to change */ + reg_value = SCB->AIRCR; // read old register configuration + reg_value &= ~(SCB_AIRCR_VECTKEY_Msk | SCB_AIRCR_PRIGROUP_Msk); // clear bits to change reg_value = (reg_value | ((uint32_t)0x5FA << SCB_AIRCR_VECTKEY_Pos) | - (PriorityGroupTmp << 8)); /* Insert write key and priorty group */ + (PriorityGroupTmp << 8)); // Insert write key & priority group SCB->AIRCR = reg_value; } @@ -255,6 +150,8 @@ static void NVIC_SetPriorityGrouping(uint32_t PriorityGroup) { // Public functions // ------------------------ +void flashFirmware(const int16_t) { hal.reboot(); } + // // Leave PA11/PA12 intact if USBSerial is not used // @@ -274,7 +171,11 @@ static void NVIC_SetPriorityGrouping(uint32_t PriorityGroup) { TERN_(POSTMORTEM_DEBUGGING, extern void install_min_serial()); -void HAL_init() { +// ------------------------ +// MarlinHAL class +// ------------------------ + +void MarlinHAL::init() { NVIC_SetPriorityGrouping(0x3); #if PIN_EXISTS(LED) OUT_WRITE(LED_PIN, LOW); @@ -287,13 +188,13 @@ void HAL_init() { #if PIN_EXISTS(USB_CONNECT) OUT_WRITE(USB_CONNECT_PIN, !USB_CONNECT_INVERTING); // USB clear connection delay(1000); // Give OS time to notice - OUT_WRITE(USB_CONNECT_PIN, USB_CONNECT_INVERTING); + WRITE(USB_CONNECT_PIN, USB_CONNECT_INVERTING); #endif TERN_(POSTMORTEM_DEBUGGING, install_min_serial()); // Install the minimal serial handler } // HAL idle task -void HAL_idletask() { +void MarlinHAL::idletask() { #if HAS_SHARED_MEDIA // If Marlin is using the SD card we need to lock it to prevent access from // a PC via USB. @@ -308,14 +209,7 @@ void HAL_idletask() { #endif } -void HAL_clear_reset_source() { } - -/** - * TODO: Check this and change or remove. - */ -uint8_t HAL_get_reset_source() { return RST_POWER_ON; } - -void _delay_ms(const int delay_ms) { delay(delay_ms); } +void MarlinHAL::reboot() { nvic_sys_reset(); } extern "C" { extern unsigned int _ebss; // end of bss section @@ -349,101 +243,99 @@ extern "C" { } */ -// ------------------------ +// // ADC -// ------------------------ +// + +enum ADCIndex : uint8_t { + OPTITEM(HAS_TEMP_ADC_0, TEMP_0) + OPTITEM(HAS_TEMP_ADC_1, TEMP_1) + OPTITEM(HAS_TEMP_ADC_2, TEMP_2) + OPTITEM(HAS_TEMP_ADC_3, TEMP_3) + OPTITEM(HAS_TEMP_ADC_4, TEMP_4) + OPTITEM(HAS_TEMP_ADC_5, TEMP_5) + OPTITEM(HAS_TEMP_ADC_6, TEMP_6) + OPTITEM(HAS_TEMP_ADC_7, TEMP_7) + OPTITEM(HAS_HEATED_BED, TEMP_BED) + OPTITEM(HAS_TEMP_CHAMBER, TEMP_CHAMBER) + OPTITEM(HAS_TEMP_ADC_PROBE, TEMP_PROBE) + OPTITEM(HAS_TEMP_COOLER, TEMP_COOLER) + OPTITEM(HAS_TEMP_BOARD, TEMP_BOARD) + OPTITEM(FILAMENT_WIDTH_SENSOR, FILWIDTH) + OPTITEM(HAS_ADC_BUTTONS, ADC_KEY) + OPTITEM(HAS_JOY_ADC_X, JOY_X) + OPTITEM(HAS_JOY_ADC_Y, JOY_Y) + OPTITEM(HAS_JOY_ADC_Z, JOY_Z) + OPTITEM(POWER_MONITOR_CURRENT, POWERMON_CURRENT) + OPTITEM(POWER_MONITOR_VOLTAGE, POWERMON_VOLTS) + ADC_COUNT +}; + +static uint16_t adc_results[ADC_COUNT]; + // Init the AD in continuous capture mode -void HAL_adc_init() { +void MarlinHAL::adc_init() { + static const uint8_t adc_pins[] = { + OPTITEM(HAS_TEMP_ADC_0, TEMP_0_PIN) + OPTITEM(HAS_TEMP_ADC_1, TEMP_1_PIN) + OPTITEM(HAS_TEMP_ADC_2, TEMP_2_PIN) + OPTITEM(HAS_TEMP_ADC_3, TEMP_3_PIN) + OPTITEM(HAS_TEMP_ADC_4, TEMP_4_PIN) + OPTITEM(HAS_TEMP_ADC_5, TEMP_5_PIN) + OPTITEM(HAS_TEMP_ADC_6, TEMP_6_PIN) + OPTITEM(HAS_TEMP_ADC_7, TEMP_7_PIN) + OPTITEM(HAS_HEATED_BED, TEMP_BED_PIN) + OPTITEM(HAS_TEMP_CHAMBER, TEMP_CHAMBER_PIN) + OPTITEM(HAS_TEMP_ADC_PROBE, TEMP_PROBE_PIN) + OPTITEM(HAS_TEMP_COOLER, TEMP_COOLER_PIN) + OPTITEM(HAS_TEMP_BOARD, TEMP_BOARD_PIN) + OPTITEM(FILAMENT_WIDTH_SENSOR, FILWIDTH_PIN) + OPTITEM(HAS_ADC_BUTTONS, ADC_KEYPAD_PIN) + OPTITEM(HAS_JOY_ADC_X, JOY_X_PIN) + OPTITEM(HAS_JOY_ADC_Y, JOY_Y_PIN) + OPTITEM(HAS_JOY_ADC_Z, JOY_Z_PIN) + OPTITEM(POWER_MONITOR_CURRENT, POWER_MONITOR_CURRENT_PIN) + OPTITEM(POWER_MONITOR_VOLTAGE, POWER_MONITOR_VOLTAGE_PIN) + }; + static STM32ADC adc(ADC1); // configure the ADC adc.calibrate(); - #if F_CPU > 72000000 - adc.setSampleRate(ADC_SMPR_71_5); // 71.5 ADC cycles - #else - adc.setSampleRate(ADC_SMPR_41_5); // 41.5 ADC cycles - #endif - adc.setPins((uint8_t *)adc_pins, ADC_PIN_COUNT); - adc.setDMA(HAL_adc_results, (uint16_t)ADC_PIN_COUNT, (uint32_t)(DMA_MINC_MODE | DMA_CIRC_MODE), nullptr); + adc.setSampleRate((F_CPU > 72000000) ? ADC_SMPR_71_5 : ADC_SMPR_41_5); // 71.5 or 41.5 ADC cycles + adc.setPins((uint8_t *)adc_pins, ADC_COUNT); + adc.setDMA(adc_results, uint16_t(ADC_COUNT), uint32_t(DMA_MINC_MODE | DMA_CIRC_MODE), nullptr); adc.setScanMode(); adc.setContinuous(); adc.startConversion(); } -void HAL_adc_start_conversion(const uint8_t adc_pin) { - //TEMP_PINS pin_index; - TempPinIndex pin_index; - switch (adc_pin) { +void MarlinHAL::adc_start(const pin_t pin) { + #define __TCASE(N,I) case N: pin_index = I; break; + #define _TCASE(C,N,I) TERN_(C, __TCASE(N, I)) + ADCIndex pin_index; + switch (pin) { default: return; - #if HAS_TEMP_ADC_0 - case TEMP_0_PIN: pin_index = TEMP_0; break; - #endif - #if HAS_TEMP_ADC_PROBE - case TEMP_PROBE_PIN: pin_index = TEMP_PROBE; break; - #endif - #if HAS_HEATED_BED - case TEMP_BED_PIN: pin_index = TEMP_BED; break; - #endif - #if HAS_TEMP_CHAMBER - case TEMP_CHAMBER_PIN: pin_index = TEMP_CHAMBER; break; - #endif - #if HAS_TEMP_ADC_1 - case TEMP_1_PIN: pin_index = TEMP_1; break; - #endif - #if HAS_TEMP_ADC_2 - case TEMP_2_PIN: pin_index = TEMP_2; break; - #endif - #if HAS_TEMP_ADC_3 - case TEMP_3_PIN: pin_index = TEMP_3; break; - #endif - #if HAS_TEMP_ADC_4 - case TEMP_4_PIN: pin_index = TEMP_4; break; - #endif - #if HAS_TEMP_ADC_5 - case TEMP_5_PIN: pin_index = TEMP_5; break; - #endif - #if HAS_TEMP_ADC_6 - case TEMP_6_PIN: pin_index = TEMP_6; break; - #endif - #if HAS_TEMP_ADC_7 - case TEMP_7_PIN: pin_index = TEMP_7; break; - #endif - #if HAS_JOY_ADC_X - case JOY_X_PIN: pin_index = JOY_X; break; - #endif - #if HAS_JOY_ADC_Y - case JOY_Y_PIN: pin_index = JOY_Y; break; - #endif - #if HAS_JOY_ADC_Z - case JOY_Z_PIN: pin_index = JOY_Z; break; - #endif - #if ENABLED(FILAMENT_WIDTH_SENSOR) - case FILWIDTH_PIN: pin_index = FILWIDTH; break; - #endif - #if HAS_ADC_BUTTONS - case ADC_KEYPAD_PIN: pin_index = ADC_KEY; break; - #endif - #if ENABLED(POWER_MONITOR_CURRENT) - case POWER_MONITOR_CURRENT_PIN: pin_index = POWERMON_CURRENT; break; - #endif - #if ENABLED(POWER_MONITOR_VOLTAGE) - case POWER_MONITOR_VOLTAGE_PIN: pin_index = POWERMON_VOLTS; break; - #endif + _TCASE(HAS_TEMP_ADC_0, TEMP_0_PIN, TEMP_0) + _TCASE(HAS_TEMP_ADC_1, TEMP_1_PIN, TEMP_1) + _TCASE(HAS_TEMP_ADC_2, TEMP_2_PIN, TEMP_2) + _TCASE(HAS_TEMP_ADC_3, TEMP_3_PIN, TEMP_3) + _TCASE(HAS_TEMP_ADC_4, TEMP_4_PIN, TEMP_4) + _TCASE(HAS_TEMP_ADC_5, TEMP_5_PIN, TEMP_5) + _TCASE(HAS_TEMP_ADC_6, TEMP_6_PIN, TEMP_6) + _TCASE(HAS_TEMP_ADC_7, TEMP_7_PIN, TEMP_7) + _TCASE(HAS_HEATED_BED, TEMP_BED_PIN, TEMP_BED) + _TCASE(HAS_TEMP_CHAMBER, TEMP_CHAMBER_PIN, TEMP_CHAMBER) + _TCASE(HAS_TEMP_ADC_PROBE, TEMP_PROBE_PIN, TEMP_PROBE) + _TCASE(HAS_TEMP_COOLER, TEMP_COOLER_PIN, TEMP_COOLER) + _TCASE(HAS_TEMP_BOARD, TEMP_BOARD_PIN, TEMP_BOARD) + _TCASE(HAS_JOY_ADC_X, JOY_X_PIN, JOY_X) + _TCASE(HAS_JOY_ADC_Y, JOY_Y_PIN, JOY_Y) + _TCASE(HAS_JOY_ADC_Z, JOY_Z_PIN, JOY_Z) + _TCASE(FILAMENT_WIDTH_SENSOR, FILWIDTH_PIN, FILWIDTH) + _TCASE(HAS_ADC_BUTTONS, ADC_KEYPAD_PIN, ADC_KEY) + _TCASE(POWER_MONITOR_CURRENT, POWER_MONITOR_CURRENT_PIN, POWERMON_CURRENT) + _TCASE(POWER_MONITOR_VOLTAGE, POWER_MONITOR_VOLTAGE_PIN, POWERMON_VOLTS) } - HAL_adc_result = (HAL_adc_results[(int)pin_index] >> 2) & 0x3FF; // shift to get 10 bits only. + adc_result = adc_results[(int)pin_index] >> (12 - HAL_ADC_RESOLUTION); // shift out unused bits } -uint16_t HAL_adc_get_result() { return HAL_adc_result; } - -uint16_t analogRead(pin_t pin) { - const bool is_analog = _GET_MODE(pin) == GPIO_INPUT_ANALOG; - return is_analog ? analogRead(uint8_t(pin)) : 0; -} - -// Wrapper to maple unprotected analogWrite -void analogWrite(pin_t pin, int pwm_val8) { - if (PWM_PIN(pin)) - analogWrite(uint8_t(pin), pwm_val8); -} - -void flashFirmware(const int16_t) { nvic_sys_reset(); } - #endif // __STM32F1__ diff --git a/Marlin/src/HAL/STM32F1/HAL.h b/Marlin/src/HAL/STM32F1/HAL.h index 30bf60b6e8a1..8fbb43c6022b 100644 --- a/Marlin/src/HAL/STM32F1/HAL.h +++ b/Marlin/src/HAL/STM32F1/HAL.h @@ -36,7 +36,6 @@ #include "fastio.h" #include "watchdog.h" - #include #include @@ -52,22 +51,32 @@ // Defines // ------------------------ +// +// Default graphical display delays +// +#define CPU_ST7920_DELAY_1 300 +#define CPU_ST7920_DELAY_2 40 +#define CPU_ST7920_DELAY_3 340 + #ifndef STM32_FLASH_SIZE - #if EITHER(MCU_STM32F103RE, MCU_STM32F103VE) + #if ANY(MCU_STM32F103RE, MCU_STM32F103VE, MCU_STM32F103ZE) #define STM32_FLASH_SIZE 512 #else #define STM32_FLASH_SIZE 256 #endif #endif -#ifdef SERIAL_USB - typedef ForwardSerial0Type< USBSerial > DefaultSerial; - extern DefaultSerial MSerial; +// ------------------------ +// Serial ports +// ------------------------ - #if !HAS_SD_HOST_DRIVE - #define UsbSerial MSerial - #else +#ifdef SERIAL_USB + typedef ForwardSerial1Class< USBSerial > DefaultSerial1; + extern DefaultSerial1 MSerial0; + #if HAS_SD_HOST_DRIVE #define UsbSerial MarlinCompositeSerial + #else + #define UsbSerial MSerial0 #endif #endif @@ -81,24 +90,33 @@ #endif #if SERIAL_PORT == -1 - #define MYSERIAL0 UsbSerial + #define MYSERIAL1 UsbSerial #elif WITHIN(SERIAL_PORT, 1, NUM_UARTS) - #define MYSERIAL0 MSERIAL(SERIAL_PORT) -#elif NUM_UARTS == 5 - #error "SERIAL_PORT must be -1 or from 1 to 5. Please update your configuration." + #define MYSERIAL1 MSERIAL(SERIAL_PORT) #else - #error "SERIAL_PORT must be -1 or from 1 to 3. Please update your configuration." + #define MYSERIAL1 MSERIAL(1) // dummy port + static_assert(false, "SERIAL_PORT must be from 1 to " STRINGIFY(NUM_UARTS) ". You can also use -1 if the board supports Native USB.") #endif #ifdef SERIAL_PORT_2 #if SERIAL_PORT_2 == -1 - #define MYSERIAL1 UsbSerial + #define MYSERIAL2 UsbSerial #elif WITHIN(SERIAL_PORT_2, 1, NUM_UARTS) - #define MYSERIAL1 MSERIAL(SERIAL_PORT_2) - #elif NUM_UARTS == 5 - #error "SERIAL_PORT_2 must be -1 or from 1 to 5. Please update your configuration." + #define MYSERIAL2 MSERIAL(SERIAL_PORT_2) + #else + #define MYSERIAL2 MSERIAL(1) // dummy port + static_assert(false, "SERIAL_PORT_2 must be from 1 to " STRINGIFY(NUM_UARTS) ". You can also use -1 if the board supports Native USB.") + #endif +#endif + +#ifdef SERIAL_PORT_3 + #if SERIAL_PORT_3 == -1 + #define MYSERIAL3 UsbSerial + #elif WITHIN(SERIAL_PORT_3, 1, NUM_UARTS) + #define MYSERIAL3 MSERIAL(SERIAL_PORT_3) #else - #error "SERIAL_PORT_2 must be -1 or from 1 to 3. Please update your configuration." + #define MYSERIAL3 MSERIAL(1) // dummy port + static_assert(false, "SERIAL_PORT_3 must be from 1 to " STRINGIFY(NUM_UARTS) ". You can also use -1 if the board supports Native USB.") #endif #endif @@ -107,10 +125,9 @@ #define MMU2_SERIAL UsbSerial #elif WITHIN(MMU2_SERIAL_PORT, 1, NUM_UARTS) #define MMU2_SERIAL MSERIAL(MMU2_SERIAL_PORT) - #elif NUM_UARTS == 5 - #error "MMU2_SERIAL_PORT must be -1 or from 1 to 5. Please update your configuration." #else - #error "MMU2_SERIAL_PORT must be -1 or from 1 to 3. Please update your configuration." + #define MMU2_SERIAL MSERIAL(1) // dummy port + static_assert(false, "MMU2_SERIAL_PORT must be from 1 to " STRINGIFY(NUM_UARTS) ". You can also use -1 if the board supports Native USB.") #endif #endif @@ -119,21 +136,15 @@ #define LCD_SERIAL UsbSerial #elif WITHIN(LCD_SERIAL_PORT, 1, NUM_UARTS) #define LCD_SERIAL MSERIAL(LCD_SERIAL_PORT) - #elif NUM_UARTS == 5 - #error "LCD_SERIAL_PORT must be -1 or from 1 to 5. Please update your configuration." #else - #error "LCD_SERIAL_PORT must be -1 or from 1 to 3. Please update your configuration." + #define LCD_SERIAL MSERIAL(1) // dummy port + static_assert(false, "LCD_SERIAL_PORT must be from 1 to " STRINGIFY(NUM_UARTS) ". You can also use -1 if the board supports Native USB.") #endif #if HAS_DGUS_LCD #define SERIAL_GET_TX_BUFFER_FREE() LCD_SERIAL.availableForWrite() #endif #endif -// Set interrupt grouping for this MCU -void HAL_init(); -#define HAL_IDLETASK 1 -void HAL_idletask(); - /** * TODO: review this to return 1 for pins that are not analog input */ @@ -146,15 +157,7 @@ void HAL_idletask(); #define NO_COMPILE_TIME_PWM #endif -#define CRITICAL_SECTION_START() uint32_t primask = __get_primask(); (void)__iCliRetVal() -#define CRITICAL_SECTION_END() if (!primask) (void)__iSeiRetVal() -#define ISRS_ENABLED() (!__get_primask()) -#define ENABLE_ISRS() ((void)__iSeiRetVal()) -#define DISABLE_ISRS() ((void)__iCliRetVal()) - -// On AVR this is in math.h? -#define square(x) ((x)*(x)) - +// Reset Reason #define RST_POWER_ON 1 #define RST_EXTERNAL 2 #define RST_BROWN_OUT 4 @@ -170,46 +173,63 @@ void HAL_idletask(); typedef int8_t pin_t; // ------------------------ -// Public Variables +// Interrupts // ------------------------ -// Result of last ADC conversion -extern uint16_t HAL_adc_result; +#define CRITICAL_SECTION_START() const bool irqon = !__get_primask(); (void)__iCliRetVal() +#define CRITICAL_SECTION_END() if (!primask) (void)__iSeiRetVal() +#define cli() noInterrupts() +#define sei() interrupts() // ------------------------ -// Public functions +// ADC // ------------------------ -// Disable interrupts -#define cli() noInterrupts() +#ifdef ADC_RESOLUTION + #define HAL_ADC_RESOLUTION ADC_RESOLUTION +#else + #define HAL_ADC_RESOLUTION 12 +#endif -// Enable interrupts -#define sei() interrupts() +#define HAL_ADC_VREF 3.3 -// Memory related -#define __bss_end __bss_end__ +uint16_t analogRead(const pin_t pin); // need hal.adc_enable() first +void analogWrite(const pin_t pin, int pwm_val8); // PWM only! mul by 257 in maple!? -// Clear reset reason -void HAL_clear_reset_source(); +// +// Pin Mapping for M42, M43, M226 +// +#define GET_PIN_MAP_PIN(index) index +#define GET_PIN_MAP_INDEX(pin) pin +#define PARSED_PIN_INDEX(code, dval) parser.intval(code, dval) -// Reset reason -uint8_t HAL_get_reset_source(); +#define JTAG_DISABLE() afio_cfg_debug_ports(AFIO_DEBUG_SW_ONLY) +#define JTAGSWD_DISABLE() afio_cfg_debug_ports(AFIO_DEBUG_NONE) -inline void HAL_reboot() {} // reboot the board or restart the bootloader +#define PLATFORM_M997_SUPPORT +void flashFirmware(const int16_t); -void _delay_ms(const int delay); +#define HAL_CAN_SET_PWM_FREQ // This HAL supports PWM Frequency adjustment +#ifndef PWM_FREQUENCY + #define PWM_FREQUENCY 1000 // Default PWM Frequency +#endif -#pragma GCC diagnostic push -#pragma GCC diagnostic ignored "-Wunused-function" +// ------------------------ +// Class Utilities +// ------------------------ -/* -extern "C" { - int freeMemory(); -} -*/ +// Memory related +#define __bss_end __bss_end__ + +void _delay_ms(const int ms); extern "C" char* _sbrk(int incr); +#pragma GCC diagnostic push +#if GCC_VERSION <= 50000 + #pragma GCC diagnostic ignored "-Wunused-function" +#endif + static inline int freeMemory() { volatile char top; return &top - _sbrk(0); @@ -217,49 +237,70 @@ static inline int freeMemory() { #pragma GCC diagnostic pop -// -// ADC -// +// ------------------------ +// MarlinHAL Class +// ------------------------ -#define HAL_ANALOG_SELECT(pin) pinMode(pin, INPUT_ANALOG); +class MarlinHAL { +public: -void HAL_adc_init(); + // Earliest possible init, before setup() + MarlinHAL() {} -#define HAL_ADC_VREF 3.3 -#define HAL_ADC_RESOLUTION 10 -#define HAL_START_ADC(pin) HAL_adc_start_conversion(pin) -#define HAL_READ_ADC() HAL_adc_result -#define HAL_ADC_READY() true + static void init(); // Called early in setup() + static void init_board() {} // Called less early in setup() + static void reboot(); // Restart the firmware from 0x0 -void HAL_adc_start_conversion(const uint8_t adc_pin); -uint16_t HAL_adc_get_result(); + // Interrupts + static bool isr_state() { return !__get_primask(); } + static void isr_on() { ((void)__iSeiRetVal()); } + static void isr_off() { ((void)__iCliRetVal()); } -uint16_t analogRead(pin_t pin); // need HAL_ANALOG_SELECT() first -void analogWrite(pin_t pin, int pwm_val8); // PWM only! mul by 257 in maple!? + static void delay_ms(const int ms) { delay(ms); } -#define GET_PIN_MAP_PIN(index) index -#define GET_PIN_MAP_INDEX(pin) pin -#define PARSED_PIN_INDEX(code, dval) parser.intval(code, dval) + // Tasks, called from idle() + static void idletask(); -#define JTAG_DISABLE() afio_cfg_debug_ports(AFIO_DEBUG_SW_ONLY) -#define JTAGSWD_DISABLE() afio_cfg_debug_ports(AFIO_DEBUG_NONE) + // Reset + static uint8_t get_reset_source() { return RST_POWER_ON; } + static void clear_reset_source() {} -#define PLATFORM_M997_SUPPORT -void flashFirmware(const int16_t); + // Free SRAM + static int freeMemory() { return ::freeMemory(); } -#define HAL_CAN_SET_PWM_FREQ // This HAL supports PWM Frequency adjustment + // + // ADC Methods + // -/** - * set_pwm_frequency - * Set the frequency of the timer corresponding to the provided pin - * All Timer PWM pins run at the same frequency - */ -void set_pwm_frequency(const pin_t pin, int f_desired); + static uint16_t adc_result; -/** - * set_pwm_duty - * Set the PWM duty cycle of the provided pin to the provided value - * Optionally allows inverting the duty cycle [default = false] - * Optionally allows changing the maximum size of the provided value to enable finer PWM duty control [default = 255] - */ -void set_pwm_duty(const pin_t pin, const uint16_t v, const uint16_t v_size=255, const bool invert=false); + // Called by Temperature::init once at startup + static void adc_init(); + + // Called by Temperature::init for each sensor at startup + static void adc_enable(const pin_t pin) { pinMode(pin, INPUT_ANALOG); } + + // Begin ADC sampling on the given channel + static void adc_start(const pin_t pin); + + // Is the ADC ready for reading? + static bool adc_ready() { return true; } + + // The current value of the ADC register + static uint16_t adc_value() { return adc_result; } + + /** + * Set the PWM duty cycle for the pin to the given value. + * Optionally invert the duty cycle [default = false] + * Optionally change the maximum size of the provided value to enable finer PWM duty control [default = 255] + * The timer must be pre-configured with set_pwm_frequency() if the default frequency is not desired. + */ + static void set_pwm_duty(const pin_t pin, const uint16_t v, const uint16_t=255, const bool=false); + + /** + * Set the frequency of the timer for the given pin. + * All Timer PWM pins run at the same frequency. + */ + static void set_pwm_frequency(const pin_t pin, const uint16_t f_desired); + +}; diff --git a/Marlin/src/HAL/STM32F1/HAL_MinSerial.cpp b/Marlin/src/HAL/STM32F1/HAL_MinSerial.cpp index e6b89f1105c7..0fc3d014d484 100644 --- a/Marlin/src/HAL/STM32F1/HAL_MinSerial.cpp +++ b/Marlin/src/HAL/STM32F1/HAL_MinSerial.cpp @@ -44,8 +44,8 @@ static void TXBegin() { #warning "Using POSTMORTEM_DEBUGGING requires a physical U(S)ART hardware in case of severe error." #warning "Disabling the severe error reporting feature currently because the used serial port is not a HW port." #else - // We use MYSERIAL0 here, so we need to figure out how to get the linked register - struct usart_dev* dev = MYSERIAL0.c_dev(); + // We use MYSERIAL1 here, so we need to figure out how to get the linked register + struct usart_dev* dev = MYSERIAL1.c_dev(); // Or use this if removing libmaple // int irq = dev->irq_num; @@ -55,7 +55,7 @@ static void TXBegin() { nvic_irq_disable(dev->irq_num); // Use this if removing libmaple - //NVIC_BASE->ICER[1] |= _BV(irq - 32); + //SBI(NVIC_BASE->ICER[1], irq - 32); // We NEED memory barriers to ensure Interrupts are actually disabled! // ( https://dzone.com/articles/nvic-disabling-interrupts-on-arm-cortex-m-and-the ) @@ -80,7 +80,7 @@ static void TXBegin() { #define sw_barrier() __asm__ volatile("": : :"memory"); static void TX(char c) { #if WITHIN(SERIAL_PORT, 1, 6) - struct usart_dev* dev = MYSERIAL0.c_dev(); + struct usart_dev* dev = MYSERIAL1.c_dev(); while (!(dev->regs->SR & USART_SR_TXE)) { TERN_(USE_WATCHDOG, HAL_watchdog_refresh()); sw_barrier(); diff --git a/Marlin/src/HAL/STM32F1/HAL_SPI.cpp b/Marlin/src/HAL/STM32F1/HAL_SPI.cpp index 7e876f765fb2..abb348d743c5 100644 --- a/Marlin/src/HAL/STM32F1/HAL_SPI.cpp +++ b/Marlin/src/HAL/STM32F1/HAL_SPI.cpp @@ -123,7 +123,7 @@ uint8_t spiRec() { * * @details Uses DMA */ -void spiRead(uint8_t* buf, uint16_t nbyte) { +void spiRead(uint8_t *buf, uint16_t nbyte) { SPI.dmaTransfer(0, const_cast(buf), nbyte); } @@ -146,7 +146,7 @@ void spiSend(uint8_t b) { * * @details Use DMA */ -void spiSendBlock(uint8_t token, const uint8_t* buf) { +void spiSendBlock(uint8_t token, const uint8_t *buf) { SPI.send(token); SPI.dmaSend(const_cast(buf), 512); } @@ -160,7 +160,7 @@ uint8_t spiRec(uint32_t chan) { return SPI.transfer(0xFF); } void spiSend(uint32_t chan, byte b) { SPI.send(b); } // Write buffer to specified SPI channel -void spiSend(uint32_t chan, const uint8_t* buf, size_t n) { +void spiSend(uint32_t chan, const uint8_t *buf, size_t n) { for (size_t p = 0; p < n; p++) spiSend(chan, buf[p]); } diff --git a/Marlin/src/HAL/STM32F1/MarlinSerial.cpp b/Marlin/src/HAL/STM32F1/MarlinSerial.cpp index c404e81b35e8..6dabcde51ead 100644 --- a/Marlin/src/HAL/STM32F1/MarlinSerial.cpp +++ b/Marlin/src/HAL/STM32F1/MarlinSerial.cpp @@ -60,7 +60,7 @@ static inline __always_inline void my_usart_irq(ring_buffer *rb, ring_buffer *wb } else if (srflags & USART_SR_ORE) { // overrun and empty data, just do a dummy read to clear ORE - // and prevent a raise condition where a continous interrupt stream (due to ORE set) occurs + // and prevent a raise condition where a continuous interrupt stream (due to ORE set) occurs // (see chapter "Overrun error" ) in STM32 reference manual regs->DR; } @@ -134,12 +134,12 @@ constexpr bool IsSerialClassAllowed(const HardwareSerial&) { return false; } // If you encounter this error, replace SerialX with MSerialX, for example MSerial3. // Non-TMC ports were already validated in HAL.h, so do not require verbose error messages. -#ifdef MYSERIAL0 - CHECK_CFG_SERIAL(MYSERIAL0); -#endif #ifdef MYSERIAL1 CHECK_CFG_SERIAL(MYSERIAL1); #endif +#ifdef MYSERIAL2 + CHECK_CFG_SERIAL(MYSERIAL2); +#endif #ifdef LCD_SERIAL CHECK_CFG_SERIAL(LCD_SERIAL); #endif @@ -167,6 +167,15 @@ constexpr bool IsSerialClassAllowed(const HardwareSerial&) { return false; } #if AXIS_HAS_HW_SERIAL(Z4) CHECK_AXIS_SERIAL(Z4); #endif +#if AXIS_HAS_HW_SERIAL(I) + CHECK_AXIS_SERIAL(I); +#endif +#if AXIS_HAS_HW_SERIAL(J) + CHECK_AXIS_SERIAL(J); +#endif +#if AXIS_HAS_HW_SERIAL(K) + CHECK_AXIS_SERIAL(K); +#endif #if AXIS_HAS_HW_SERIAL(E0) CHECK_AXIS_SERIAL(E0); #endif diff --git a/Marlin/src/HAL/STM32F1/MarlinSerial.h b/Marlin/src/HAL/STM32F1/MarlinSerial.h index 692e97e61832..dda32fe7a2ef 100644 --- a/Marlin/src/HAL/STM32F1/MarlinSerial.h +++ b/Marlin/src/HAL/STM32F1/MarlinSerial.h @@ -47,7 +47,7 @@ struct MarlinSerial : public HardwareSerial { #endif }; -typedef Serial0Type MSerialT; +typedef Serial1Class MSerialT; extern MSerialT MSerial1; extern MSerialT MSerial2; diff --git a/Marlin/src/HAL/STM32F1/SPI.cpp b/Marlin/src/HAL/STM32F1/SPI.cpp index c0a35b88d1a2..1ce2c7d3fd5d 100644 --- a/Marlin/src/HAL/STM32F1/SPI.cpp +++ b/Marlin/src/HAL/STM32F1/SPI.cpp @@ -91,6 +91,14 @@ static const spi_pins board_spi_pins[] __FLASH__ = { static void *_spi3_this; #endif +/** + * @brief Wait until TXE (tx empty) flag is set and BSY (busy) flag unset. + */ +static inline void waitSpiTxEnd(spi_dev *spi_d) { + while (spi_is_tx_empty(spi_d) == 0) { /* nada */ } // wait until TXE=1 + while (spi_is_busy(spi_d) != 0) { /* nada */ } // wait until BSY=0 +} + /** * Constructor */ @@ -363,8 +371,8 @@ uint16_t SPIClass::transfer16(uint16_t data) const { /** * Roger Clark and Victor Perez, 2015 * Performs a DMA SPI transfer with at least a receive buffer. - * If a TX buffer is not provided, FF is sent over and over for the lenght of the transfer. - * On exit TX buffer is not modified, and RX buffer cotains the received data. + * If a TX buffer is not provided, FF is sent over and over for the length of the transfer. + * On exit TX buffer is not modified, and RX buffer contains the received data. * Still in progress. */ void SPIClass::dmaTransferSet(const void *transmitBuf, void *receiveBuf) { diff --git a/Marlin/src/HAL/STM32F1/SPI.h b/Marlin/src/HAL/STM32F1/SPI.h index 828644f1ddb5..13f4d5ed6cfe 100644 --- a/Marlin/src/HAL/STM32F1/SPI.h +++ b/Marlin/src/HAL/STM32F1/SPI.h @@ -138,8 +138,8 @@ class SPISettings { spi_dev *spi_d; dma_channel spiRxDmaChannel, spiTxDmaChannel; dma_dev* spiDmaDev; - void (*receiveCallback)() = NULL; - void (*transmitCallback)() = NULL; + void (*receiveCallback)() = nullptr; + void (*transmitCallback)() = nullptr; friend class SPIClass; }; @@ -414,12 +414,4 @@ class SPIClass { */ }; -/** - * @brief Wait until TXE (tx empty) flag is set and BSY (busy) flag unset. - */ -static inline void waitSpiTxEnd(spi_dev *spi_d) { - while (spi_is_tx_empty(spi_d) == 0) { /* nada */ } // wait until TXE=1 - while (spi_is_busy(spi_d) != 0) { /* nada */ } // wait until BSY=0 -} - extern SPIClass SPI; diff --git a/Marlin/src/HAL/STM32F1/Servo.cpp b/Marlin/src/HAL/STM32F1/Servo.cpp index 36f7c6d51273..8dc1ef7b6a5b 100644 --- a/Marlin/src/HAL/STM32F1/Servo.cpp +++ b/Marlin/src/HAL/STM32F1/Servo.cpp @@ -60,7 +60,7 @@ uint8_t ServoCount = 0; #define US_TO_ANGLE(us) int16_t(map((us), SERVO_DEFAULT_MIN_PW, SERVO_DEFAULT_MAX_PW, minAngle, maxAngle)) void libServo::servoWrite(uint8_t inPin, uint16_t duty_cycle) { - #ifdef SERVO0_TIMER_NUM + #ifdef MF_TIMER_SERVO0 if (servoIndex == 0) { pwmSetDuty(duty_cycle); return; @@ -74,7 +74,7 @@ void libServo::servoWrite(uint8_t inPin, uint16_t duty_cycle) { libServo::libServo() { servoIndex = ServoCount < MAX_SERVOS ? ServoCount++ : INVALID_SERVO; - timer_set_interrupt_priority(SERVO0_TIMER_NUM, SERVO0_TIMER_IRQ_PRIO); + HAL_timer_set_interrupt_priority(MF_TIMER_SERVO0, SERVO0_TIMER_IRQ_PRIO); } bool libServo::attach(const int32_t inPin, const int32_t inMinAngle, const int32_t inMaxAngle) { @@ -85,7 +85,7 @@ bool libServo::attach(const int32_t inPin, const int32_t inMinAngle, const int32 maxAngle = inMaxAngle; angle = -1; - #ifdef SERVO0_TIMER_NUM + #ifdef MF_TIMER_SERVO0 if (servoIndex == 0 && setupSoftPWM(inPin)) { pin = inPin; // set attached() return true; @@ -119,7 +119,7 @@ bool libServo::detach() { int32_t libServo::read() const { if (attached()) { - #ifdef SERVO0_TIMER_NUM + #ifdef MF_TIMER_SERVO0 if (servoIndex == 0) return angle; #endif timer_dev *tdev = PIN_MAP[pin].timer_device; @@ -141,9 +141,9 @@ void libServo::move(const int32_t value) { } } -#ifdef SERVO0_TIMER_NUM +#ifdef MF_TIMER_SERVO0 extern "C" void Servo_IRQHandler() { - static timer_dev *tdev = get_timer_dev(SERVO0_TIMER_NUM); + static timer_dev *tdev = HAL_get_timer_dev(MF_TIMER_SERVO0); uint16_t SR = timer_get_status(tdev); if (SR & TIMER_SR_CC1IF) { // channel 1 off #ifdef SERVO0_PWM_OD @@ -164,7 +164,7 @@ void libServo::move(const int32_t value) { } bool libServo::setupSoftPWM(const int32_t inPin) { - timer_dev *tdev = get_timer_dev(SERVO0_TIMER_NUM); + timer_dev *tdev = HAL_get_timer_dev(MF_TIMER_SERVO0); if (!tdev) return false; #ifdef SERVO0_PWM_OD OUT_WRITE_OD(inPin, 1); @@ -189,7 +189,7 @@ void libServo::move(const int32_t value) { } void libServo::pwmSetDuty(const uint16_t duty_cycle) { - timer_dev *tdev = get_timer_dev(SERVO0_TIMER_NUM); + timer_dev *tdev = HAL_get_timer_dev(MF_TIMER_SERVO0); timer_set_compare(tdev, 1, duty_cycle); timer_generate_update(tdev); if (duty_cycle) { @@ -208,7 +208,7 @@ void libServo::move(const int32_t value) { } void libServo::pauseSoftPWM() { // detach - timer_dev *tdev = get_timer_dev(SERVO0_TIMER_NUM); + timer_dev *tdev = HAL_get_timer_dev(MF_TIMER_SERVO0); timer_pause(tdev); pwmSetDuty(0); } diff --git a/Marlin/src/HAL/STM32F1/Servo.h b/Marlin/src/HAL/STM32F1/Servo.h index b6143de81d62..745a1c93f07d 100644 --- a/Marlin/src/HAL/STM32F1/Servo.h +++ b/Marlin/src/HAL/STM32F1/Servo.h @@ -35,7 +35,8 @@ #define SERVO_DEFAULT_MIN_ANGLE 0 #define SERVO_DEFAULT_MAX_ANGLE 180 -#define HAL_SERVO_LIB libServo +class libServo; +typedef libServo hal_servo_t; class libServo { public: diff --git a/Marlin/src/HAL/STM32F1/build_flags.py b/Marlin/src/HAL/STM32F1/build_flags.py index c51fd4fa58fd..970ca8b767f9 100755 --- a/Marlin/src/HAL/STM32F1/build_flags.py +++ b/Marlin/src/HAL/STM32F1/build_flags.py @@ -11,6 +11,7 @@ "-fsigned-char", "-fno-move-loop-invariants", "-fno-strict-aliasing", + "-fsingle-precision-constant", "--specs=nano.specs", "--specs=nosys.specs", @@ -29,25 +30,27 @@ # extra script for linker options else: - from SCons.Script import DefaultEnvironment - env = DefaultEnvironment() - env.Append( + import pioutil + if pioutil.is_pio_build(): + from SCons.Script import DefaultEnvironment + env = DefaultEnvironment() + env.Append( ARFLAGS=["rcs"], ASFLAGS=["-x", "assembler-with-cpp"], CXXFLAGS=[ - "-fabi-version=0", - "-fno-use-cxa-atexit", - "-fno-threadsafe-statics" + "-fabi-version=0", + "-fno-use-cxa-atexit", + "-fno-threadsafe-statics" ], LINKFLAGS=[ - "-Os", - "-mcpu=cortex-m3", - "-ffreestanding", - "-mthumb", - "--specs=nano.specs", - "--specs=nosys.specs", - "-u_printf_float", + "-Os", + "-mcpu=cortex-m3", + "-ffreestanding", + "-mthumb", + "--specs=nano.specs", + "--specs=nosys.specs", + "-u_printf_float", ], - ) + ) diff --git a/Marlin/src/HAL/STM32F1/dogm/u8g_com_stm32duino_swspi.cpp b/Marlin/src/HAL/STM32F1/dogm/u8g_com_stm32duino_swspi.cpp index 784a80c29fd3..26ea1ea19af9 100644 --- a/Marlin/src/HAL/STM32F1/dogm/u8g_com_stm32duino_swspi.cpp +++ b/Marlin/src/HAL/STM32F1/dogm/u8g_com_stm32duino_swspi.cpp @@ -2,6 +2,9 @@ * Marlin 3D Printer Firmware * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * * This program is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation, either version 3 of the License, or @@ -16,13 +19,14 @@ * along with this program. If not, see . * */ + #ifdef __STM32F1__ #include "../../../inc/MarlinConfig.h" #if BOTH(HAS_MARLINUI_U8GLIB, FORCE_SOFT_SPI) -#include +#include #include "../../shared/HAL_SPI.h" #ifndef LCD_SPI_SPEED diff --git a/Marlin/src/HAL/STM32F1/eeprom_bl24cxx.cpp b/Marlin/src/HAL/STM32F1/eeprom_bl24cxx.cpp index a6395698aae7..4e25bc69da4e 100644 --- a/Marlin/src/HAL/STM32F1/eeprom_bl24cxx.cpp +++ b/Marlin/src/HAL/STM32F1/eeprom_bl24cxx.cpp @@ -19,14 +19,13 @@ * along with this program. If not, see . * */ +#ifdef __STM32F1__ /** * PersistentStore for Arduino-style EEPROM interface * with simple implementations supplied by Marlin. */ -#ifdef __STM32F1__ - #include "../../inc/MarlinConfig.h" #if ENABLED(IIC_BL24CXX_EEPROM) @@ -48,13 +47,11 @@ bool PersistentStore::access_start() { eeprom_init(); return true; } bool PersistentStore::access_finish() { return true; } bool PersistentStore::write_data(int &pos, const uint8_t *value, size_t size, uint16_t *crc) { - size_t written = 0; + uint16_t written = 0; while (size--) { uint8_t v = *value; uint8_t * const p = (uint8_t * const)pos; - // EEPROM has only ~100,000 write cycles, - // so only write bytes that have changed! - if (v != eeprom_read_byte(p)) { + if (v != eeprom_read_byte(p)) { // EEPROM has only ~100,000 write cycles, so only write bytes that have changed! eeprom_write_byte(p, v); if (++written & 0x7F) delay(2); else safe_delay(2); // Avoid triggering watchdog during long EEPROM writes if (eeprom_read_byte(p) != v) { diff --git a/Marlin/src/HAL/STM32F1/eeprom_flash.cpp b/Marlin/src/HAL/STM32F1/eeprom_flash.cpp index dfcaaaf29ff3..e7d9dd29e2c5 100644 --- a/Marlin/src/HAL/STM32F1/eeprom_flash.cpp +++ b/Marlin/src/HAL/STM32F1/eeprom_flash.cpp @@ -48,8 +48,8 @@ static uint8_t ram_eeprom[MARLIN_EEPROM_SIZE] __attribute__((aligned(4))) = {0}; static bool eeprom_dirty = false; bool PersistentStore::access_start() { - const uint32_t* source = reinterpret_cast(EEPROM_PAGE0_BASE); - uint32_t* destination = reinterpret_cast(ram_eeprom); + const uint32_t *source = reinterpret_cast(EEPROM_PAGE0_BASE); + uint32_t *destination = reinterpret_cast(ram_eeprom); static_assert(0 == (MARLIN_EEPROM_SIZE) % 4, "MARLIN_EEPROM_SIZE is corrupted. (Must be a multiple of 4.)"); // Ensure copying as uint32_t is safe constexpr size_t eeprom_size_u32 = (MARLIN_EEPROM_SIZE) / 4; diff --git a/Marlin/src/HAL/STM32F1/eeprom_if_iic.cpp b/Marlin/src/HAL/STM32F1/eeprom_if_iic.cpp index ccc3fc537f1a..78b7af0b0418 100644 --- a/Marlin/src/HAL/STM32F1/eeprom_if_iic.cpp +++ b/Marlin/src/HAL/STM32F1/eeprom_if_iic.cpp @@ -40,7 +40,7 @@ void eeprom_init() { BL24CXX::init(); } // Public functions // ------------------------ -void eeprom_write_byte(uint8_t *pos, unsigned char value) { +void eeprom_write_byte(uint8_t *pos, uint8_t value) { const unsigned eeprom_address = (unsigned)pos; return BL24CXX::writeOneByte(eeprom_address, value); } diff --git a/Marlin/src/HAL/STM32F1/eeprom_wired.cpp b/Marlin/src/HAL/STM32F1/eeprom_wired.cpp index 16cfc24af6f6..bc48eef34fa3 100644 --- a/Marlin/src/HAL/STM32F1/eeprom_wired.cpp +++ b/Marlin/src/HAL/STM32F1/eeprom_wired.cpp @@ -2,6 +2,9 @@ * Marlin 3D Printer Firmware * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * * This program is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation, either version 3 of the License, or @@ -44,7 +47,7 @@ bool PersistentStore::access_start() { SET_OUTPUT(BOARD_SPI1_SCK_PIN); SET_OUTPUT(BOARD_SPI1_MOSI_PIN); SET_INPUT(BOARD_SPI1_MISO_PIN); - SET_OUTPUT(SPI_EEPROM1_CS); + SET_OUTPUT(SPI_EEPROM1_CS_PIN); #endif spiInit(0); #endif @@ -52,13 +55,13 @@ bool PersistentStore::access_start() { } bool PersistentStore::write_data(int &pos, const uint8_t *value, size_t size, uint16_t *crc) { + uint16_t written = 0; while (size--) { uint8_t * const p = (uint8_t * const)pos; uint8_t v = *value; - // EEPROM has only ~100,000 write cycles, - // so only write bytes that have changed! - if (v != eeprom_read_byte(p)) { + if (v != eeprom_read_byte(p)) { // EEPROM has only ~100,000 write cycles, so only write bytes that have changed! eeprom_write_byte(p, v); + if (++written & 0x7F) delay(2); else safe_delay(2); // Avoid triggering watchdog during long EEPROM writes if (eeprom_read_byte(p) != v) { SERIAL_ECHO_MSG(STR_ERR_EEPROM_WRITE); return true; diff --git a/Marlin/src/HAL/STM32F1/endstop_interrupts.h b/Marlin/src/HAL/STM32F1/endstop_interrupts.h index bcb07d991d75..4d7edb9496c1 100644 --- a/Marlin/src/HAL/STM32F1/endstop_interrupts.h +++ b/Marlin/src/HAL/STM32F1/endstop_interrupts.h @@ -71,4 +71,10 @@ void setup_endstop_interrupts() { TERN_(HAS_Z4_MAX, _ATTACH(Z4_MAX_PIN)); TERN_(HAS_Z4_MIN, _ATTACH(Z4_MIN_PIN)); TERN_(HAS_Z_MIN_PROBE_PIN, _ATTACH(Z_MIN_PROBE_PIN)); + TERN_(HAS_I_MAX, _ATTACH(I_MAX_PIN)); + TERN_(HAS_I_MIN, _ATTACH(I_MIN_PIN)); + TERN_(HAS_J_MAX, _ATTACH(J_MAX_PIN)); + TERN_(HAS_J_MIN, _ATTACH(J_MIN_PIN)); + TERN_(HAS_K_MAX, _ATTACH(K_MAX_PIN)); + TERN_(HAS_K_MIN, _ATTACH(K_MIN_PIN)); } diff --git a/Marlin/src/HAL/STM32F1/fast_pwm.cpp b/Marlin/src/HAL/STM32F1/fast_pwm.cpp index 884d482af5f1..297804a3ac42 100644 --- a/Marlin/src/HAL/STM32F1/fast_pwm.cpp +++ b/Marlin/src/HAL/STM32F1/fast_pwm.cpp @@ -21,32 +21,57 @@ */ #ifdef __STM32F1__ -#include "../../inc/MarlinConfigPre.h" - -#if NEEDS_HARDWARE_PWM +#include "../../inc/MarlinConfig.h" #include -#include "HAL.h" -#include "timers.h" -void set_pwm_frequency(const pin_t pin, int f_desired) { +#define NR_TIMERS TERN(STM32_XL_DENSITY, 14, 8) // Maple timers, 14 for STM32_XL_DENSITY (F/G chips), 8 for HIGH density (C D E) + +static uint16_t timer_freq[NR_TIMERS]; + +inline uint8_t timer_and_index_for_pin(const pin_t pin, timer_dev **timer_ptr) { + *timer_ptr = PIN_MAP[pin].timer_device; + for (uint8_t i = 0; i < NR_TIMERS; i++) if (*timer_ptr == HAL_get_timer_dev(i)) + return i; + return 0; +} + +void MarlinHAL::set_pwm_duty(const pin_t pin, const uint16_t v, const uint16_t v_size/*=255*/, const bool invert/*=false*/) { + const uint16_t duty = invert ? v_size - v : v; + if (PWM_PIN(pin)) { + timer_dev *timer; UNUSED(timer); + if (timer_freq[timer_and_index_for_pin(pin, &timer)] == 0) + set_pwm_frequency(pin, PWM_FREQUENCY); + const uint8_t channel = PIN_MAP[pin].timer_channel; + timer_set_compare(timer, channel, duty); + timer_set_mode(timer, channel, TIMER_PWM); // PWM Output Mode + } + else { + pinMode(pin, OUTPUT); + digitalWrite(pin, duty < v_size / 2 ? LOW : HIGH); + } +} + +void MarlinHAL::set_pwm_frequency(const pin_t pin, const uint16_t f_desired) { if (!PWM_PIN(pin)) return; // Don't proceed if no hardware timer - timer_dev *timer = PIN_MAP[pin].timer_device; - uint8_t channel = PIN_MAP[pin].timer_channel; + timer_dev *timer; UNUSED(timer); + timer_freq[timer_and_index_for_pin(pin, &timer)] = f_desired; // Protect used timers - if (timer == get_timer_dev(TEMP_TIMER_NUM)) return; - if (timer == get_timer_dev(STEP_TIMER_NUM)) return; - #if PULSE_TIMER_NUM != STEP_TIMER_NUM - if (timer == get_timer_dev(PULSE_TIMER_NUM)) return; + if (timer == HAL_get_timer_dev(MF_TIMER_TEMP)) return; + if (timer == HAL_get_timer_dev(MF_TIMER_STEP)) return; + #if MF_TIMER_PULSE != MF_TIMER_STEP + if (timer == HAL_get_timer_dev(MF_TIMER_PULSE)) return; #endif if (!(timer->regs.bas->SR & TIMER_CR1_CEN)) // Ensure the timer is enabled timer_init(timer); + const uint8_t channel = PIN_MAP[pin].timer_channel; timer_set_mode(timer, channel, TIMER_PWM); - uint16_t preload = 255; // Lock 255 PWM resolution for high frequencies + // Preload (resolution) cannot be equal to duty of 255 otherwise it may not result in digital off or on. + uint16_t preload = 254; int32_t prescaler = (HAL_TIMER_RATE) / (preload + 1) / f_desired - 1; if (prescaler > 65535) { // For low frequencies increase prescaler prescaler = 65535; @@ -57,12 +82,4 @@ void set_pwm_frequency(const pin_t pin, int f_desired) { timer_set_prescaler(timer, prescaler); } -void set_pwm_duty(const pin_t pin, const uint16_t v, const uint16_t v_size/*=255*/, const bool invert/*=false*/) { - timer_dev *timer = PIN_MAP[pin].timer_device; - uint16_t max_val = timer->regs.bas->ARR * v / v_size; - if (invert) max_val = v_size - max_val; - pwmWrite(pin, max_val); -} - -#endif // NEEDS_HARDWARE_PWM #endif // __STM32F1__ diff --git a/Marlin/src/HAL/STM32F1/inc/SanityCheck.h b/Marlin/src/HAL/STM32F1/inc/SanityCheck.h index 89ee66d6464b..fe8f6e0ec24b 100644 --- a/Marlin/src/HAL/STM32F1/inc/SanityCheck.h +++ b/Marlin/src/HAL/STM32F1/inc/SanityCheck.h @@ -39,7 +39,7 @@ #error "SERIAL_STATS_DROPPED_RX is not supported on the STM32F1 platform." #endif -#if ENABLED(NEOPIXEL_LED) +#if ENABLED(NEOPIXEL_LED) && DISABLED(FYSETC_MINI_12864_2_1) #error "NEOPIXEL_LED (Adafruit NeoPixel) is not supported for HAL/STM32F1. Comment out this line to proceed at your own risk!" #endif diff --git a/Marlin/src/HAL/STM32F1/msc_sd.cpp b/Marlin/src/HAL/STM32F1/msc_sd.cpp index a91618499995..f490c83ed829 100644 --- a/Marlin/src/HAL/STM32F1/msc_sd.cpp +++ b/Marlin/src/HAL/STM32F1/msc_sd.cpp @@ -13,9 +13,11 @@ * along with this program. If not, see . * */ +#ifdef __STM32F1__ + #include "../../inc/MarlinConfigPre.h" -#if defined(__STM32F1__) && HAS_SD_HOST_DRIVE +#if HAS_SD_HOST_DRIVE #include "msc_sd.h" #include "SPI.h" @@ -24,7 +26,7 @@ #define PRODUCT_ID 0x29 USBMassStorage MarlinMSC; -Serial0Type MarlinCompositeSerial(true); +Serial1Class MarlinCompositeSerial(true); #include "../../inc/MarlinConfig.h" @@ -43,26 +45,27 @@ Serial0Type MarlinCompositeSerial(true); #if ENABLED(EMERGENCY_PARSER) -// The original callback is not called (no way to retrieve address). -// That callback detects a special STM32 reset sequence: this functionality is not essential -// as M997 achieves the same. -void my_rx_callback(unsigned int, void*) { - // max length of 16 is enough to contain all emergency commands - uint8 buf[16]; + // The original callback is not called (no way to retrieve address). + // That callback detects a special STM32 reset sequence: this functionality is not essential + // as M997 achieves the same. + void my_rx_callback(unsigned int, void*) { + // max length of 16 is enough to contain all emergency commands + uint8 buf[16]; - //rx is usbSerialPart.endpoints[2] - uint16 len = usb_get_ep_rx_count(usbSerialPart.endpoints[2].address); - uint32 total = composite_cdcacm_data_available(); + //rx is usbSerialPart.endpoints[2] + uint16 len = usb_get_ep_rx_count(usbSerialPart.endpoints[2].address); + uint32 total = composite_cdcacm_data_available(); - if (len == 0 || total == 0 || !WITHIN(total, len, COUNT(buf))) - return; + if (len == 0 || total == 0 || !WITHIN(total, len, COUNT(buf))) + return; - // cannot get character by character due to bug in composite_cdcacm_peek_ex - len = composite_cdcacm_peek(buf, total); + // cannot get character by character due to bug in composite_cdcacm_peek_ex + len = composite_cdcacm_peek(buf, total); + + for (uint32 i = 0; i < len; i++) + emergency_parser.update(MarlinCompositeSerial.emergency_state, buf[i+total-len]); + } - for (uint32 i = 0; i < len; i++) - emergency_parser.update(MarlinCompositeSerial.emergency_state, buf[i+total-len]); -} #endif void MSC_SD_init() { @@ -87,8 +90,9 @@ void MSC_SD_init() { MarlinCompositeSerial.registerComponent(); USBComposite.begin(); #if ENABLED(EMERGENCY_PARSER) - composite_cdcacm_set_hooks(USBHID_CDCACM_HOOK_RX, my_rx_callback); + composite_cdcacm_set_hooks(USBHID_CDCACM_HOOK_RX, my_rx_callback); #endif } -#endif // __STM32F1__ && HAS_SD_HOST_DRIVE +#endif // HAS_SD_HOST_DRIVE +#endif // __STM32F1__ diff --git a/Marlin/src/HAL/STM32F1/msc_sd.h b/Marlin/src/HAL/STM32F1/msc_sd.h index 151287f7a7e2..f4636bdff702 100644 --- a/Marlin/src/HAL/STM32F1/msc_sd.h +++ b/Marlin/src/HAL/STM32F1/msc_sd.h @@ -21,6 +21,6 @@ #include "../../core/serial_hook.h" extern USBMassStorage MarlinMSC; -extern Serial0Type MarlinCompositeSerial; +extern Serial1Class MarlinCompositeSerial; void MSC_SD_init(); diff --git a/Marlin/src/HAL/STM32F1/onboard_sd.cpp b/Marlin/src/HAL/STM32F1/onboard_sd.cpp index 6092aea32054..a3d8dcb2d57e 100644 --- a/Marlin/src/HAL/STM32F1/onboard_sd.cpp +++ b/Marlin/src/HAL/STM32F1/onboard_sd.cpp @@ -38,8 +38,13 @@ #define SPI_CLOCK_MAX SPI_BAUD_PCLK_DIV_2 #endif -#define CS_LOW() WRITE(ONBOARD_SD_CS_PIN, LOW) /* Set OnboardSPI cs low */ -#define CS_HIGH() WRITE(ONBOARD_SD_CS_PIN, HIGH) /* Set OnboardSPI cs high */ +#if PIN_EXISTS(ONBOARD_SD_CS) && ONBOARD_SD_CS_PIN != SD_SS_PIN + #define CS_LOW() WRITE(ONBOARD_SD_CS_PIN, LOW) // Set OnboardSPI cs low + #define CS_HIGH() WRITE(ONBOARD_SD_CS_PIN, HIGH) // Set OnboardSPI cs high +#else + #define CS_LOW() + #define CS_HIGH() +#endif #define FCLK_FAST() ONBOARD_SD_SPI.setClockDivider(SPI_CLOCK_MAX) #define FCLK_SLOW() ONBOARD_SD_SPI.setClockDivider(SPI_BAUD_PCLK_DIV_256) @@ -49,32 +54,32 @@ ---------------------------------------------------------------------------*/ /* MMC/SD command */ -#define CMD0 (0) /* GO_IDLE_STATE */ -#define CMD1 (1) /* SEND_OP_COND (MMC) */ -#define ACMD41 (0x80+41) /* SEND_OP_COND (SDC) */ -#define CMD8 (8) /* SEND_IF_COND */ -#define CMD9 (9) /* SEND_CSD */ -#define CMD10 (10) /* SEND_CID */ -#define CMD12 (12) /* STOP_TRANSMISSION */ -#define ACMD13 (0x80+13) /* SD_STATUS (SDC) */ -#define CMD16 (16) /* SET_BLOCKLEN */ -#define CMD17 (17) /* READ_SINGLE_BLOCK */ -#define CMD18 (18) /* READ_MULTIPLE_BLOCK */ -#define CMD23 (23) /* SET_BLOCK_COUNT (MMC) */ -#define ACMD23 (0x80+23) /* SET_WR_BLK_ERASE_COUNT (SDC) */ -#define CMD24 (24) /* WRITE_BLOCK */ -#define CMD25 (25) /* WRITE_MULTIPLE_BLOCK */ -#define CMD32 (32) /* ERASE_ER_BLK_START */ -#define CMD33 (33) /* ERASE_ER_BLK_END */ -#define CMD38 (38) /* ERASE */ -#define CMD48 (48) /* READ_EXTR_SINGLE */ -#define CMD49 (49) /* WRITE_EXTR_SINGLE */ -#define CMD55 (55) /* APP_CMD */ -#define CMD58 (58) /* READ_OCR */ - -static volatile DSTATUS Stat = STA_NOINIT; /* Physical drive status */ +#define CMD0 (0) // GO_IDLE_STATE +#define CMD1 (1) // SEND_OP_COND (MMC) +#define ACMD41 (0x80+41) // SEND_OP_COND (SDC) +#define CMD8 (8) // SEND_IF_COND +#define CMD9 (9) // SEND_CSD +#define CMD10 (10) // SEND_CID +#define CMD12 (12) // STOP_TRANSMISSION +#define ACMD13 (0x80+13) // SD_STATUS (SDC) +#define CMD16 (16) // SET_BLOCKLEN +#define CMD17 (17) // READ_SINGLE_BLOCK +#define CMD18 (18) // READ_MULTIPLE_BLOCK +#define CMD23 (23) // SET_BLOCK_COUNT (MMC) +#define ACMD23 (0x80+23) // SET_WR_BLK_ERASE_COUNT (SDC) +#define CMD24 (24) // WRITE_BLOCK +#define CMD25 (25) // WRITE_MULTIPLE_BLOCK +#define CMD32 (32) // ERASE_ER_BLK_START +#define CMD33 (33) // ERASE_ER_BLK_END +#define CMD38 (38) // ERASE +#define CMD48 (48) // READ_EXTR_SINGLE +#define CMD49 (49) // WRITE_EXTR_SINGLE +#define CMD55 (55) // APP_CMD +#define CMD58 (58) // READ_OCR + +static volatile DSTATUS Stat = STA_NOINIT; // Physical drive status static volatile UINT timeout; -static BYTE CardType; /* Card type flags */ +static BYTE CardType; // Card type flags /*-----------------------------------------------------------------------*/ /* Send/Receive data to the MMC (Platform dependent) */ @@ -82,7 +87,7 @@ static BYTE CardType; /* Card type flags */ /* Exchange a byte */ static BYTE xchg_spi ( - BYTE dat /* Data to send */ + BYTE dat // Data to send ) { BYTE returnByte = ONBOARD_SD_SPI.transfer(dat); return returnByte; @@ -90,18 +95,18 @@ static BYTE xchg_spi ( /* Receive multiple byte */ static void rcvr_spi_multi ( - BYTE *buff, /* Pointer to data buffer */ - UINT btr /* Number of bytes to receive (16, 64 or 512) */ + BYTE *buff, // Pointer to data buffer + UINT btr // Number of bytes to receive (16, 64 or 512) ) { ONBOARD_SD_SPI.dmaTransfer(0, const_cast(buff), btr); } #if _DISKIO_WRITE - /* Send multiple bytes */ + // Send multiple bytes static void xmit_spi_multi ( - const BYTE *buff, /* Pointer to the data */ - UINT btx /* Number of bytes to send (multiple of 16) */ + const BYTE *buff, // Pointer to the data + UINT btx // Number of bytes to send (multiple of 16) ) { ONBOARD_SD_SPI.dmaSend(const_cast(buff), btx); } @@ -112,16 +117,15 @@ static void rcvr_spi_multi ( /* Wait for card ready */ /*-----------------------------------------------------------------------*/ -static int wait_ready ( /* 1:Ready, 0:Timeout */ - UINT wt /* Timeout [ms] */ +static int wait_ready ( // 1:Ready, 0:Timeout + UINT wt // Timeout [ms] ) { BYTE d; - timeout = millis() + wt; do { d = xchg_spi(0xFF); - /* This loop takes a while. Insert rot_rdq() here for multitask environment. */ - } while (d != 0xFF && (timeout > millis())); /* Wait for card goes ready or timeout */ + // This loop takes a while. Insert rot_rdq() here for multitask environment. + } while (d != 0xFF && (timeout > millis())); // Wait for card goes ready or timeout return (d == 0xFF) ? 1 : 0; } @@ -131,21 +135,21 @@ static int wait_ready ( /* 1:Ready, 0:Timeout */ /*-----------------------------------------------------------------------*/ static void deselect() { - CS_HIGH(); /* CS = H */ - xchg_spi(0xFF); /* Dummy clock (force DO hi-z for multiple slave SPI) */ + CS_HIGH(); // CS = H + xchg_spi(0xFF); // Dummy clock (force DO hi-z for multiple slave SPI) } /*-----------------------------------------------------------------------*/ /* Select card and wait for ready */ /*-----------------------------------------------------------------------*/ -static int select() { /* 1:OK, 0:Timeout */ - CS_LOW(); /* CS = L */ - xchg_spi(0xFF); /* Dummy clock (force DO enabled) */ +static int select() { // 1:OK, 0:Timeout + CS_LOW(); // CS = L + xchg_spi(0xFF); // Dummy clock (force DO enabled) - if (wait_ready(500)) return 1; /* Leading busy check: Wait for card ready */ + if (wait_ready(500)) return 1; // Leading busy check: Wait for card ready - deselect(); /* Timeout */ + deselect(); // Timeout return 0; } @@ -153,16 +157,18 @@ static int select() { /* 1:OK, 0:Timeout */ /* Control SPI module (Platform dependent) */ /*-----------------------------------------------------------------------*/ -static void power_on() { /* Enable SSP module and attach it to I/O pads */ +// Enable SSP module and attach it to I/O pads +static void sd_power_on() { ONBOARD_SD_SPI.setModule(ONBOARD_SPI_DEVICE); ONBOARD_SD_SPI.begin(); ONBOARD_SD_SPI.setBitOrder(MSBFIRST); ONBOARD_SD_SPI.setDataMode(SPI_MODE0); - OUT_WRITE(ONBOARD_SD_CS_PIN, HIGH); /* Set CS# high */ + CS_HIGH(); } -static void power_off() { /* Disable SPI function */ - select(); /* Wait for card ready */ +// Disable SPI function +static void sd_power_off() { + select(); // Wait for card ready deselect(); } @@ -170,23 +176,23 @@ static void power_off() { /* Disable SPI function */ /* Receive a data packet from the MMC */ /*-----------------------------------------------------------------------*/ -static int rcvr_datablock ( /* 1:OK, 0:Error */ - BYTE *buff, /* Data buffer */ - UINT btr /* Data block length (byte) */ +static int rcvr_datablock ( // 1:OK, 0:Error + BYTE *buff, // Data buffer + UINT btr // Data block length (byte) ) { BYTE token; timeout = millis() + 200; - do { /* Wait for DataStart token in timeout of 200ms */ + do { // Wait for DataStart token in timeout of 200ms token = xchg_spi(0xFF); - /* This loop will take a while. Insert rot_rdq() here for multitask environment. */ + // This loop will take a while. Insert rot_rdq() here for multitask environment. } while ((token == 0xFF) && (timeout > millis())); - if (token != 0xFE) return 0; /* Function fails if invalid DataStart token or timeout */ + if (token != 0xFE) return 0; // Function fails if invalid DataStart token or timeout - rcvr_spi_multi(buff, btr); /* Store trailing data to the buffer */ - xchg_spi(0xFF); xchg_spi(0xFF); /* Discard CRC */ + rcvr_spi_multi(buff, btr); // Store trailing data to the buffer + xchg_spi(0xFF); xchg_spi(0xFF); // Discard CRC - return 1; /* Function succeeded */ + return 1; // Function succeeded } /*-----------------------------------------------------------------------*/ @@ -195,25 +201,25 @@ static int rcvr_datablock ( /* 1:OK, 0:Error */ #if _DISKIO_WRITE - static int xmit_datablock ( /* 1:OK, 0:Failed */ - const BYTE *buff, /* Ponter to 512 byte data to be sent */ - BYTE token /* Token */ + static int xmit_datablock( // 1:OK, 0:Failed + const BYTE *buff, // Pointer to 512 byte data to be sent + BYTE token // Token ) { BYTE resp; - if (!wait_ready(500)) return 0; /* Leading busy check: Wait for card ready to accept data block */ + if (!wait_ready(500)) return 0; // Leading busy check: Wait for card ready to accept data block - xchg_spi(token); /* Send token */ - if (token == 0xFD) return 1; /* Do not send data if token is StopTran */ + xchg_spi(token); // Send token + if (token == 0xFD) return 1; // Do not send data if token is StopTran - xmit_spi_multi(buff, 512); /* Data */ - xchg_spi(0xFF); xchg_spi(0xFF); /* Dummy CRC */ + xmit_spi_multi(buff, 512); // Data + xchg_spi(0xFF); xchg_spi(0xFF); // Dummy CRC - resp = xchg_spi(0xFF); /* Receive data resp */ + resp = xchg_spi(0xFF); // Receive data resp - return (resp & 0x1F) == 0x05 ? 1 : 0; /* Data was accepted or not */ + return (resp & 0x1F) == 0x05 ? 1 : 0; // Data was accepted or not - /* Busy check is done at next transmission */ + // Busy check is done at next transmission } #endif // _DISKIO_WRITE @@ -222,43 +228,43 @@ static int rcvr_datablock ( /* 1:OK, 0:Error */ /* Send a command packet to the MMC */ /*-----------------------------------------------------------------------*/ -static BYTE send_cmd ( /* Return value: R1 resp (bit7==1:Failed to send) */ - BYTE cmd, /* Command index */ - DWORD arg /* Argument */ +static BYTE send_cmd( // Return value: R1 resp (bit7==1:Failed to send) + BYTE cmd, // Command index + DWORD arg // Argument ) { BYTE n, res; - if (cmd & 0x80) { /* Send a CMD55 prior to ACMD */ + if (cmd & 0x80) { // Send a CMD55 prior to ACMD cmd &= 0x7F; res = send_cmd(CMD55, 0); if (res > 1) return res; } - /* Select the card and wait for ready except to stop multiple block read */ + // Select the card and wait for ready except to stop multiple block read if (cmd != CMD12) { deselect(); if (!select()) return 0xFF; } - /* Send command packet */ - xchg_spi(0x40 | cmd); /* Start + command index */ - xchg_spi((BYTE)(arg >> 24)); /* Argument[31..24] */ - xchg_spi((BYTE)(arg >> 16)); /* Argument[23..16] */ - xchg_spi((BYTE)(arg >> 8)); /* Argument[15..8] */ - xchg_spi((BYTE)arg); /* Argument[7..0] */ - n = 0x01; /* Dummy CRC + Stop */ - if (cmd == CMD0) n = 0x95; /* Valid CRC for CMD0(0) */ - if (cmd == CMD8) n = 0x87; /* Valid CRC for CMD8(0x1AA) */ + // Send command packet + xchg_spi(0x40 | cmd); // Start + command index + xchg_spi((BYTE)(arg >> 24)); // Argument[31..24] + xchg_spi((BYTE)(arg >> 16)); // Argument[23..16] + xchg_spi((BYTE)(arg >> 8)); // Argument[15..8] + xchg_spi((BYTE)arg); // Argument[7..0] + n = 0x01; // Dummy CRC + Stop + if (cmd == CMD0) n = 0x95; // Valid CRC for CMD0(0) + if (cmd == CMD8) n = 0x87; // Valid CRC for CMD8(0x1AA) xchg_spi(n); - /* Receive command resp */ - if (cmd == CMD12) xchg_spi(0xFF); /* Diacard following one byte when CMD12 */ - n = 10; /* Wait for response (10 bytes max) */ + // Receive command response + if (cmd == CMD12) xchg_spi(0xFF); // Discard the following byte when CMD12 + n = 10; // Wait for response (10 bytes max) do res = xchg_spi(0xFF); while ((res & 0x80) && --n); - return res; /* Return received response */ + return res; // Return received response } /*-------------------------------------------------------------------------- @@ -270,49 +276,52 @@ static BYTE send_cmd ( /* Return value: R1 resp (bit7==1:Failed to send) */ /*-----------------------------------------------------------------------*/ DSTATUS disk_initialize ( - BYTE drv /* Physical drive number (0) */ + BYTE drv // Physical drive number (0) ) { BYTE n, cmd, ty, ocr[4]; - if (drv) return STA_NOINIT; /* Supports only drive 0 */ - power_on(); /* Initialize SPI */ + if (drv) return STA_NOINIT; // Supports only drive 0 + sd_power_on(); // Initialize SPI - if (Stat & STA_NODISK) return Stat; /* Is a card existing in the soket? */ + if (Stat & STA_NODISK) return Stat; // Is a card existing in the socket? FCLK_SLOW(); - for (n = 10; n; n--) xchg_spi(0xFF); /* Send 80 dummy clocks */ + for (n = 10; n; n--) xchg_spi(0xFF); // Send 80 dummy clocks ty = 0; - if (send_cmd(CMD0, 0) == 1) { /* Put the card SPI state */ - timeout = millis() + 1000; /* Initialization timeout = 1 sec */ - if (send_cmd(CMD8, 0x1AA) == 1) { /* Is the catd SDv2? */ - for (n = 0; n < 4; n++) ocr[n] = xchg_spi(0xFF); /* Get 32 bit return value of R7 resp */ - if (ocr[2] == 0x01 && ocr[3] == 0xAA) { /* Does the card support 2.7-3.6V? */ - while ((timeout > millis()) && send_cmd(ACMD41, 1UL << 30)) ; /* Wait for end of initialization with ACMD41(HCS) */ - if ((timeout > millis()) && send_cmd(CMD58, 0) == 0) { /* Check CCS bit in the OCR */ + if (send_cmd(CMD0, 0) == 1) { // Put the card SPI state + timeout = millis() + 1000; // Initialization timeout = 1 sec + if (send_cmd(CMD8, 0x1AA) == 1) { // Is the catd SDv2? + for (n = 0; n < 4; n++) ocr[n] = xchg_spi(0xFF); // Get 32 bit return value of R7 resp + if (ocr[2] == 0x01 && ocr[3] == 0xAA) { // Does the card support 2.7-3.6V? + while ((timeout > millis()) && send_cmd(ACMD41, 1UL << 30)); // Wait for end of initialization with ACMD41(HCS) + if ((timeout > millis()) && send_cmd(CMD58, 0) == 0) { // Check CCS bit in the OCR for (n = 0; n < 4; n++) ocr[n] = xchg_spi(0xFF); - ty = (ocr[0] & 0x40) ? CT_SD2 | CT_BLOCK : CT_SD2; /* Check if the card is SDv2 */ + ty = (ocr[0] & 0x40) ? CT_SD2 | CT_BLOCK : CT_SD2; // Check if the card is SDv2 } } - } else { /* Not an SDv2 card */ - if (send_cmd(ACMD41, 0) <= 1) { /* SDv1 or MMCv3? */ - ty = CT_SD1; cmd = ACMD41; /* SDv1 (ACMD41(0)) */ - } else { - ty = CT_MMC; cmd = CMD1; /* MMCv3 (CMD1(0)) */ + } + else { // Not an SDv2 card + if (send_cmd(ACMD41, 0) <= 1) { // SDv1 or MMCv3? + ty = CT_SD1; cmd = ACMD41; // SDv1 (ACMD41(0)) + } + else { + ty = CT_MMC; cmd = CMD1; // MMCv3 (CMD1(0)) } - while ((timeout > millis()) && send_cmd(cmd, 0)) ; /* Wait for the card leaves idle state */ - if (!(timeout > millis()) || send_cmd(CMD16, 512) != 0) /* Set block length: 512 */ + while ((timeout > millis()) && send_cmd(cmd, 0)); // Wait for the card leaves idle state + if (!(timeout > millis()) || send_cmd(CMD16, 512) != 0) // Set block length: 512 ty = 0; } } - CardType = ty; /* Card type */ + CardType = ty; // Card type deselect(); - if (ty) { /* OK */ - FCLK_FAST(); /* Set fast clock */ - Stat &= ~STA_NOINIT; /* Clear STA_NOINIT flag */ - } else { /* Failed */ - power_off(); + if (ty) { // OK + FCLK_FAST(); // Set fast clock + Stat &= ~STA_NOINIT; // Clear STA_NOINIT flag + } + else { // Failed + sd_power_off(); Stat = STA_NOINIT; } @@ -324,10 +333,10 @@ DSTATUS disk_initialize ( /*-----------------------------------------------------------------------*/ DSTATUS disk_status ( - BYTE drv /* Physical drive number (0) */ + BYTE drv // Physical drive number (0) ) { - if (drv) return STA_NOINIT; /* Supports only drive 0 */ - return Stat; /* Return disk status */ + if (drv) return STA_NOINIT; // Supports only drive 0 + return Stat; // Return disk status } /*-----------------------------------------------------------------------*/ @@ -335,28 +344,28 @@ DSTATUS disk_status ( /*-----------------------------------------------------------------------*/ DRESULT disk_read ( - BYTE drv, /* Physical drive number (0) */ - BYTE *buff, /* Pointer to the data buffer to store read data */ - DWORD sector, /* Start sector number (LBA) */ - UINT count /* Number of sectors to read (1..128) */ + BYTE drv, // Physical drive number (0) + BYTE *buff, // Pointer to the data buffer to store read data + DWORD sector, // Start sector number (LBA) + UINT count // Number of sectors to read (1..128) ) { BYTE cmd; - if (drv || !count) return RES_PARERR; /* Check parameter */ - if (Stat & STA_NOINIT) return RES_NOTRDY; /* Check if drive is ready */ - if (!(CardType & CT_BLOCK)) sector *= 512; /* LBA ot BA conversion (byte addressing cards) */ + if (drv || !count) return RES_PARERR; // Check parameter + if (Stat & STA_NOINIT) return RES_NOTRDY; // Check if drive is ready + if (!(CardType & CT_BLOCK)) sector *= 512; // LBA ot BA conversion (byte addressing cards) FCLK_FAST(); - cmd = count > 1 ? CMD18 : CMD17; /* READ_MULTIPLE_BLOCK : READ_SINGLE_BLOCK */ + cmd = count > 1 ? CMD18 : CMD17; // READ_MULTIPLE_BLOCK : READ_SINGLE_BLOCK if (send_cmd(cmd, sector) == 0) { do { if (!rcvr_datablock(buff, 512)) break; buff += 512; } while (--count); - if (cmd == CMD18) send_cmd(CMD12, 0); /* STOP_TRANSMISSION */ + if (cmd == CMD18) send_cmd(CMD12, 0); // STOP_TRANSMISSION } deselect(); - return count ? RES_ERROR : RES_OK; /* Return result */ + return count ? RES_ERROR : RES_OK; // Return result } /*-----------------------------------------------------------------------*/ @@ -366,36 +375,36 @@ DRESULT disk_read ( #if _DISKIO_WRITE DRESULT disk_write( - BYTE drv, /* Physical drive number (0) */ - const BYTE *buff, /* Ponter to the data to write */ - DWORD sector, /* Start sector number (LBA) */ - UINT count /* Number of sectors to write (1..128) */ + BYTE drv, // Physical drive number (0) + const BYTE *buff, // Pointer to the data to write + DWORD sector, // Start sector number (LBA) + UINT count // Number of sectors to write (1..128) ) { - if (drv || !count) return RES_PARERR; /* Check parameter */ - if (Stat & STA_NOINIT) return RES_NOTRDY; /* Check drive status */ - if (Stat & STA_PROTECT) return RES_WRPRT; /* Check write protect */ + if (drv || !count) return RES_PARERR; // Check parameter + if (Stat & STA_NOINIT) return RES_NOTRDY; // Check drive status + if (Stat & STA_PROTECT) return RES_WRPRT; // Check write protect FCLK_FAST(); - if (!(CardType & CT_BLOCK)) sector *= 512; /* LBA ==> BA conversion (byte addressing cards) */ + if (!(CardType & CT_BLOCK)) sector *= 512; // LBA ==> BA conversion (byte addressing cards) - if (count == 1) { /* Single sector write */ - if ((send_cmd(CMD24, sector) == 0) /* WRITE_BLOCK */ + if (count == 1) { // Single sector write + if ((send_cmd(CMD24, sector) == 0) // WRITE_BLOCK && xmit_datablock(buff, 0xFE)) { count = 0; } } - else { /* Multiple sector write */ - if (CardType & CT_SDC) send_cmd(ACMD23, count); /* Predefine number of sectors */ - if (send_cmd(CMD25, sector) == 0) { /* WRITE_MULTIPLE_BLOCK */ + else { // Multiple sector write + if (CardType & CT_SDC) send_cmd(ACMD23, count); // Predefine number of sectors + if (send_cmd(CMD25, sector) == 0) { // WRITE_MULTIPLE_BLOCK do { if (!xmit_datablock(buff, 0xFC)) break; buff += 512; } while (--count); - if (!xmit_datablock(0, 0xFD)) count = 1; /* STOP_TRAN token */ + if (!xmit_datablock(0, 0xFD)) count = 1; // STOP_TRAN token } } deselect(); - return count ? RES_ERROR : RES_OK; /* Return result */ + return count ? RES_ERROR : RES_OK; // Return result } #endif // _DISKIO_WRITE @@ -407,9 +416,9 @@ DRESULT disk_read ( #if _DISKIO_IOCTL DRESULT disk_ioctl ( - BYTE drv, /* Physical drive number (0) */ - BYTE cmd, /* Control command code */ - void *buff /* Pointer to the conrtol data */ + BYTE drv, // Physical drive number (0) + BYTE cmd, // Control command code + void *buff // Pointer to the conrtol data ) { DRESULT res; BYTE n, csd[16], *ptr = (BYTE *)buff; @@ -420,22 +429,23 @@ DRESULT disk_read ( UINT dc; #endif - if (drv) return RES_PARERR; /* Check parameter */ - if (Stat & STA_NOINIT) return RES_NOTRDY; /* Check if drive is ready */ + if (drv) return RES_PARERR; // Check parameter + if (Stat & STA_NOINIT) return RES_NOTRDY; // Check if drive is ready res = RES_ERROR; FCLK_FAST(); switch (cmd) { - case CTRL_SYNC: /* Wait for end of internal write process of the drive */ + case CTRL_SYNC: // Wait for end of internal write process of the drive if (select()) res = RES_OK; break; - case GET_SECTOR_COUNT: /* Get drive capacity in unit of sector (DWORD) */ + case GET_SECTOR_COUNT: // Get drive capacity in unit of sector (DWORD) if ((send_cmd(CMD9, 0) == 0) && rcvr_datablock(csd, 16)) { - if ((csd[0] >> 6) == 1) { /* SDC ver 2.00 */ + if ((csd[0] >> 6) == 1) { // SDC ver 2.00 csize = csd[9] + ((WORD)csd[8] << 8) + ((DWORD)(csd[7] & 63) << 16) + 1; *(DWORD*)buff = csize << 10; - } else { /* SDC ver 1.XX or MMC ver 3 */ + } + else { // SDC ver 1.XX or MMC ver 3 n = (csd[5] & 15) + ((csd[10] & 128) >> 7) + ((csd[9] & 3) << 1) + 2; csize = (csd[8] >> 6) + ((WORD)csd[7] << 2) + ((WORD)(csd[6] & 3) << 10) + 1; *(DWORD*)buff = csize << (n - 9); @@ -444,21 +454,23 @@ DRESULT disk_read ( } break; - case GET_BLOCK_SIZE: /* Get erase block size in unit of sector (DWORD) */ - if (CardType & CT_SD2) { /* SDC ver 2.00 */ - if (send_cmd(ACMD13, 0) == 0) { /* Read SD status */ + case GET_BLOCK_SIZE: // Get erase block size in unit of sector (DWORD) + if (CardType & CT_SD2) { // SDC ver 2.00 + if (send_cmd(ACMD13, 0) == 0) { // Read SD status xchg_spi(0xFF); - if (rcvr_datablock(csd, 16)) { /* Read partial block */ - for (n = 64 - 16; n; n--) xchg_spi(0xFF); /* Purge trailing data */ + if (rcvr_datablock(csd, 16)) { // Read partial block + for (n = 64 - 16; n; n--) xchg_spi(0xFF); // Purge trailing data *(DWORD*)buff = 16UL << (csd[10] >> 4); res = RES_OK; } } - } else { /* SDC ver 1.XX or MMC */ - if ((send_cmd(CMD9, 0) == 0) && rcvr_datablock(csd, 16)) { /* Read CSD */ - if (CardType & CT_SD1) { /* SDC ver 1.XX */ + } + else { // SDC ver 1.XX or MMC + if ((send_cmd(CMD9, 0) == 0) && rcvr_datablock(csd, 16)) { // Read CSD + if (CardType & CT_SD1) { // SDC ver 1.XX *(DWORD*)buff = (((csd[10] & 63) << 1) + ((WORD)(csd[11] & 128) >> 7) + 1) << ((csd[13] >> 6) - 1); - } else { /* MMC */ + } + else { // MMC *(DWORD*)buff = ((WORD)((csd[10] & 124) >> 2) + 1) * (((csd[11] & 3) << 3) + ((csd[11] & 224) >> 5) + 1); } res = RES_OK; @@ -466,47 +478,47 @@ DRESULT disk_read ( } break; - case CTRL_TRIM: /* Erase a block of sectors (used when _USE_TRIM in ffconf.h is 1) */ - if (!(CardType & CT_SDC)) break; /* Check if the card is SDC */ - if (disk_ioctl(drv, MMC_GET_CSD, csd)) break; /* Get CSD */ - if (!(csd[0] >> 6) && !(csd[10] & 0x40)) break; /* Check if sector erase can be applied to the card */ - dp = (DWORD *)buff; st = dp[0]; ed = dp[1]; /* Load sector block */ + case CTRL_TRIM: // Erase a block of sectors (used when _USE_TRIM in ffconf.h is 1) + if (!(CardType & CT_SDC)) break; // Check if the card is SDC + if (disk_ioctl(drv, MMC_GET_CSD, csd)) break; // Get CSD + if (!(csd[0] >> 6) && !(csd[10] & 0x40)) break; // Check if sector erase can be applied to the card + dp = (DWORD *)buff; st = dp[0]; ed = dp[1]; // Load sector block if (!(CardType & CT_BLOCK)) { st *= 512; ed *= 512; } - if (send_cmd(CMD32, st) == 0 && send_cmd(CMD33, ed) == 0 && send_cmd(CMD38, 0) == 0 && wait_ready(30000)) { /* Erase sector block */ - res = RES_OK; /* FatFs does not check result of this command */ + if (send_cmd(CMD32, st) == 0 && send_cmd(CMD33, ed) == 0 && send_cmd(CMD38, 0) == 0 && wait_ready(30000)) { // Erase sector block + res = RES_OK; // FatFs does not check result of this command } break; - /* Following commands are never used by FatFs module */ + // The following commands are never used by FatFs module - case MMC_GET_TYPE: /* Get MMC/SDC type (BYTE) */ + case MMC_GET_TYPE: // Get MMC/SDC type (BYTE) *ptr = CardType; res = RES_OK; break; - case MMC_GET_CSD: /* Read CSD (16 bytes) */ - if (send_cmd(CMD9, 0) == 0 && rcvr_datablock(ptr, 16)) { /* READ_CSD */ + case MMC_GET_CSD: // Read CSD (16 bytes) + if (send_cmd(CMD9, 0) == 0 && rcvr_datablock(ptr, 16)) { res = RES_OK; } break; - case MMC_GET_CID: /* Read CID (16 bytes) */ - if (send_cmd(CMD10, 0) == 0 && rcvr_datablock(ptr, 16)) { /* READ_CID */ + case MMC_GET_CID: // Read CID (16 bytes) + if (send_cmd(CMD10, 0) == 0 && rcvr_datablock(ptr, 16)) { res = RES_OK; } break; - case MMC_GET_OCR: /* Read OCR (4 bytes) */ - if (send_cmd(CMD58, 0) == 0) { /* READ_OCR */ + case MMC_GET_OCR: // Read OCR (4 bytes) + if (send_cmd(CMD58, 0) == 0) { for (n = 4; n; n--) *ptr++ = xchg_spi(0xFF); res = RES_OK; } break; - case MMC_GET_SDSTAT: /* Read SD status (64 bytes) */ - if (send_cmd(ACMD13, 0) == 0) { /* SD_STATUS */ + case MMC_GET_SDSTAT: // Read SD status (64 bytes) + if (send_cmd(ACMD13, 0) == 0) { xchg_spi(0xFF); if (rcvr_datablock(ptr, 64)) res = RES_OK; } diff --git a/Marlin/src/HAL/STM32F1/onboard_sd.h b/Marlin/src/HAL/STM32F1/onboard_sd.h index 1dc7ec5b3bfa..f228d068c9db 100644 --- a/Marlin/src/HAL/STM32F1/onboard_sd.h +++ b/Marlin/src/HAL/STM32F1/onboard_sd.h @@ -7,8 +7,8 @@ #pragma once #define _DISKIO_WRITE 1 /* 1: Enable disk_write function */ -#define _DISKIO_IOCTL 1 /* 1: Enable disk_ioctl fucntion */ -#define _DISKIO_ISDIO 0 /* 1: Enable iSDIO control fucntion */ +#define _DISKIO_IOCTL 1 /* 1: Enable disk_ioctl function */ +#define _DISKIO_ISDIO 0 /* 1: Enable iSDIO control function */ typedef unsigned char BYTE; typedef unsigned short WORD; @@ -48,7 +48,7 @@ DRESULT disk_read(BYTE pdrv, BYTE* buff, DWORD sector, UINT count); DRESULT disk_write(BYTE pdrv, const BYTE* buff, DWORD sector, UINT count); #endif #if _DISKIO_IOCTL - DRESULT disk_ioctl(BYTE pdrv, BYTE cmd, void* buff); + DRESULT disk_ioctl(BYTE pdrv, BYTE cmd, void *buff); #endif /* Disk Status Bits (DSTATUS) */ @@ -56,7 +56,7 @@ DRESULT disk_read(BYTE pdrv, BYTE* buff, DWORD sector, UINT count); #define STA_NODISK 0x02 /* No medium in the drive */ #define STA_PROTECT 0x04 /* Write protected */ -/* Command code for disk_ioctrl fucntion */ +/* Command code for disk_ioctrl function */ /* Generic command (Used by FatFs) */ #define CTRL_SYNC 0 /* Complete pending write process (needed at _FS_READONLY == 0) */ diff --git a/Marlin/src/HAL/STM32F1/pinsDebug.h b/Marlin/src/HAL/STM32F1/pinsDebug.h index 8e7a3d8135fd..7828479658a9 100644 --- a/Marlin/src/HAL/STM32F1/pinsDebug.h +++ b/Marlin/src/HAL/STM32F1/pinsDebug.h @@ -2,6 +2,9 @@ * Marlin 3D Printer Firmware * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * * This program is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation, either version 3 of the License, or @@ -19,15 +22,15 @@ #pragma once /** - * Support routines for STM32GENERIC (Maple) + * Support routines for MAPLE_STM32F1 */ /** * Translation of routines & variables used by pinsDebug.h */ -#ifndef BOARD_NR_GPIO_PINS // Only in STM32GENERIC (Maple) - #error "Expected BOARD_NR_GPIO_PINS not found" +#ifndef BOARD_NR_GPIO_PINS // Only in MAPLE_STM32F1 + #error "Expected BOARD_NR_GPIO_PINS not found" #endif #include "fastio.h" @@ -41,6 +44,7 @@ extern const stm32_pin_info PIN_MAP[BOARD_NR_GPIO_PINS]; #define pwm_status(pin) PWM_PIN(pin) #define digitalRead_mod(p) extDigitalRead(p) #define PRINT_PIN(p) do{ sprintf_P(buffer, PSTR("%3hd "), int16_t(p)); SERIAL_ECHO(buffer); }while(0) +#define PRINT_PIN_ANALOG(p) do{ sprintf_P(buffer, PSTR(" (A%2d) "), DIGITAL_PIN_TO_ANALOG_PIN(pin)); SERIAL_ECHO(buffer); }while(0) #define PRINT_PORT(p) print_port(p) #define PRINT_ARRAY_NAME(x) do{ sprintf_P(buffer, PSTR("%-" STRINGIFY(MAX_NAME_LENGTH) "s"), pin_array[x].name); SERIAL_ECHO(buffer); }while(0) #define MULTI_NAME_PAD 21 // space needed to be pretty if not first name assigned to a pin @@ -50,11 +54,11 @@ extern const stm32_pin_info PIN_MAP[BOARD_NR_GPIO_PINS]; #define M43_NEVER_TOUCH(Q) (Q >= 9 && Q <= 12) // SERIAL/USB pins PA9(TX) PA10(RX) #endif -static inline int8_t get_pin_mode(pin_t pin) { +static int8_t get_pin_mode(pin_t pin) { return VALID_PIN(pin) ? _GET_MODE(pin) : -1; } -static inline pin_t DIGITAL_PIN_TO_ANALOG_PIN(pin_t pin) { +static pin_t DIGITAL_PIN_TO_ANALOG_PIN(pin_t pin) { if (!VALID_PIN(pin)) return -1; int8_t adc_channel = int8_t(PIN_MAP[pin].adc_channel); #ifdef NUM_ANALOG_INPUTS @@ -63,7 +67,7 @@ static inline pin_t DIGITAL_PIN_TO_ANALOG_PIN(pin_t pin) { return pin_t(adc_channel); } -static inline bool IS_ANALOG(pin_t pin) { +static bool IS_ANALOG(pin_t pin) { if (!VALID_PIN(pin)) return false; if (PIN_MAP[pin].adc_channel != ADCx) { #ifdef NUM_ANALOG_INPUTS @@ -74,11 +78,11 @@ static inline bool IS_ANALOG(pin_t pin) { return false; } -static inline bool GET_PINMODE(const pin_t pin) { +static bool GET_PINMODE(const pin_t pin) { return VALID_PIN(pin) && !IS_INPUT(pin); } -static inline bool GET_ARRAY_IS_DIGITAL(const int16_t array_pin) { +static bool GET_ARRAY_IS_DIGITAL(const int16_t array_pin) { const pin_t pin = GET_ARRAY_PIN(array_pin); return (!IS_ANALOG(pin) #ifdef NUM_ANALOG_INPUTS @@ -89,7 +93,7 @@ static inline bool GET_ARRAY_IS_DIGITAL(const int16_t array_pin) { #include "../../inc/MarlinConfig.h" // Allow pins/pins.h to set density -static inline void pwm_details(const pin_t pin) { +static void pwm_details(const pin_t pin) { if (PWM_PIN(pin)) { timer_dev * const tdev = PIN_MAP[pin].timer_device; const uint8_t channel = PIN_MAP[pin].timer_channel; @@ -109,7 +113,7 @@ static inline void pwm_details(const pin_t pin) { } } -static inline void print_port(pin_t pin) { +static void print_port(pin_t pin) { const char port = 'A' + char(pin >> 4); // pin div 16 const int16_t gbit = PIN_MAP[pin].gpio_bit; char buffer[8]; diff --git a/Marlin/src/HAL/STM32F1/sdio.cpp b/Marlin/src/HAL/STM32F1/sdio.cpp index ffa6db1206ae..6e41d2cbf1b4 100644 --- a/Marlin/src/HAL/STM32F1/sdio.cpp +++ b/Marlin/src/HAL/STM32F1/sdio.cpp @@ -184,6 +184,10 @@ bool SDIO_WriteBlock(uint32_t blockAddress, const uint8_t *data) { inline uint32_t SDIO_GetCardState() { return SDIO_CmdSendStatus(SdCard.RelCardAdd << 16U) ? (SDIO_GetResponse(SDIO_RESP1) >> 9U) & 0x0FU : SDIO_CARD_ERROR; } +// No F1 board with SDIO + MSC using Maple, that I aware of... +bool SDIO_IsReady() { return true; } +uint32_t SDIO_GetCardSize() { return 0; } + // ------------------------ // SD Commands and Responses // ------------------------ diff --git a/Marlin/src/HAL/STM32F1/spi_pins.h b/Marlin/src/HAL/STM32F1/spi_pins.h index 7d650ffe373f..3d3c8f8d2f18 100644 --- a/Marlin/src/HAL/STM32F1/spi_pins.h +++ b/Marlin/src/HAL/STM32F1/spi_pins.h @@ -2,6 +2,9 @@ * Marlin 3D Printer Firmware * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * * This program is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation, either version 3 of the License, or diff --git a/Marlin/src/HAL/STM32F1/tft/tft_spi.cpp b/Marlin/src/HAL/STM32F1/tft/tft_spi.cpp index 1095389946ad..9bf6bbb32bc7 100644 --- a/Marlin/src/HAL/STM32F1/tft/tft_spi.cpp +++ b/Marlin/src/HAL/STM32F1/tft/tft_spi.cpp @@ -26,36 +26,20 @@ #include "tft_spi.h" -// TFT_SPI tft; - SPIClass TFT_SPI::SPIx(1); -#define TFT_CS_H OUT_WRITE(TFT_CS_PIN, HIGH) -#define TFT_CS_L OUT_WRITE(TFT_CS_PIN, LOW) - -#define TFT_DC_H OUT_WRITE(TFT_DC_PIN, HIGH) -#define TFT_DC_L OUT_WRITE(TFT_DC_PIN, LOW) - -#define TFT_RST_H OUT_WRITE(TFT_RST_PIN, HIGH) -#define TFT_RST_L OUT_WRITE(TFT_RST_PIN, LOW) - -#define TFT_BLK_H OUT_WRITE(TFT_BACKLIGHT_PIN, HIGH) -#define TFT_BLK_L OUT_WRITE(TFT_BACKLIGHT_PIN, LOW) - void TFT_SPI::Init() { #if PIN_EXISTS(TFT_RESET) - // OUT_WRITE(TFT_RESET_PIN, HIGH); - TFT_RST_H; + OUT_WRITE(TFT_RST_PIN, HIGH); delay(100); #endif #if PIN_EXISTS(TFT_BACKLIGHT) - // OUT_WRITE(TFT_BACKLIGHT_PIN, HIGH); - TFT_BLK_H; + OUT_WRITE(TFT_BACKLIGHT_PIN, HIGH); #endif - TFT_DC_H; - TFT_CS_H; + OUT_WRITE(TFT_DC_PIN, HIGH); + OUT_WRITE(TFT_CS_PIN, HIGH); /** * STM32F1 APB2 = 72MHz, APB1 = 36MHz, max SPI speed of this MCU if 18Mhz @@ -87,15 +71,23 @@ void TFT_SPI::Init() { void TFT_SPI::DataTransferBegin(uint16_t DataSize) { SPIx.setDataSize(DataSize); SPIx.begin(); - TFT_CS_L; + OUT_WRITE(TFT_CS_PIN, LOW); } +#ifdef TFT_DEFAULT_DRIVER + #include "../../../lcd/tft_io/tft_ids.h" +#endif + uint32_t TFT_SPI::GetID() { uint32_t id; id = ReadID(LCD_READ_ID); - - if ((id & 0xFFFF) == 0 || (id & 0xFFFF) == 0xFFFF) + if ((id & 0xFFFF) == 0 || (id & 0xFFFF) == 0xFFFF) { id = ReadID(LCD_READ_ID4); + #ifdef TFT_DEFAULT_DRIVER + if ((id & 0xFFFF) == 0 || (id & 0xFFFF) == 0xFFFF) + id = TFT_DEFAULT_DRIVER; + #endif + } return id; } @@ -121,28 +113,16 @@ uint32_t TFT_SPI::ReadID(uint16_t Reg) { #endif } -bool TFT_SPI::isBusy() { - return false; -} +bool TFT_SPI::isBusy() { return false; } -void TFT_SPI::Abort() { - DataTransferEnd(); -} +void TFT_SPI::Abort() { DataTransferEnd(); } -void TFT_SPI::Transmit(uint16_t Data) { - SPIx.send(Data); -} +void TFT_SPI::Transmit(uint16_t Data) { SPIx.send(Data); } void TFT_SPI::TransmitDMA(uint32_t MemoryIncrease, uint16_t *Data, uint16_t Count) { DataTransferBegin(); - TFT_DC_H; - if (MemoryIncrease == DMA_MINC_ENABLE) { - SPIx.dmaSend(Data, Count, true); - } - else { - SPIx.dmaSend(Data, Count, false); - } - + OUT_WRITE(TFT_DC_PIN, HIGH); + SPIx.dmaSend(Data, Count, MemoryIncrease == DMA_MINC_ENABLE); DataTransferEnd(); } diff --git a/Marlin/src/HAL/STM32F1/tft/xpt2046.cpp b/Marlin/src/HAL/STM32F1/tft/xpt2046.cpp index 98371c5ffb85..ac9ad072aa05 100644 --- a/Marlin/src/HAL/STM32F1/tft/xpt2046.cpp +++ b/Marlin/src/HAL/STM32F1/tft/xpt2046.cpp @@ -22,7 +22,7 @@ #include "../../../inc/MarlinConfig.h" -#if HAS_TFT_XPT2046 || HAS_TOUCH_BUTTONS +#if HAS_TFT_XPT2046 || HAS_RES_TOUCH_BUTTONS #include "xpt2046.h" #include diff --git a/Marlin/src/HAL/STM32F1/tft/xpt2046.h b/Marlin/src/HAL/STM32F1/tft/xpt2046.h index 65602bda0f40..7c456cf00e1b 100644 --- a/Marlin/src/HAL/STM32F1/tft/xpt2046.h +++ b/Marlin/src/HAL/STM32F1/tft/xpt2046.h @@ -54,7 +54,7 @@ enum XPTCoordinate : uint8_t { XPT2046_Z2 = 0x40 | XPT2046_CONTROL | XPT2046_DFR_MODE, }; -#if !defined(XPT2046_Z1_THRESHOLD) +#ifndef XPT2046_Z1_THRESHOLD #define XPT2046_Z1_THRESHOLD 10 #endif @@ -65,8 +65,8 @@ class XPT2046 { static uint16_t getRawData(const XPTCoordinate coordinate); static bool isTouched(); - static inline void DataTransferBegin() { WRITE(TOUCH_CS_PIN, LOW); }; - static inline void DataTransferEnd() { WRITE(TOUCH_CS_PIN, HIGH); }; + static void DataTransferBegin() { WRITE(TOUCH_CS_PIN, LOW); }; + static void DataTransferEnd() { WRITE(TOUCH_CS_PIN, HIGH); }; #if ENABLED(TOUCH_BUTTONS_HW_SPI) static uint16_t HardwareIO(uint16_t data); #endif diff --git a/Marlin/src/HAL/STM32F1/timers.cpp b/Marlin/src/HAL/STM32F1/timers.cpp index 8c2df1e216e7..112c730b9acc 100644 --- a/Marlin/src/HAL/STM32F1/timers.cpp +++ b/Marlin/src/HAL/STM32F1/timers.cpp @@ -47,10 +47,7 @@ * TODO: Calculate Timer prescale value, so we get the 32bit to adjust */ - - - -void timer_set_interrupt_priority(uint_fast8_t timer_num, uint_fast8_t priority) { +void HAL_timer_set_interrupt_priority(uint_fast8_t timer_num, uint_fast8_t priority) { nvic_irq_num irq_num; switch (timer_num) { case 1: irq_num = NVIC_TIMER1_CC; break; @@ -73,7 +70,6 @@ void timer_set_interrupt_priority(uint_fast8_t timer_num, uint_fast8_t priority) nvic_irq_set_priority(irq_num, priority); } - void HAL_timer_start(const uint8_t timer_num, const uint32_t frequency) { /** * Give the Stepper ISR a higher priority (lower number) @@ -81,7 +77,7 @@ void HAL_timer_start(const uint8_t timer_num, const uint32_t frequency) { */ switch (timer_num) { - case STEP_TIMER_NUM: + case MF_TIMER_STEP: timer_pause(STEP_TIMER_DEV); timer_set_mode(STEP_TIMER_DEV, STEP_TIMER_CHAN, TIMER_OUTPUT_COMPARE); // counter timer_set_count(STEP_TIMER_DEV, 0); @@ -91,11 +87,11 @@ void HAL_timer_start(const uint8_t timer_num, const uint32_t frequency) { timer_set_compare(STEP_TIMER_DEV, STEP_TIMER_CHAN, _MIN(hal_timer_t(HAL_TIMER_TYPE_MAX), (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); - timer_set_interrupt_priority(STEP_TIMER_NUM, STEP_TIMER_IRQ_PRIO); + HAL_timer_set_interrupt_priority(MF_TIMER_STEP, STEP_TIMER_IRQ_PRIO); timer_generate_update(STEP_TIMER_DEV); timer_resume(STEP_TIMER_DEV); break; - case TEMP_TIMER_NUM: + case MF_TIMER_TEMP: timer_pause(TEMP_TIMER_DEV); timer_set_mode(TEMP_TIMER_DEV, TEMP_TIMER_CHAN, TIMER_OUTPUT_COMPARE); timer_set_count(TEMP_TIMER_DEV, 0); @@ -103,7 +99,7 @@ void HAL_timer_start(const uint8_t timer_num, const uint32_t frequency) { 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_attach_interrupt(TEMP_TIMER_DEV, TEMP_TIMER_CHAN, tempTC_Handler); - timer_set_interrupt_priority(TEMP_TIMER_NUM, TEMP_TIMER_IRQ_PRIO); + HAL_timer_set_interrupt_priority(MF_TIMER_TEMP, TEMP_TIMER_IRQ_PRIO); timer_generate_update(TEMP_TIMER_DEV); timer_resume(TEMP_TIMER_DEV); break; @@ -112,31 +108,31 @@ void HAL_timer_start(const uint8_t timer_num, const uint32_t frequency) { void HAL_timer_enable_interrupt(const uint8_t timer_num) { switch (timer_num) { - case STEP_TIMER_NUM: ENABLE_STEPPER_DRIVER_INTERRUPT(); break; - case TEMP_TIMER_NUM: ENABLE_TEMPERATURE_INTERRUPT(); break; + case MF_TIMER_STEP: ENABLE_STEPPER_DRIVER_INTERRUPT(); break; + case MF_TIMER_TEMP: ENABLE_TEMPERATURE_INTERRUPT(); break; } } void HAL_timer_disable_interrupt(const uint8_t timer_num) { switch (timer_num) { - case STEP_TIMER_NUM: DISABLE_STEPPER_DRIVER_INTERRUPT(); break; - case TEMP_TIMER_NUM: DISABLE_TEMPERATURE_INTERRUPT(); break; + case MF_TIMER_STEP: DISABLE_STEPPER_DRIVER_INTERRUPT(); break; + case MF_TIMER_TEMP: DISABLE_TEMPERATURE_INTERRUPT(); break; } } -static inline bool timer_irq_enabled(const timer_dev * const dev, const uint8_t interrupt) { +static inline bool HAL_timer_irq_enabled(const timer_dev * const dev, const uint8_t interrupt) { return bool(*bb_perip(&(dev->regs).gen->DIER, interrupt)); } bool HAL_timer_interrupt_enabled(const uint8_t timer_num) { switch (timer_num) { - case STEP_TIMER_NUM: return timer_irq_enabled(STEP_TIMER_DEV, STEP_TIMER_CHAN); - case TEMP_TIMER_NUM: return timer_irq_enabled(TEMP_TIMER_DEV, TEMP_TIMER_CHAN); + case MF_TIMER_STEP: return HAL_timer_irq_enabled(STEP_TIMER_DEV, STEP_TIMER_CHAN); + case MF_TIMER_TEMP: return HAL_timer_irq_enabled(TEMP_TIMER_DEV, TEMP_TIMER_CHAN); } return false; } -timer_dev* get_timer_dev(int number) { +timer_dev* HAL_get_timer_dev(int number) { switch (number) { #if STM32_HAVE_TIMER(1) case 1: return &timer1; diff --git a/Marlin/src/HAL/STM32F1/timers.h b/Marlin/src/HAL/STM32F1/timers.h index 3e2e7775f1e3..0cd807fc8479 100644 --- a/Marlin/src/HAL/STM32F1/timers.h +++ b/Marlin/src/HAL/STM32F1/timers.h @@ -25,9 +25,10 @@ * HAL for stm32duino.com based on Libmaple and compatible (STM32F1) */ -#include +#include "../../inc/MarlinConfig.h" +#include "HAL.h" + #include -#include "../../core/boards.h" // ------------------------ // Defines @@ -37,7 +38,6 @@ * TODO: Check and confirm what timer we will use for each Temps and stepper driving. * We should probable drive temps with PWM. */ -#define FORCE_INLINE __attribute__((always_inline)) inline typedef uint16_t hal_timer_t; #define HAL_TIMER_TYPE_MAX 0xFFFF @@ -65,30 +65,30 @@ typedef uint16_t hal_timer_t; * - Otherwise it uses Timer 8 on boards with STM32_HIGH_DENSITY * or Timer 4 on other boards. */ -#ifndef STEP_TIMER_NUM +#ifndef MF_TIMER_STEP #if defined(MCU_STM32F103CB) || defined(MCU_STM32F103C8) - #define STEP_TIMER_NUM 4 // For C8/CB boards, use timer 4 + #define MF_TIMER_STEP 4 // For C8/CB boards, use timer 4 #else - #define STEP_TIMER_NUM 5 // for other boards, five is fine. + #define MF_TIMER_STEP 5 // for other boards, five is fine. #endif #endif -#ifndef PULSE_TIMER_NUM - #define PULSE_TIMER_NUM STEP_TIMER_NUM +#ifndef MF_TIMER_PULSE + #define MF_TIMER_PULSE MF_TIMER_STEP #endif -#ifndef TEMP_TIMER_NUM - #define TEMP_TIMER_NUM 2 // Timer Index for Temperature - //#define TEMP_TIMER_NUM 4 // 2->4, Timer 2 for Stepper Current PWM +#ifndef MF_TIMER_TEMP + #define MF_TIMER_TEMP 2 // Timer Index for Temperature + //#define MF_TIMER_TEMP 4 // 2->4, Timer 2 for Stepper Current PWM #endif -#if MB(BTT_SKR_MINI_E3_V1_0, BTT_SKR_E3_DIP, BTT_SKR_MINI_E3_V1_2, MKS_ROBIN_LITE) +#if MB(BTT_SKR_MINI_E3_V1_0, BTT_SKR_E3_DIP, BTT_SKR_MINI_E3_V1_2, MKS_ROBIN_LITE, MKS_ROBIN_E3D, MKS_ROBIN_E3) // SKR Mini E3 boards use PA8 as FAN_PIN, so TIMER 1 is used for Fan PWM. #ifdef STM32_HIGH_DENSITY - #define SERVO0_TIMER_NUM 8 // tone.cpp uses Timer 4 + #define MF_TIMER_SERVO0 8 // tone.cpp uses Timer 4 #else - #define SERVO0_TIMER_NUM 3 // tone.cpp uses Timer 8 + #define MF_TIMER_SERVO0 3 // tone.cpp uses Timer 8 #endif #else - #define SERVO0_TIMER_NUM 1 // SERVO0 or BLTOUCH + #define MF_TIMER_SERVO0 1 // SERVO0 or BLTOUCH #endif #define STEP_TIMER_IRQ_PRIO 2 @@ -98,22 +98,22 @@ typedef uint16_t hal_timer_t; #define TEMP_TIMER_PRESCALE 1000 // prescaler for setting Temp timer, 72Khz #define TEMP_TIMER_FREQUENCY 1000 // temperature interrupt frequency -#define STEPPER_TIMER_PRESCALE 18 // prescaler for setting stepper timer, 4Mhz -#define STEPPER_TIMER_RATE (HAL_TIMER_RATE / STEPPER_TIMER_PRESCALE) // frequency of stepper timer -#define STEPPER_TIMER_TICKS_PER_US ((STEPPER_TIMER_RATE) / 1000000) // stepper timer ticks per µs +#define STEPPER_TIMER_PRESCALE 18 // prescaler for setting stepper timer, 4Mhz +#define STEPPER_TIMER_RATE (HAL_TIMER_RATE / STEPPER_TIMER_PRESCALE) // frequency of stepper timer +#define STEPPER_TIMER_TICKS_PER_US ((STEPPER_TIMER_RATE) / 1000000) // stepper timer ticks per µs -#define PULSE_TIMER_RATE STEPPER_TIMER_RATE // frequency of pulse timer -#define PULSE_TIMER_PRESCALE STEPPER_TIMER_PRESCALE -#define PULSE_TIMER_TICKS_PER_US STEPPER_TIMER_TICKS_PER_US +#define PULSE_TIMER_RATE STEPPER_TIMER_RATE // frequency of pulse timer +#define PULSE_TIMER_PRESCALE STEPPER_TIMER_PRESCALE +#define PULSE_TIMER_TICKS_PER_US STEPPER_TIMER_TICKS_PER_US -timer_dev* get_timer_dev(int number); -#define TIMER_DEV(num) get_timer_dev(num) -#define STEP_TIMER_DEV TIMER_DEV(STEP_TIMER_NUM) -#define TEMP_TIMER_DEV TIMER_DEV(TEMP_TIMER_NUM) +timer_dev* HAL_get_timer_dev(int number); +#define TIMER_DEV(num) HAL_get_timer_dev(num) +#define STEP_TIMER_DEV TIMER_DEV(MF_TIMER_STEP) +#define TEMP_TIMER_DEV TIMER_DEV(MF_TIMER_TEMP) #define ENABLE_STEPPER_DRIVER_INTERRUPT() timer_enable_irq(STEP_TIMER_DEV, STEP_TIMER_CHAN) #define DISABLE_STEPPER_DRIVER_INTERRUPT() timer_disable_irq(STEP_TIMER_DEV, STEP_TIMER_CHAN) -#define STEPPER_ISR_ENABLED() HAL_timer_interrupt_enabled(STEP_TIMER_NUM) +#define STEPPER_ISR_ENABLED() HAL_timer_interrupt_enabled(MF_TIMER_STEP) #define ENABLE_TEMPERATURE_INTERRUPT() timer_enable_irq(TEMP_TIMER_DEV, TEMP_TIMER_CHAN) #define DISABLE_TEMPERATURE_INTERRUPT() timer_disable_irq(TEMP_TIMER_DEV, TEMP_TIMER_CHAN) @@ -138,8 +138,8 @@ extern "C" { // Public Variables // ------------------------ -//static HardwareTimer StepperTimer(STEP_TIMER_NUM); -//static HardwareTimer TempTimer(TEMP_TIMER_NUM); +//static HardwareTimer StepperTimer(MF_TIMER_STEP); +//static HardwareTimer TempTimer(MF_TIMER_TEMP); // ------------------------ // Public functions @@ -163,13 +163,13 @@ bool HAL_timer_interrupt_enabled(const uint8_t timer_num); FORCE_INLINE static void HAL_timer_set_compare(const uint8_t timer_num, const hal_timer_t compare) { switch (timer_num) { - case STEP_TIMER_NUM: + case MF_TIMER_STEP: // NOTE: WE have set ARPE = 0, which means the Auto reload register is not preloaded // and there is no need to use any compare, as in the timer mode used, setting ARR to the compare value - // will result in exactly the same effect, ie trigerring an interrupt, and on top, set counter to 0 + // will result in exactly the same effect, ie triggering an interrupt, and on top, set counter to 0 timer_set_reload(STEP_TIMER_DEV, compare); // We reload direct ARR as needed during counting up break; - case TEMP_TIMER_NUM: + case MF_TIMER_TEMP: timer_set_compare(TEMP_TIMER_DEV, TEMP_TIMER_CHAN, compare); break; } @@ -177,18 +177,18 @@ FORCE_INLINE static void HAL_timer_set_compare(const uint8_t timer_num, const ha FORCE_INLINE static void HAL_timer_isr_prologue(const uint8_t timer_num) { switch (timer_num) { - case STEP_TIMER_NUM: - // No counter to clear - timer_generate_update(STEP_TIMER_DEV); - return; - case TEMP_TIMER_NUM: - timer_set_count(TEMP_TIMER_DEV, 0); - timer_generate_update(TEMP_TIMER_DEV); - return; + case MF_TIMER_STEP: + // No counter to clear + timer_generate_update(STEP_TIMER_DEV); + return; + case MF_TIMER_TEMP: + timer_set_count(TEMP_TIMER_DEV, 0); + timer_generate_update(TEMP_TIMER_DEV); + return; } } -#define HAL_timer_isr_epilogue(TIMER_NUM) +#define HAL_timer_isr_epilogue(T) NOOP // No command is available in framework to turn off ARPE bit, which is turned on by default in libmaple. // Needed here to reset ARPE=0 for stepper timer @@ -196,6 +196,6 @@ FORCE_INLINE static void timer_no_ARR_preload_ARPE(timer_dev *dev) { bb_peri_set_bit(&(dev->regs).gen->CR1, TIMER_CR1_ARPE_BIT, 0); } -void timer_set_interrupt_priority(uint_fast8_t timer_num, uint_fast8_t priority); +void HAL_timer_set_interrupt_priority(uint_fast8_t timer_num, uint_fast8_t priority); #define TIMER_OC_NO_PRELOAD 0 // Need to disable preload also on compare registers. diff --git a/Marlin/src/HAL/TEENSY31_32/HAL.cpp b/Marlin/src/HAL/TEENSY31_32/HAL.cpp index 51636d29bf14..b923ab77b1f1 100644 --- a/Marlin/src/HAL/TEENSY31_32/HAL.cpp +++ b/Marlin/src/HAL/TEENSY31_32/HAL.cpp @@ -31,36 +31,45 @@ #include -DefaultSerial MSerial(false); +// ------------------------ +// Serial ports +// ------------------------ + +#define _IMPLEMENT_SERIAL(X) DefaultSerial##X MSerial##X(false, Serial##X) +#define IMPLEMENT_SERIAL(X) _IMPLEMENT_SERIAL(X) +#if WITHIN(SERIAL_PORT, 0, 3) + IMPLEMENT_SERIAL(SERIAL_PORT); +#else + #error "SERIAL_PORT must be from 0 to 3." +#endif USBSerialType USBSerial(false, SerialUSB); -uint16_t HAL_adc_result; +// ------------------------ +// Class Utilities +// ------------------------ -static const uint8_t pin2sc1a[] = { - 5, 14, 8, 9, 13, 12, 6, 7, 15, 4, 0, 19, 3, 31, // 0-13, we treat them as A0-A13 - 5, 14, 8, 9, 13, 12, 6, 7, 15, 4, // 14-23 (A0-A9) - 31, 31, 31, 31, 31, 31, 31, 31, 31, 31, // 24-33 - 0+64, 19+64, 3+64, 31+64, // 34-37 (A10-A13) - 26, 22, 23, 27, 29, 30 // 38-43: temp. sensor, VREF_OUT, A14, bandgap, VREFH, VREFL. A14 isn't connected to anything in Teensy 3.0. -}; - -/* - // disable interrupts - void cli() { noInterrupts(); } - - // enable interrupts - void sei() { interrupts(); } -*/ +extern "C" { + extern char __bss_end; + extern char __heap_start; + extern void* __brkval; -void HAL_adc_init() { - analog_init(); - while (ADC0_SC3 & ADC_SC3_CAL) {}; // Wait for calibration to finish - NVIC_ENABLE_IRQ(IRQ_FTM1); + int freeMemory() { + int free_memory; + if ((int)__brkval == 0) + free_memory = ((int)&free_memory) - ((int)&__bss_end); + else + free_memory = ((int)&free_memory) - ((int)__brkval); + return free_memory; + } } -void HAL_clear_reset_source() { } +// ------------------------ +// MarlinHAL Class +// ------------------------ -uint8_t HAL_get_reset_source() { +void MarlinHAL::reboot() { _reboot_Teensyduino_(); } + +uint8_t MarlinHAL::get_reset_source() { switch (RCM_SRS0) { case 128: return RST_POWER_ON; break; case 64: return RST_EXTERNAL; break; @@ -72,23 +81,25 @@ uint8_t HAL_get_reset_source() { return 0; } -extern "C" { - extern char __bss_end; - extern char __heap_start; - extern void* __brkval; +// ADC - int freeMemory() { - int free_memory; - if ((int)__brkval == 0) - free_memory = ((int)&free_memory) - ((int)&__bss_end); - else - free_memory = ((int)&free_memory) - ((int)__brkval); - return free_memory; - } +void MarlinHAL::adc_init() { + analog_init(); + while (ADC0_SC3 & ADC_SC3_CAL) {}; // Wait for calibration to finish + NVIC_ENABLE_IRQ(IRQ_FTM1); } -void HAL_adc_start_conversion(const uint8_t adc_pin) { ADC0_SC1A = pin2sc1a[adc_pin]; } +void MarlinHAL::adc_start(const pin_t pin) { + static const uint8_t pin2sc1a[] = { + 5, 14, 8, 9, 13, 12, 6, 7, 15, 4, 0, 19, 3, 31, // 0-13, we treat them as A0-A13 + 5, 14, 8, 9, 13, 12, 6, 7, 15, 4, // 14-23 (A0-A9) + 31, 31, 31, 31, 31, 31, 31, 31, 31, 31, // 24-33 + 0+64, 19+64, 3+64, 31+64, // 34-37 (A10-A13) + 26, 22, 23, 27, 29, 30 // 38-43: temp. sensor, VREF_OUT, A14, bandgap, VREFH, VREFL. A14 isn't connected to anything in Teensy 3.0. + }; + ADC0_SC1A = pin2sc1a[pin]; +} -uint16_t HAL_adc_get_result() { return ADC0_RA; } +uint16_t MarlinHAL::adc_value() { return ADC0_RA; } #endif // __MK20DX256__ diff --git a/Marlin/src/HAL/TEENSY31_32/HAL.h b/Marlin/src/HAL/TEENSY31_32/HAL.h index 5273b3863774..50c0f411cf92 100644 --- a/Marlin/src/HAL/TEENSY31_32/HAL.h +++ b/Marlin/src/HAL/TEENSY31_32/HAL.h @@ -34,15 +34,11 @@ #include "fastio.h" #include "watchdog.h" - #include -#define ST7920_DELAY_1 DELAY_NS(600) -#define ST7920_DELAY_2 DELAY_NS(750) -#define ST7920_DELAY_3 DELAY_NS(750) - -//#undef MOTHERBOARD -//#define MOTHERBOARD BOARD_TEENSY31_32 +// ------------------------ +// Defines +// ------------------------ #define IS_32BIT_TEENSY 1 #define IS_TEENSY_31_32 1 @@ -50,74 +46,142 @@ #define IS_TEENSY32 1 #endif +#define CPU_ST7920_DELAY_1 600 +#define CPU_ST7920_DELAY_2 750 +#define CPU_ST7920_DELAY_3 750 + +// ------------------------ +// Serial ports +// ------------------------ + #include "../../core/serial_hook.h" -typedef Serial0Type DefaultSerial; -extern DefaultSerial MSerial; -typedef ForwardSerial0Type USBSerialType; + +#define Serial0 Serial +#define _DECLARE_SERIAL(X) \ + typedef ForwardSerial1Class DefaultSerial##X; \ + extern DefaultSerial##X MSerial##X +#define DECLARE_SERIAL(X) _DECLARE_SERIAL(X) + +typedef ForwardSerial1Class USBSerialType; extern USBSerialType USBSerial; #define _MSERIAL(X) MSerial##X #define MSERIAL(X) _MSERIAL(X) -#define MSerial0 MSerial #if SERIAL_PORT == -1 - #define MYSERIAL0 USBSerial + #define MYSERIAL1 USBSerial #elif WITHIN(SERIAL_PORT, 0, 3) - #define MYSERIAL0 MSERIAL(SERIAL_PORT) + DECLARE_SERIAL(SERIAL_PORT); + #define MYSERIAL1 MSERIAL(SERIAL_PORT) +#else + #error "The required SERIAL_PORT must be from 0 to 3, or -1 for Native USB." #endif -#define HAL_SERVO_LIB libServo +// ------------------------ +// Types +// ------------------------ + +class libServo; +typedef libServo hal_servo_t; typedef int8_t pin_t; -#ifndef analogInputToDigitalPin - #define analogInputToDigitalPin(p) ((p < 12U) ? (p) + 54U : -1) -#endif +// ------------------------ +// Interrupts +// ------------------------ -#define CRITICAL_SECTION_START() uint32_t primask = __get_PRIMASK(); __disable_irq() -#define CRITICAL_SECTION_END() if (!primask) __enable_irq() -#define ISRS_ENABLED() (!__get_PRIMASK()) -#define ENABLE_ISRS() __enable_irq() -#define DISABLE_ISRS() __disable_irq() +uint32_t __get_PRIMASK(void); // CMSIS +#define CRITICAL_SECTION_START() const bool irqon = !__get_PRIMASK(); __disable_irq() +#define CRITICAL_SECTION_END() if (irqon) __enable_irq() -inline void HAL_init() {} +// ------------------------ +// ADC +// ------------------------ -// Clear the reset reason -void HAL_clear_reset_source(); +#ifndef analogInputToDigitalPin + #define analogInputToDigitalPin(p) ((p < 12U) ? (p) + 54U : -1) +#endif -// Get the reason for the reset -uint8_t HAL_get_reset_source(); +#define HAL_ADC_VREF 3.3 +#define HAL_ADC_RESOLUTION 10 -inline void HAL_reboot() {} // reboot the board or restart the bootloader +// +// Pin Mapping for M42, M43, M226 +// +#define GET_PIN_MAP_PIN(index) index +#define GET_PIN_MAP_INDEX(pin) pin +#define PARSED_PIN_INDEX(code, dval) parser.intval(code, dval) -FORCE_INLINE void _delay_ms(const int delay_ms) { delay(delay_ms); } +// ------------------------ +// Class Utilities +// ------------------------ +#pragma GCC diagnostic push #if GCC_VERSION <= 50000 - #pragma GCC diagnostic push #pragma GCC diagnostic ignored "-Wunused-function" #endif extern "C" int freeMemory(); -#if GCC_VERSION <= 50000 - #pragma GCC diagnostic pop -#endif +#pragma GCC diagnostic pop -// ADC +// ------------------------ +// MarlinHAL Class +// ------------------------ -void HAL_adc_init(); +class MarlinHAL { +public: -#define HAL_ADC_VREF 3.3 -#define HAL_ADC_RESOLUTION 10 -#define HAL_START_ADC(pin) HAL_adc_start_conversion(pin) -#define HAL_READ_ADC() HAL_adc_get_result() -#define HAL_ADC_READY() true + // Earliest possible init, before setup() + MarlinHAL() {} -#define HAL_ANALOG_SELECT(pin) + static void init() {} // Called early in setup() + static void init_board() {} // Called less early in setup() + static void reboot(); // Restart the firmware from 0x0 -void HAL_adc_start_conversion(const uint8_t adc_pin); -uint16_t HAL_adc_get_result(); + // Interrupts + static bool isr_state() { return !__get_PRIMASK(); } + static void isr_on() { __enable_irq(); } + static void isr_off() { __disable_irq(); } -#define GET_PIN_MAP_PIN(index) index -#define GET_PIN_MAP_INDEX(pin) pin -#define PARSED_PIN_INDEX(code, dval) parser.intval(code, dval) + static void delay_ms(const int ms) { delay(ms); } + + // Tasks, called from idle() + static void idletask() {} + + // Reset + static uint8_t get_reset_source(); + static void clear_reset_source() {} + + // Free SRAM + static int freeMemory() { return ::freeMemory(); } + + // + // ADC Methods + // + + // Called by Temperature::init once at startup + static void adc_init(); + + // Called by Temperature::init for each sensor at startup + static void adc_enable(const pin_t ch) {} + + // Begin ADC sampling on the given channel + static void adc_start(const pin_t ch); + + // Is the ADC ready for reading? + static bool adc_ready() { return true; } + + // The current value of the ADC register + static uint16_t adc_value(); + + /** + * Set the PWM duty cycle for the pin to the given value. + * No option to invert the duty cycle [default = false] + * No option to change the scale of the provided value to enable finer PWM duty control [default = 255] + */ + static void set_pwm_duty(const pin_t pin, const uint16_t v, const uint16_t=255, const bool=false) { + analogWrite(pin, v); + } + +}; diff --git a/Marlin/src/HAL/TEENSY31_32/HAL_SPI.cpp b/Marlin/src/HAL/TEENSY31_32/HAL_SPI.cpp index dce236ef6bd0..415c69222986 100644 --- a/Marlin/src/HAL/TEENSY31_32/HAL_SPI.cpp +++ b/Marlin/src/HAL/TEENSY31_32/HAL_SPI.cpp @@ -21,11 +21,12 @@ */ #ifdef __MK20DX256__ +#include "../../inc/MarlinConfig.h" #include "HAL.h" + #include #include #include "spi_pins.h" -#include "../../core/macros.h" static SPISettings spiConfig; @@ -35,10 +36,9 @@ static SPISettings spiConfig; // Initialize SPI bus void spiBegin() { - #if !PIN_EXISTS(SD_SS) - #error "SD_SS_PIN not defined!" + #if PIN_EXISTS(SD_SS) + OUT_WRITE(SD_SS_PIN, HIGH); #endif - OUT_WRITE(SD_SS_PIN, HIGH); SET_OUTPUT(SD_SCK_PIN); SET_INPUT(SD_MISO_PIN); SET_OUTPUT(SD_MOSI_PIN); @@ -64,7 +64,7 @@ void spiInit(uint8_t spiRate) { case SPI_EIGHTH_SPEED: clock = 1250000; break; case SPI_SPEED_5: clock = 625000; break; case SPI_SPEED_6: clock = 312500; break; - default: clock = 4000000; // Default from the SPI libarary + default: clock = 4000000; // Default from the SPI library } spiConfig = SPISettings(clock, MSBFIRST, SPI_MODE0); SPI.begin(); @@ -82,7 +82,7 @@ uint8_t spiRec() { } // SPI read data -void spiRead(uint8_t* buf, uint16_t nbyte) { +void spiRead(uint8_t *buf, uint16_t nbyte) { SPI.beginTransaction(spiConfig); SPI.transfer(buf, nbyte); SPI.endTransaction(); @@ -107,7 +107,7 @@ void spiSend(uint8_t b) { } // SPI send block -void spiSendBlock(uint8_t token, const uint8_t* buf) { +void spiSendBlock(uint8_t token, const uint8_t *buf) { SPI.beginTransaction(spiConfig); SPDR = token; for (uint16_t i = 0; i < 512; i += 2) { diff --git a/Marlin/src/HAL/TEENSY31_32/MarlinSPI.h b/Marlin/src/HAL/TEENSY31_32/MarlinSPI.h new file mode 100644 index 000000000000..0c447ba4cb3d --- /dev/null +++ b/Marlin/src/HAL/TEENSY31_32/MarlinSPI.h @@ -0,0 +1,26 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2021 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + * + */ +#pragma once + +#include + +using MarlinSPI = SPIClass; diff --git a/Marlin/src/HAL/TEENSY31_32/eeprom.cpp b/Marlin/src/HAL/TEENSY31_32/eeprom.cpp index f2ae5dd534ec..d1ff9408229f 100644 --- a/Marlin/src/HAL/TEENSY31_32/eeprom.cpp +++ b/Marlin/src/HAL/TEENSY31_32/eeprom.cpp @@ -2,6 +2,9 @@ * Marlin 3D Printer Firmware * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * * This program is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation, either version 3 of the License, or @@ -16,16 +19,17 @@ * along with this program. If not, see . * */ -#ifdef __MK20DX256__ -#include "../../inc/MarlinConfig.h" - -#if USE_WIRED_EEPROM +#ifdef __MK20DX256__ /** * HAL PersistentStore for Teensy 3.2 (MK20DX256) */ +#include "../../inc/MarlinConfig.h" + +#if USE_WIRED_EEPROM + #include "../shared/eeprom_api.h" #include @@ -38,13 +42,13 @@ bool PersistentStore::access_start() { return true; } bool PersistentStore::access_finish() { return true; } bool PersistentStore::write_data(int &pos, const uint8_t *value, size_t size, uint16_t *crc) { + uint16_t written = 0; while (size--) { uint8_t * const p = (uint8_t * const)pos; uint8_t v = *value; - // EEPROM has only ~100,000 write cycles, - // so only write bytes that have changed! - if (v != eeprom_read_byte(p)) { + if (v != eeprom_read_byte(p)) { // EEPROM has only ~100,000 write cycles, so only write bytes that have changed! eeprom_write_byte(p, v); + if (++written & 0x7F) delay(2); else safe_delay(2); // Avoid triggering watchdog during long EEPROM writes if (eeprom_read_byte(p) != v) { SERIAL_ECHO_MSG(STR_ERR_EEPROM_WRITE); return true; diff --git a/Marlin/src/HAL/TEENSY31_32/endstop_interrupts.h b/Marlin/src/HAL/TEENSY31_32/endstop_interrupts.h index 999ada512761..9c7e2104882e 100644 --- a/Marlin/src/HAL/TEENSY31_32/endstop_interrupts.h +++ b/Marlin/src/HAL/TEENSY31_32/endstop_interrupts.h @@ -64,4 +64,10 @@ void setup_endstop_interrupts() { TERN_(HAS_Z4_MAX, _ATTACH(Z4_MAX_PIN)); TERN_(HAS_Z4_MIN, _ATTACH(Z4_MIN_PIN)); TERN_(HAS_Z_MIN_PROBE_PIN, _ATTACH(Z_MIN_PROBE_PIN)); + TERN_(HAS_I_MAX, _ATTACH(I_MAX_PIN)); + TERN_(HAS_I_MIN, _ATTACH(I_MIN_PIN)); + TERN_(HAS_J_MAX, _ATTACH(J_MAX_PIN)); + TERN_(HAS_J_MIN, _ATTACH(J_MIN_PIN)); + TERN_(HAS_K_MAX, _ATTACH(K_MAX_PIN)); + TERN_(HAS_K_MIN, _ATTACH(K_MIN_PIN)); } diff --git a/Marlin/src/HAL/TEENSY31_32/timers.cpp b/Marlin/src/HAL/TEENSY31_32/timers.cpp index 7e01a38f8946..f217715a3fed 100644 --- a/Marlin/src/HAL/TEENSY31_32/timers.cpp +++ b/Marlin/src/HAL/TEENSY31_32/timers.cpp @@ -47,7 +47,7 @@ FORCE_INLINE static void __DSB() { void HAL_timer_start(const uint8_t timer_num, const uint32_t frequency) { switch (timer_num) { - case 0: + case MF_TIMER_STEP: FTM0_MODE = FTM_MODE_WPDIS | FTM_MODE_FTMEN; FTM0_SC = 0x00; // Set this to zero before changing the modulus FTM0_CNT = 0x0000; // Reset the count to zero @@ -56,7 +56,7 @@ void HAL_timer_start(const uint8_t timer_num, const uint32_t frequency) { FTM0_SC = (FTM_SC_CLKS(0b1) & FTM_SC_CLKS_MASK) | (FTM_SC_PS(FTM0_TIMER_PRESCALE_BITS) & FTM_SC_PS_MASK); // Bus clock 60MHz divided by prescaler 8 FTM0_C0SC = FTM_CSC_CHIE | FTM_CSC_MSA | FTM_CSC_ELSA; break; - case 1: + case MF_TIMER_TEMP: FTM1_MODE = FTM_MODE_WPDIS | FTM_MODE_FTMEN; // Disable write protection, Enable FTM1 FTM1_SC = 0x00; // Set this to zero before changing the modulus FTM1_CNT = 0x0000; // Reset the count to zero @@ -70,15 +70,15 @@ void HAL_timer_start(const uint8_t timer_num, const uint32_t frequency) { void HAL_timer_enable_interrupt(const uint8_t timer_num) { switch (timer_num) { - case 0: NVIC_ENABLE_IRQ(IRQ_FTM0); break; - case 1: NVIC_ENABLE_IRQ(IRQ_FTM1); break; + case MF_TIMER_STEP: NVIC_ENABLE_IRQ(IRQ_FTM0); break; + case MF_TIMER_TEMP: NVIC_ENABLE_IRQ(IRQ_FTM1); break; } } void HAL_timer_disable_interrupt(const uint8_t timer_num) { switch (timer_num) { - case 0: NVIC_DISABLE_IRQ(IRQ_FTM0); break; - case 1: NVIC_DISABLE_IRQ(IRQ_FTM1); break; + case MF_TIMER_STEP: NVIC_DISABLE_IRQ(IRQ_FTM0); break; + case MF_TIMER_TEMP: NVIC_DISABLE_IRQ(IRQ_FTM1); break; } // We NEED memory barriers to ensure Interrupts are actually disabled! @@ -89,20 +89,20 @@ void HAL_timer_disable_interrupt(const uint8_t timer_num) { bool HAL_timer_interrupt_enabled(const uint8_t timer_num) { switch (timer_num) { - case 0: return NVIC_IS_ENABLED(IRQ_FTM0); - case 1: return NVIC_IS_ENABLED(IRQ_FTM1); + case MF_TIMER_STEP: return NVIC_IS_ENABLED(IRQ_FTM0); + case MF_TIMER_TEMP: return NVIC_IS_ENABLED(IRQ_FTM1); } return false; } void HAL_timer_isr_prologue(const uint8_t timer_num) { switch (timer_num) { - case 0: + case MF_TIMER_STEP: FTM0_CNT = 0x0000; FTM0_SC &= ~FTM_SC_TOF; // Clear FTM Overflow flag FTM0_C0SC &= ~FTM_CSC_CHF; // Clear FTM Channel Compare flag break; - case 1: + case MF_TIMER_TEMP: FTM1_CNT = 0x0000; FTM1_SC &= ~FTM_SC_TOF; // Clear FTM Overflow flag FTM1_C0SC &= ~FTM_CSC_CHF; // Clear FTM Channel Compare flag diff --git a/Marlin/src/HAL/TEENSY31_32/timers.h b/Marlin/src/HAL/TEENSY31_32/timers.h index 61b86735967e..9fcbb6f232c9 100644 --- a/Marlin/src/HAL/TEENSY31_32/timers.h +++ b/Marlin/src/HAL/TEENSY31_32/timers.h @@ -46,14 +46,14 @@ typedef uint32_t hal_timer_t; #define HAL_TIMER_RATE (FTM0_TIMER_RATE) -#ifndef STEP_TIMER_NUM - #define STEP_TIMER_NUM 0 // Timer Index for Stepper +#ifndef MF_TIMER_STEP + #define MF_TIMER_STEP 0 // Timer Index for Stepper #endif -#ifndef PULSE_TIMER_NUM - #define PULSE_TIMER_NUM STEP_TIMER_NUM +#ifndef MF_TIMER_PULSE + #define MF_TIMER_PULSE MF_TIMER_STEP #endif -#ifndef TEMP_TIMER_NUM - #define TEMP_TIMER_NUM 1 // Timer Index for Temperature +#ifndef MF_TIMER_TEMP + #define MF_TIMER_TEMP 1 // Timer Index for Temperature #endif #define TEMP_TIMER_FREQUENCY 1000 @@ -66,12 +66,12 @@ typedef uint32_t hal_timer_t; #define PULSE_TIMER_PRESCALE STEPPER_TIMER_PRESCALE #define PULSE_TIMER_TICKS_PER_US STEPPER_TIMER_TICKS_PER_US -#define ENABLE_STEPPER_DRIVER_INTERRUPT() HAL_timer_enable_interrupt(STEP_TIMER_NUM) -#define DISABLE_STEPPER_DRIVER_INTERRUPT() HAL_timer_disable_interrupt(STEP_TIMER_NUM) -#define STEPPER_ISR_ENABLED() HAL_timer_interrupt_enabled(STEP_TIMER_NUM) +#define ENABLE_STEPPER_DRIVER_INTERRUPT() HAL_timer_enable_interrupt(MF_TIMER_STEP) +#define DISABLE_STEPPER_DRIVER_INTERRUPT() HAL_timer_disable_interrupt(MF_TIMER_STEP) +#define STEPPER_ISR_ENABLED() HAL_timer_interrupt_enabled(MF_TIMER_STEP) -#define ENABLE_TEMPERATURE_INTERRUPT() HAL_timer_enable_interrupt(TEMP_TIMER_NUM) -#define DISABLE_TEMPERATURE_INTERRUPT() HAL_timer_disable_interrupt(TEMP_TIMER_NUM) +#define ENABLE_TEMPERATURE_INTERRUPT() HAL_timer_enable_interrupt(MF_TIMER_TEMP) +#define DISABLE_TEMPERATURE_INTERRUPT() HAL_timer_disable_interrupt(MF_TIMER_TEMP) #ifndef HAL_STEP_TIMER_ISR #define HAL_STEP_TIMER_ISR() extern "C" void ftm0_isr() //void TC3_Handler() @@ -84,23 +84,23 @@ void HAL_timer_start(const uint8_t timer_num, const uint32_t frequency); FORCE_INLINE static void HAL_timer_set_compare(const uint8_t timer_num, const hal_timer_t compare) { switch (timer_num) { - case 0: FTM0_C0V = compare; break; - case 1: FTM1_C0V = compare; break; + case MF_TIMER_STEP: FTM0_C0V = compare; break; + case MF_TIMER_TEMP: FTM1_C0V = compare; break; } } FORCE_INLINE static hal_timer_t HAL_timer_get_compare(const uint8_t timer_num) { switch (timer_num) { - case 0: return FTM0_C0V; - case 1: return FTM1_C0V; + case MF_TIMER_STEP: return FTM0_C0V; + case MF_TIMER_TEMP: return FTM1_C0V; } return 0; } FORCE_INLINE static hal_timer_t HAL_timer_get_count(const uint8_t timer_num) { switch (timer_num) { - case 0: return FTM0_CNT; - case 1: return FTM1_CNT; + case MF_TIMER_STEP: return FTM0_CNT; + case MF_TIMER_TEMP: return FTM1_CNT; } return 0; } @@ -110,4 +110,4 @@ void HAL_timer_disable_interrupt(const uint8_t timer_num); bool HAL_timer_interrupt_enabled(const uint8_t timer_num); void HAL_timer_isr_prologue(const uint8_t timer_num); -#define HAL_timer_isr_epilogue(TIMER_NUM) +#define HAL_timer_isr_epilogue(T) NOOP diff --git a/Marlin/src/HAL/TEENSY35_36/HAL.cpp b/Marlin/src/HAL/TEENSY35_36/HAL.cpp index 547681de5f12..54a5ad385536 100644 --- a/Marlin/src/HAL/TEENSY35_36/HAL.cpp +++ b/Marlin/src/HAL/TEENSY35_36/HAL.cpp @@ -31,55 +31,21 @@ #include -DefaultSerial MSerial(false); -USBSerialType USBSerial(false, SerialUSB); +// ------------------------ +// Serial ports +// ------------------------ -uint16_t HAL_adc_result, HAL_adc_select; - -static const uint8_t pin2sc1a[] = { - 5, 14, 8, 9, 13, 12, 6, 7, 15, 4, 3, 19+128, 14+128, 15+128, // 0-13 -> A0-A13 - 5, 14, 8, 9, 13, 12, 6, 7, 15, 4, // 14-23 are A0-A9 - 255, 255, 255, 255, 255, 255, 255, // 24-30 are digital only - 14+128, 15+128, 17, 18, 4+128, 5+128, 6+128, 7+128, 17+128, // 31-39 are A12-A20 - 255, 255, 255, 255, 255, 255, 255, 255, 255, // 40-48 are digital only - 10+128, 11+128, // 49-50 are A23-A24 - 255, 255, 255, 255, 255, 255, 255, // 51-57 are digital only - 255, 255, 255, 255, 255, 255, // 58-63 (sd card pins) are digital only - 3, 19+128, // 64-65 are A10-A11 - 23, 23+128,// 66-67 are A21-A22 (DAC pins) - 1, 1+128, // 68-69 are A25-A26 (unused USB host port on Teensy 3.5) - 26, // 70 is Temperature Sensor - 18+128 // 71 is Vref -}; - -/* - // disable interrupts - void cli() { noInterrupts(); } - - // enable interrupts - void sei() { interrupts(); } -*/ - -void HAL_adc_init() { - analog_init(); - while (ADC0_SC3 & ADC_SC3_CAL) {}; // Wait for calibration to finish - while (ADC1_SC3 & ADC_SC3_CAL) {}; // Wait for calibration to finish - NVIC_ENABLE_IRQ(IRQ_FTM1); -} +#define _IMPLEMENT_SERIAL(X) DefaultSerial##X MSerial##X(false, Serial##X) +#define IMPLEMENT_SERIAL(X) _IMPLEMENT_SERIAL(X) +#if WITHIN(SERIAL_PORT, 0, 3) + IMPLEMENT_SERIAL(SERIAL_PORT); +#endif -void HAL_clear_reset_source() { } +USBSerialType USBSerial(false, SerialUSB); -uint8_t HAL_get_reset_source() { - switch (RCM_SRS0) { - case 128: return RST_POWER_ON; break; - case 64: return RST_EXTERNAL; break; - case 32: return RST_WATCHDOG; break; - // case 8: return RST_LOSS_OF_LOCK; break; - // case 4: return RST_LOSS_OF_CLOCK; break; - // case 2: return RST_LOW_VOLTAGE; break; - } - return 0; -} +// ------------------------ +// Class Utilities +// ------------------------ extern "C" { extern char __bss_end; @@ -96,24 +62,69 @@ extern "C" { } } -void HAL_adc_start_conversion(const uint8_t adc_pin) { +// ------------------------ +// MarlinHAL Class +// ------------------------ + +void MarlinHAL::reboot() { _reboot_Teensyduino_(); } + +// Reset + +uint8_t MarlinHAL::get_reset_source() { + switch (RCM_SRS0) { + case 128: return RST_POWER_ON; break; + case 64: return RST_EXTERNAL; break; + case 32: return RST_WATCHDOG; break; + // case 8: return RST_LOSS_OF_LOCK; break; + // case 4: return RST_LOSS_OF_CLOCK; break; + // case 2: return RST_LOW_VOLTAGE; break; + } + return 0; +} + +// ADC + +int8_t MarlinHAL::adc_select; + +void MarlinHAL::adc_init() { + analog_init(); + while (ADC0_SC3 & ADC_SC3_CAL) { /* Wait for calibration to finish */ } + while (ADC1_SC3 & ADC_SC3_CAL) { /* Wait for calibration to finish */ } + NVIC_ENABLE_IRQ(IRQ_FTM1); +} + +void MarlinHAL::adc_start(const pin_t adc_pin) { + static const uint8_t pin2sc1a[] = { + 5, 14, 8, 9, 13, 12, 6, 7, 15, 4, 3, 19+128, 14+128, 15+128, // 0-13 -> A0-A13 + 5, 14, 8, 9, 13, 12, 6, 7, 15, 4, // 14-23 are A0-A9 + 255, 255, 255, 255, 255, 255, 255, // 24-30 are digital only + 14+128, 15+128, 17, 18, 4+128, 5+128, 6+128, 7+128, 17+128, // 31-39 are A12-A20 + 255, 255, 255, 255, 255, 255, 255, 255, 255, // 40-48 are digital only + 10+128, 11+128, // 49-50 are A23-A24 + 255, 255, 255, 255, 255, 255, 255, // 51-57 are digital only + 255, 255, 255, 255, 255, 255, // 58-63 (sd card pins) are digital only + 3, 19+128, // 64-65 are A10-A11 + 23, 23+128,// 66-67 are A21-A22 (DAC pins) + 1, 1+128, // 68-69 are A25-A26 (unused USB host port on Teensy 3.5) + 26, // 70 is Temperature Sensor + 18+128 // 71 is Vref + }; const uint16_t pin = pin2sc1a[adc_pin]; if (pin == 0xFF) { - // Digital only - HAL_adc_select = -1; + adc_select = -1; // Digital only } else if (pin & 0x80) { - HAL_adc_select = 1; + adc_select = 1; ADC1_SC1A = pin & 0x7F; } else { - HAL_adc_select = 0; + adc_select = 0; ADC0_SC1A = pin; } } -uint16_t HAL_adc_get_result() { - switch (HAL_adc_select) { +uint16_t MarlinHAL::adc_value() { + switch (adc_select) { case 0: return ADC0_RA; case 1: return ADC1_RA; } diff --git a/Marlin/src/HAL/TEENSY35_36/HAL.h b/Marlin/src/HAL/TEENSY35_36/HAL.h index 94c514bf6275..e4c57f8d1e10 100644 --- a/Marlin/src/HAL/TEENSY35_36/HAL.h +++ b/Marlin/src/HAL/TEENSY35_36/HAL.h @@ -37,10 +37,6 @@ #include #include -#define ST7920_DELAY_1 DELAY_NS(600) -#define ST7920_DELAY_2 DELAY_NS(750) -#define ST7920_DELAY_3 DELAY_NS(750) - // ------------------------ // Defines // ------------------------ @@ -53,77 +49,146 @@ #define IS_TEENSY35 1 #endif +#define CPU_ST7920_DELAY_1 600 +#define CPU_ST7920_DELAY_2 750 +#define CPU_ST7920_DELAY_3 750 + +#undef sq +#define sq(x) ((x)*(x)) + +// ------------------------ +// Serial ports +// ------------------------ + #include "../../core/serial_hook.h" -typedef Serial0Type DefaultSerial; -extern DefaultSerial MSerial; -typedef ForwardSerial0Type USBSerialType; + +#define Serial0 Serial +#define _DECLARE_SERIAL(X) \ + typedef ForwardSerial1Class DefaultSerial##X; \ + extern DefaultSerial##X MSerial##X +#define DECLARE_SERIAL(X) _DECLARE_SERIAL(X) + +typedef ForwardSerial1Class USBSerialType; extern USBSerialType USBSerial; #define _MSERIAL(X) MSerial##X #define MSERIAL(X) _MSERIAL(X) -#define MSerial0 MSerial #if SERIAL_PORT == -1 - #define MYSERIAL0 USBSerial + #define MYSERIAL1 USBSerial #elif WITHIN(SERIAL_PORT, 0, 3) - #define MYSERIAL0 MSERIAL(SERIAL_PORT) + #define MYSERIAL1 MSERIAL(SERIAL_PORT) + DECLARE_SERIAL(SERIAL_PORT); +#else + #error "SERIAL_PORT must be from 0 to 3, or -1 for Native USB." #endif -#define HAL_SERVO_LIB libServo +// ------------------------ +// Types +// ------------------------ -typedef int8_t pin_t; +class libServo; +typedef libServo hal_servo_t; -#ifndef analogInputToDigitalPin - #define analogInputToDigitalPin(p) ((p < 12U) ? (p) + 54U : -1) -#endif +typedef int8_t pin_t; -#define CRITICAL_SECTION_START() uint32_t primask = __get_primask(); __disable_irq() -#define CRITICAL_SECTION_END() if (!primask) __enable_irq() -#define ISRS_ENABLED() (!__get_primask()) -#define ENABLE_ISRS() __enable_irq() -#define DISABLE_ISRS() __disable_irq() +// ------------------------ +// Interrupts +// ------------------------ -#undef sq -#define sq(x) ((x)*(x)) +#define CRITICAL_SECTION_START() const bool irqon = !__get_primask(); __disable_irq() +#define CRITICAL_SECTION_END() if (irqon) __enable_irq() -inline void HAL_init() {} +// ------------------------ +// ADC +// ------------------------ -// Clear reset reason -void HAL_clear_reset_source(); +#ifndef analogInputToDigitalPin + #define analogInputToDigitalPin(p) ((p < 12U) ? (p) + 54U : -1) +#endif -// Reset reason -uint8_t HAL_get_reset_source(); +#define HAL_ADC_VREF 3.3 +#define HAL_ADC_RESOLUTION 10 -inline void HAL_reboot() {} // reboot the board or restart the bootloader +// +// Pin Mapping for M42, M43, M226 +// +#define GET_PIN_MAP_PIN(index) index +#define GET_PIN_MAP_INDEX(pin) pin +#define PARSED_PIN_INDEX(code, dval) parser.intval(code, dval) -FORCE_INLINE void _delay_ms(const int delay_ms) { delay(delay_ms); } +// ------------------------ +// Class Utilities +// ------------------------ +#pragma GCC diagnostic push #if GCC_VERSION <= 50000 - #pragma GCC diagnostic push #pragma GCC diagnostic ignored "-Wunused-function" #endif extern "C" int freeMemory(); -#if GCC_VERSION <= 50000 - #pragma GCC diagnostic pop -#endif +#pragma GCC diagnostic pop -// ADC +// ------------------------ +// MarlinHAL Class +// ------------------------ -void HAL_adc_init(); +class MarlinHAL { +public: -#define HAL_ADC_VREF 3.3 -#define HAL_ADC_RESOLUTION 10 -#define HAL_START_ADC(pin) HAL_adc_start_conversion(pin) -#define HAL_READ_ADC() HAL_adc_get_result() -#define HAL_ADC_READY() true + // Earliest possible init, before setup() + MarlinHAL() {} -#define HAL_ANALOG_SELECT(pin) + static void init() {} // Called early in setup() + static void init_board() {} // Called less early in setup() + static void reboot(); // Restart the firmware from 0x0 -void HAL_adc_start_conversion(const uint8_t adc_pin); -uint16_t HAL_adc_get_result(); + // Interrupts + static bool isr_state() { return true; } + static void isr_on() { __enable_irq(); } + static void isr_off() { __disable_irq(); } -#define GET_PIN_MAP_PIN(index) index -#define GET_PIN_MAP_INDEX(pin) pin -#define PARSED_PIN_INDEX(code, dval) parser.intval(code, dval) + static void delay_ms(const int ms) { delay(ms); } + + // Tasks, called from idle() + static void idletask() {} + + // Reset + static uint8_t get_reset_source(); + static void clear_reset_source() {} + + // Free SRAM + static int freeMemory() { return ::freeMemory(); } + + // + // ADC Methods + // + + static int8_t adc_select; + + // Called by Temperature::init once at startup + static void adc_init(); + + // Called by Temperature::init for each sensor at startup + static void adc_enable(const pin_t) {} + + // Begin ADC sampling on the given channel + static void adc_start(const pin_t pin); + + // Is the ADC ready for reading? + static bool adc_ready() { return true; } + + // The current value of the ADC register + static uint16_t adc_value(); + + /** + * Set the PWM duty cycle for the pin to the given value. + * No option to invert the duty cycle [default = false] + * No option to change the scale of the provided value to enable finer PWM duty control [default = 255] + */ + static void set_pwm_duty(const pin_t pin, const uint16_t v, const uint16_t=255, const bool=false) { + analogWrite(pin, v); + } + +}; diff --git a/Marlin/src/HAL/TEENSY35_36/HAL_SPI.cpp b/Marlin/src/HAL/TEENSY35_36/HAL_SPI.cpp index 84852cd3580d..d80f57b2c436 100644 --- a/Marlin/src/HAL/TEENSY35_36/HAL_SPI.cpp +++ b/Marlin/src/HAL/TEENSY35_36/HAL_SPI.cpp @@ -26,19 +26,19 @@ #if defined(__MK64FX512__) || defined(__MK66FX1M0__) +#include "../../inc/MarlinConfig.h" #include "HAL.h" + #include #include #include "spi_pins.h" -#include "../../core/macros.h" static SPISettings spiConfig; void spiBegin() { - #if !PIN_EXISTS(SD_SS) - #error "SD_SS_PIN not defined!" + #if PIN_EXISTS(SD_SS) + OUT_WRITE(SD_SS_PIN, HIGH); #endif - OUT_WRITE(SD_SS_PIN, HIGH); SET_OUTPUT(SD_SCK_PIN); SET_INPUT(SD_MISO_PIN); SET_OUTPUT(SD_MOSI_PIN); @@ -64,7 +64,7 @@ void spiInit(uint8_t spiRate) { case SPI_SPEED_5: clock = 625000; break; case SPI_SPEED_6: clock = 312500; break; default: - clock = 4000000; // Default from the SPI libarary + clock = 4000000; // Default from the SPI library } spiConfig = SPISettings(clock, MSBFIRST, SPI_MODE0); SPI.begin(); @@ -80,7 +80,7 @@ uint8_t spiRec() { //return SPDR; } -void spiRead(uint8_t* buf, uint16_t nbyte) { +void spiRead(uint8_t *buf, uint16_t nbyte) { SPI.beginTransaction(spiConfig); SPI.transfer(buf, nbyte); SPI.endTransaction(); @@ -103,7 +103,7 @@ void spiSend(uint8_t b) { //while (!TEST(SPSR, SPIF)) { /* Intentionally left empty */ } } -void spiSendBlock(uint8_t token, const uint8_t* buf) { +void spiSendBlock(uint8_t token, const uint8_t *buf) { SPI.beginTransaction(spiConfig); SPDR = token; for (uint16_t i = 0; i < 512; i += 2) { diff --git a/Marlin/src/HAL/TEENSY35_36/MarlinSPI.h b/Marlin/src/HAL/TEENSY35_36/MarlinSPI.h new file mode 100644 index 000000000000..0c447ba4cb3d --- /dev/null +++ b/Marlin/src/HAL/TEENSY35_36/MarlinSPI.h @@ -0,0 +1,26 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2021 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + * + */ +#pragma once + +#include + +using MarlinSPI = SPIClass; diff --git a/Marlin/src/HAL/TEENSY35_36/eeprom.cpp b/Marlin/src/HAL/TEENSY35_36/eeprom.cpp index 8cd6b4ff41d7..b80e93b536a5 100644 --- a/Marlin/src/HAL/TEENSY35_36/eeprom.cpp +++ b/Marlin/src/HAL/TEENSY35_36/eeprom.cpp @@ -42,13 +42,13 @@ bool PersistentStore::access_start() { return true; } bool PersistentStore::access_finish() { return true; } bool PersistentStore::write_data(int &pos, const uint8_t *value, size_t size, uint16_t *crc) { + uint16_t written = 0; while (size--) { uint8_t * const p = (uint8_t * const)pos; uint8_t v = *value; - // EEPROM has only ~100,000 write cycles, - // so only write bytes that have changed! - if (v != eeprom_read_byte(p)) { + if (v != eeprom_read_byte(p)) { // EEPROM has only ~100,000 write cycles, so only write bytes that have changed! eeprom_write_byte(p, v); + if (++written & 0x7F) delay(2); else safe_delay(2); // Avoid triggering watchdog during long EEPROM writes if (eeprom_read_byte(p) != v) { SERIAL_ECHO_MSG(STR_ERR_EEPROM_WRITE); return true; diff --git a/Marlin/src/HAL/TEENSY35_36/endstop_interrupts.h b/Marlin/src/HAL/TEENSY35_36/endstop_interrupts.h index 87e6a7507abc..a30024888535 100644 --- a/Marlin/src/HAL/TEENSY35_36/endstop_interrupts.h +++ b/Marlin/src/HAL/TEENSY35_36/endstop_interrupts.h @@ -63,4 +63,10 @@ void setup_endstop_interrupts() { TERN_(HAS_Z4_MAX, _ATTACH(Z4_MAX_PIN)); TERN_(HAS_Z4_MIN, _ATTACH(Z4_MIN_PIN)); TERN_(HAS_Z_MIN_PROBE_PIN, _ATTACH(Z_MIN_PROBE_PIN)); + TERN_(HAS_I_MAX, _ATTACH(I_MAX_PIN)); + TERN_(HAS_I_MIN, _ATTACH(I_MIN_PIN)); + TERN_(HAS_J_MAX, _ATTACH(J_MAX_PIN)); + TERN_(HAS_J_MIN, _ATTACH(J_MIN_PIN)); + TERN_(HAS_K_MAX, _ATTACH(K_MAX_PIN)); + TERN_(HAS_K_MIN, _ATTACH(K_MIN_PIN)); } diff --git a/Marlin/src/HAL/TEENSY35_36/pinsDebug.h b/Marlin/src/HAL/TEENSY35_36/pinsDebug.h index e529fa93be16..7a2e1d6e592e 100644 --- a/Marlin/src/HAL/TEENSY35_36/pinsDebug.h +++ b/Marlin/src/HAL/TEENSY35_36/pinsDebug.h @@ -2,6 +2,9 @@ * Marlin 3D Printer Firmware * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * * This program is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation, either version 3 of the License, or diff --git a/Marlin/src/HAL/TEENSY35_36/timers.cpp b/Marlin/src/HAL/TEENSY35_36/timers.cpp index 8067d091dd01..39095fbd77af 100644 --- a/Marlin/src/HAL/TEENSY35_36/timers.cpp +++ b/Marlin/src/HAL/TEENSY35_36/timers.cpp @@ -47,7 +47,7 @@ FORCE_INLINE static void __DSB() { void HAL_timer_start(const uint8_t timer_num, const uint32_t frequency) { switch (timer_num) { - case 0: + case MF_TIMER_STEP: FTM0_MODE = FTM_MODE_WPDIS | FTM_MODE_FTMEN; FTM0_SC = 0x00; // Set this to zero before changing the modulus FTM0_CNT = 0x0000; // Reset the count to zero @@ -56,7 +56,7 @@ void HAL_timer_start(const uint8_t timer_num, const uint32_t frequency) { FTM0_SC = (FTM_SC_CLKS(0b1) & FTM_SC_CLKS_MASK) | (FTM_SC_PS(FTM0_TIMER_PRESCALE_BITS) & FTM_SC_PS_MASK); // Bus clock 60MHz divided by prescaler 8 FTM0_C0SC = FTM_CSC_CHIE | FTM_CSC_MSA | FTM_CSC_ELSA; break; - case 1: + case MF_TIMER_TEMP: FTM1_MODE = FTM_MODE_WPDIS | FTM_MODE_FTMEN; // Disable write protection, Enable FTM1 FTM1_SC = 0x00; // Set this to zero before changing the modulus FTM1_CNT = 0x0000; // Reset the count to zero @@ -70,15 +70,15 @@ void HAL_timer_start(const uint8_t timer_num, const uint32_t frequency) { void HAL_timer_enable_interrupt(const uint8_t timer_num) { switch (timer_num) { - case 0: NVIC_ENABLE_IRQ(IRQ_FTM0); break; - case 1: NVIC_ENABLE_IRQ(IRQ_FTM1); break; + case MF_TIMER_STEP: NVIC_ENABLE_IRQ(IRQ_FTM0); break; + case MF_TIMER_TEMP: NVIC_ENABLE_IRQ(IRQ_FTM1); break; } } void HAL_timer_disable_interrupt(const uint8_t timer_num) { switch (timer_num) { - case 0: NVIC_DISABLE_IRQ(IRQ_FTM0); break; - case 1: NVIC_DISABLE_IRQ(IRQ_FTM1); break; + case MF_TIMER_STEP: NVIC_DISABLE_IRQ(IRQ_FTM0); break; + case MF_TIMER_TEMP: NVIC_DISABLE_IRQ(IRQ_FTM1); break; } // We NEED memory barriers to ensure Interrupts are actually disabled! @@ -89,20 +89,20 @@ void HAL_timer_disable_interrupt(const uint8_t timer_num) { bool HAL_timer_interrupt_enabled(const uint8_t timer_num) { switch (timer_num) { - case 0: return NVIC_IS_ENABLED(IRQ_FTM0); - case 1: return NVIC_IS_ENABLED(IRQ_FTM1); + case MF_TIMER_STEP: return NVIC_IS_ENABLED(IRQ_FTM0); + case MF_TIMER_TEMP: return NVIC_IS_ENABLED(IRQ_FTM1); } return false; } void HAL_timer_isr_prologue(const uint8_t timer_num) { switch (timer_num) { - case 0: + case MF_TIMER_STEP: FTM0_CNT = 0x0000; FTM0_SC &= ~FTM_SC_TOF; // Clear FTM Overflow flag FTM0_C0SC &= ~FTM_CSC_CHF; // Clear FTM Channel Compare flag break; - case 1: + case MF_TIMER_TEMP: FTM1_CNT = 0x0000; FTM1_SC &= ~FTM_SC_TOF; // Clear FTM Overflow flag FTM1_C0SC &= ~FTM_CSC_CHF; // Clear FTM Channel Compare flag diff --git a/Marlin/src/HAL/TEENSY35_36/timers.h b/Marlin/src/HAL/TEENSY35_36/timers.h index 99269ac6571f..8af79d73928e 100644 --- a/Marlin/src/HAL/TEENSY35_36/timers.h +++ b/Marlin/src/HAL/TEENSY35_36/timers.h @@ -45,14 +45,14 @@ typedef uint32_t hal_timer_t; #define HAL_TIMER_RATE (FTM0_TIMER_RATE) -#ifndef STEP_TIMER_NUM - #define STEP_TIMER_NUM 0 // Timer Index for Stepper +#ifndef MF_TIMER_STEP + #define MF_TIMER_STEP 0 // Timer Index for Stepper #endif -#ifndef PULSE_TIMER_NUM - #define PULSE_TIMER_NUM STEP_TIMER_NUM +#ifndef MF_TIMER_PULSE + #define MF_TIMER_PULSE MF_TIMER_STEP #endif -#ifndef TEMP_TIMER_NUM - #define TEMP_TIMER_NUM 1 // Timer Index for Temperature +#ifndef MF_TIMER_TEMP + #define MF_TIMER_TEMP 1 // Timer Index for Temperature #endif #define TEMP_TIMER_FREQUENCY 1000 @@ -65,12 +65,12 @@ typedef uint32_t hal_timer_t; #define PULSE_TIMER_PRESCALE STEPPER_TIMER_PRESCALE #define PULSE_TIMER_TICKS_PER_US STEPPER_TIMER_TICKS_PER_US -#define ENABLE_STEPPER_DRIVER_INTERRUPT() HAL_timer_enable_interrupt(STEP_TIMER_NUM) -#define DISABLE_STEPPER_DRIVER_INTERRUPT() HAL_timer_disable_interrupt(STEP_TIMER_NUM) -#define STEPPER_ISR_ENABLED() HAL_timer_interrupt_enabled(STEP_TIMER_NUM) +#define ENABLE_STEPPER_DRIVER_INTERRUPT() HAL_timer_enable_interrupt(MF_TIMER_STEP) +#define DISABLE_STEPPER_DRIVER_INTERRUPT() HAL_timer_disable_interrupt(MF_TIMER_STEP) +#define STEPPER_ISR_ENABLED() HAL_timer_interrupt_enabled(MF_TIMER_STEP) -#define ENABLE_TEMPERATURE_INTERRUPT() HAL_timer_enable_interrupt(TEMP_TIMER_NUM) -#define DISABLE_TEMPERATURE_INTERRUPT() HAL_timer_disable_interrupt(TEMP_TIMER_NUM) +#define ENABLE_TEMPERATURE_INTERRUPT() HAL_timer_enable_interrupt(MF_TIMER_TEMP) +#define DISABLE_TEMPERATURE_INTERRUPT() HAL_timer_disable_interrupt(MF_TIMER_TEMP) #ifndef HAL_STEP_TIMER_ISR #define HAL_STEP_TIMER_ISR() extern "C" void ftm0_isr() //void TC3_Handler() @@ -83,23 +83,23 @@ void HAL_timer_start(const uint8_t timer_num, const uint32_t frequency); FORCE_INLINE static void HAL_timer_set_compare(const uint8_t timer_num, const hal_timer_t compare) { switch (timer_num) { - case 0: FTM0_C0V = compare; break; - case 1: FTM1_C0V = compare; break; + case MF_TIMER_STEP: FTM0_C0V = compare; break; + case MF_TIMER_TEMP: FTM1_C0V = compare; break; } } FORCE_INLINE static hal_timer_t HAL_timer_get_compare(const uint8_t timer_num) { switch (timer_num) { - case 0: return FTM0_C0V; - case 1: return FTM1_C0V; + case MF_TIMER_STEP: return FTM0_C0V; + case MF_TIMER_TEMP: return FTM1_C0V; } return 0; } FORCE_INLINE static hal_timer_t HAL_timer_get_count(const uint8_t timer_num) { switch (timer_num) { - case 0: return FTM0_CNT; - case 1: return FTM1_CNT; + case MF_TIMER_STEP: return FTM0_CNT; + case MF_TIMER_TEMP: return FTM1_CNT; } return 0; } @@ -109,4 +109,4 @@ void HAL_timer_disable_interrupt(const uint8_t timer_num); bool HAL_timer_interrupt_enabled(const uint8_t timer_num); void HAL_timer_isr_prologue(const uint8_t timer_num); -#define HAL_timer_isr_epilogue(TIMER_NUM) +#define HAL_timer_isr_epilogue(T) NOOP diff --git a/Marlin/src/HAL/TEENSY40_41/HAL.cpp b/Marlin/src/HAL/TEENSY40_41/HAL.cpp index 26449d7eb298..68bd38f72ff8 100644 --- a/Marlin/src/HAL/TEENSY40_41/HAL.cpp +++ b/Marlin/src/HAL/TEENSY40_41/HAL.cpp @@ -26,145 +26,158 @@ #ifdef __IMXRT1062__ +#include "../../inc/MarlinConfig.h" #include "HAL.h" + #include "../shared/Delay.h" #include "timers.h" - #include -DefaultSerial MSerial(false); +// ------------------------ +// Serial ports +// ------------------------ + +#define _IMPLEMENT_SERIAL(X) DefaultSerial##X MSerial##X(false, Serial##X) +#define IMPLEMENT_SERIAL(X) _IMPLEMENT_SERIAL(X) +#if WITHIN(SERIAL_PORT, 0, 3) + IMPLEMENT_SERIAL(SERIAL_PORT); +#endif USBSerialType USBSerial(false, SerialUSB); -uint16_t HAL_adc_result, HAL_adc_select; - -static const uint8_t pin2sc1a[] = { - 0x07, // 0/A0 AD_B1_02 - 0x08, // 1/A1 AD_B1_03 - 0x0C, // 2/A2 AD_B1_07 - 0x0B, // 3/A3 AD_B1_06 - 0x06, // 4/A4 AD_B1_01 - 0x05, // 5/A5 AD_B1_00 - 0x0F, // 6/A6 AD_B1_10 - 0x00, // 7/A7 AD_B1_11 - 0x0D, // 8/A8 AD_B1_08 - 0x0E, // 9/A9 AD_B1_09 - 0x01, // 24/A10 AD_B0_12 - 0x02, // 25/A11 AD_B0_13 - 0x83, // 26/A12 AD_B1_14 - only on ADC2, 3 - 0x84, // 27/A13 AD_B1_15 - only on ADC2, 4 - 0x07, // 14/A0 AD_B1_02 - 0x08, // 15/A1 AD_B1_03 - 0x0C, // 16/A2 AD_B1_07 - 0x0B, // 17/A3 AD_B1_06 - 0x06, // 18/A4 AD_B1_01 - 0x05, // 19/A5 AD_B1_00 - 0x0F, // 20/A6 AD_B1_10 - 0x00, // 21/A7 AD_B1_11 - 0x0D, // 22/A8 AD_B1_08 - 0x0E, // 23/A9 AD_B1_09 - 0x01, // 24/A10 AD_B0_12 - 0x02, // 25/A11 AD_B0_13 - 0x83, // 26/A12 AD_B1_14 - only on ADC2, 3 - 0x84, // 27/A13 AD_B1_15 - only on ADC2, 4 - #ifdef ARDUINO_TEENSY41 - 0xFF, // 28 - 0xFF, // 29 - 0xFF, // 30 - 0xFF, // 31 - 0xFF, // 32 - 0xFF, // 33 - 0xFF, // 34 - 0xFF, // 35 - 0xFF, // 36 - 0xFF, // 37 - 0x81, // 38/A14 AD_B1_12 - only on ADC2, 1 - 0x82, // 39/A15 AD_B1_13 - only on ADC2, 2 - 0x09, // 40/A16 AD_B1_04 - 0x0A, // 41/A17 AD_B1_05 - #endif -}; - -/* -// disable interrupts -void cli() { noInterrupts(); } - -// enable interrupts -void sei() { interrupts(); } -*/ - -void HAL_adc_init() { - analog_init(); - while (ADC1_GC & ADC_GC_CAL) ; - while (ADC2_GC & ADC_GC_CAL) ; +// ------------------------ +// Class Utilities +// ------------------------ + +#define __bss_end _ebss + +extern "C" { + extern char __bss_end; + extern char __heap_start; + extern void* __brkval; + + // Doesn't work on Teensy 4.x + uint32_t freeMemory() { + uint32_t free_memory; + free_memory = ((uint32_t)&free_memory) - (((uint32_t)__brkval) ?: ((uint32_t)&__bss_end)); + return free_memory; + } } -void HAL_clear_reset_source() { - uint32_t reset_source = SRC_SRSR; - SRC_SRSR = reset_source; - } +// ------------------------ +// FastIO +// ------------------------ + +bool is_output(pin_t pin) { + const struct digital_pin_bitband_and_config_table_struct *p; + p = digital_pin_to_info_PGM + pin; + return (*(p->reg + 1) & p->mask); +} + +// ------------------------ +// MarlinHAL Class +// ------------------------ + +void MarlinHAL::reboot() { _reboot_Teensyduino_(); } -uint8_t HAL_get_reset_source() { +uint8_t MarlinHAL::get_reset_source() { switch (SRC_SRSR & 0xFF) { case 1: return RST_POWER_ON; break; case 2: return RST_SOFTWARE; break; case 4: return RST_EXTERNAL; break; - // case 8: return RST_BROWN_OUT; break; + //case 8: return RST_BROWN_OUT; break; case 16: return RST_WATCHDOG; break; - case 64: return RST_JTAG; break; - // case 128: return RST_OVERTEMP; break; + case 64: return RST_JTAG; break; + //case 128: return RST_OVERTEMP; break; } return 0; } -#define __bss_end _ebss +void MarlinHAL::clear_reset_source() { + uint32_t reset_source = SRC_SRSR; + SRC_SRSR = reset_source; +} -extern "C" { - extern char __bss_end; - extern char __heap_start; - extern void* __brkval; +// ADC - // Doesn't work on Teensy 4.x - uint32_t freeMemory() { - uint32_t free_memory; - if ((uint32_t)__brkval == 0) - free_memory = ((uint32_t)&free_memory) - ((uint32_t)&__bss_end); - else - free_memory = ((uint32_t)&free_memory) - ((uint32_t)__brkval); - return free_memory; - } +int8_t MarlinHAL::adc_select; + +void MarlinHAL::adc_init() { + analog_init(); + while (ADC1_GC & ADC_GC_CAL) { /* wait */ } + while (ADC2_GC & ADC_GC_CAL) { /* wait */ } } -void HAL_adc_start_conversion(const uint8_t adc_pin) { +void MarlinHAL::adc_start(const pin_t adc_pin) { + static const uint8_t pin2sc1a[] = { + 0x07, // 0/A0 AD_B1_02 + 0x08, // 1/A1 AD_B1_03 + 0x0C, // 2/A2 AD_B1_07 + 0x0B, // 3/A3 AD_B1_06 + 0x06, // 4/A4 AD_B1_01 + 0x05, // 5/A5 AD_B1_00 + 0x0F, // 6/A6 AD_B1_10 + 0x00, // 7/A7 AD_B1_11 + 0x0D, // 8/A8 AD_B1_08 + 0x0E, // 9/A9 AD_B1_09 + 0x01, // 24/A10 AD_B0_12 + 0x02, // 25/A11 AD_B0_13 + 0x83, // 26/A12 AD_B1_14 - only on ADC2, 3 + 0x84, // 27/A13 AD_B1_15 - only on ADC2, 4 + 0x07, // 14/A0 AD_B1_02 + 0x08, // 15/A1 AD_B1_03 + 0x0C, // 16/A2 AD_B1_07 + 0x0B, // 17/A3 AD_B1_06 + 0x06, // 18/A4 AD_B1_01 + 0x05, // 19/A5 AD_B1_00 + 0x0F, // 20/A6 AD_B1_10 + 0x00, // 21/A7 AD_B1_11 + 0x0D, // 22/A8 AD_B1_08 + 0x0E, // 23/A9 AD_B1_09 + 0x01, // 24/A10 AD_B0_12 + 0x02, // 25/A11 AD_B0_13 + 0x83, // 26/A12 AD_B1_14 - only on ADC2, 3 + 0x84, // 27/A13 AD_B1_15 - only on ADC2, 4 + #ifdef ARDUINO_TEENSY41 + 0xFF, // 28 + 0xFF, // 29 + 0xFF, // 30 + 0xFF, // 31 + 0xFF, // 32 + 0xFF, // 33 + 0xFF, // 34 + 0xFF, // 35 + 0xFF, // 36 + 0xFF, // 37 + 0x81, // 38/A14 AD_B1_12 - only on ADC2, 1 + 0x82, // 39/A15 AD_B1_13 - only on ADC2, 2 + 0x09, // 40/A16 AD_B1_04 + 0x0A, // 41/A17 AD_B1_05 + #endif + }; const uint16_t pin = pin2sc1a[adc_pin]; if (pin == 0xFF) { - HAL_adc_select = -1; // Digital only + adc_select = -1; // Digital only } else if (pin & 0x80) { - HAL_adc_select = 1; + adc_select = 1; ADC2_HC0 = pin & 0x7F; } else { - HAL_adc_select = 0; + adc_select = 0; ADC1_HC0 = pin; } } -uint16_t HAL_adc_get_result() { - switch (HAL_adc_select) { +uint16_t MarlinHAL::adc_value() { + switch (adc_select) { case 0: - while (!(ADC1_HS & ADC_HS_COCO0)) ; // wait + while (!(ADC1_HS & ADC_HS_COCO0)) { /* wait */ } return ADC1_R0; case 1: - while (!(ADC2_HS & ADC_HS_COCO0)) ; // wait + while (!(ADC2_HS & ADC_HS_COCO0)) { /* wait */ } return ADC2_R0; } return 0; } -bool is_output(uint8_t pin) { - const struct digital_pin_bitband_and_config_table_struct *p; - p = digital_pin_to_info_PGM + pin; - return (*(p->reg + 1) & p->mask); -} - #endif // __IMXRT1062__ diff --git a/Marlin/src/HAL/TEENSY40_41/HAL.h b/Marlin/src/HAL/TEENSY40_41/HAL.h index 6aa1e521a48b..a21e65285409 100644 --- a/Marlin/src/HAL/TEENSY40_41/HAL.h +++ b/Marlin/src/HAL/TEENSY40_41/HAL.h @@ -41,10 +41,6 @@ #include "../../feature/ethernet.h" #endif -//#define ST7920_DELAY_1 DELAY_NS(600) -//#define ST7920_DELAY_2 DELAY_NS(750) -//#define ST7920_DELAY_3 DELAY_NS(750) - // ------------------------ // Defines // ------------------------ @@ -55,99 +51,166 @@ #define IS_TEENSY41 1 #endif +#define CPU_ST7920_DELAY_1 600 +#define CPU_ST7920_DELAY_2 750 +#define CPU_ST7920_DELAY_3 750 + +#undef sq +#define sq(x) ((x)*(x)) + +// Don't place string constants in PROGMEM +#undef PSTR +#define PSTR(str) ({static const char *data = (str); &data[0];}) + +// ------------------------ +// Serial ports +// ------------------------ + #include "../../core/serial_hook.h" -typedef Serial0Type DefaultSerial; -extern DefaultSerial MSerial; -typedef ForwardSerial0Type USBSerialType; + +#define Serial0 Serial +#define _DECLARE_SERIAL(X) \ + typedef ForwardSerial1Class DefaultSerial##X; \ + extern DefaultSerial##X MSerial##X +#define DECLARE_SERIAL(X) _DECLARE_SERIAL(X) + +typedef ForwardSerial1Class USBSerialType; extern USBSerialType USBSerial; #define _MSERIAL(X) MSerial##X #define MSERIAL(X) _MSERIAL(X) -#define MSerial0 MSerial #if SERIAL_PORT == -1 - #define MYSERIAL0 SerialUSB + #define MYSERIAL1 SerialUSB #elif WITHIN(SERIAL_PORT, 0, 8) - #define MYSERIAL0 MSERIAL(SERIAL_PORT) + DECLARE_SERIAL(SERIAL_PORT); + #define MYSERIAL1 MSERIAL(SERIAL_PORT) #else - #error "The required SERIAL_PORT must be from -1 to 8. Please update your configuration." + #error "The required SERIAL_PORT must be from 0 to 8, or -1 for Native USB." #endif #ifdef SERIAL_PORT_2 #if SERIAL_PORT_2 == -1 - #define MYSERIAL1 usbSerial + #define MYSERIAL2 usbSerial #elif SERIAL_PORT_2 == -2 - #define MYSERIAL1 ethernet.telnetClient + #define MYSERIAL2 ethernet.telnetClient #elif WITHIN(SERIAL_PORT_2, 0, 8) - #define MYSERIAL1 MSERIAL(SERIAL_PORT_2) + #define MYSERIAL2 MSERIAL(SERIAL_PORT_2) #else - #error "SERIAL_PORT_2 must be from -2 to 8. Please update your configuration." + #error "SERIAL_PORT_2 must be from 0 to 8, or -1 for Native USB, or -2 for Ethernet." #endif #endif -#define HAL_SERVO_LIB libServo +// ------------------------ +// Types +// ------------------------ + +class libServo; +typedef libServo hal_servo_t; typedef int8_t pin_t; -#ifndef analogInputToDigitalPin - #define analogInputToDigitalPin(p) ((p < 12U) ? (p) + 54U : -1) -#endif +// ------------------------ +// Interrupts +// ------------------------ -#define CRITICAL_SECTION_START() uint32_t primask = __get_primask(); __disable_irq() -#define CRITICAL_SECTION_END() if (!primask) __enable_irq() -#define ISRS_ENABLED() (!__get_primask()) -#define ENABLE_ISRS() __enable_irq() -#define DISABLE_ISRS() __disable_irq() +#define CRITICAL_SECTION_START() const bool irqon = !__get_primask(); __disable_irq() +#define CRITICAL_SECTION_END() if (irqon) __enable_irq() -#undef sq -#define sq(x) ((x)*(x)) +// ------------------------ +// ADC +// ------------------------ -// Don't place string constants in PROGMEM -#undef PSTR -#define PSTR(str) ({static const char *data = (str); &data[0];}) +#ifndef analogInputToDigitalPin + #define analogInputToDigitalPin(p) ((p < 12U) ? (p) + 54U : -1) +#endif -// Enable hooks into idle and setup for HAL -#define HAL_IDLETASK 1 -FORCE_INLINE void HAL_idletask() {} -FORCE_INLINE void HAL_init() {} +#define HAL_ADC_VREF 3.3 +#define HAL_ADC_RESOLUTION 10 +#define HAL_ADC_FILTERED // turn off ADC oversampling -// Clear reset reason -void HAL_clear_reset_source(); +// +// Pin Mapping for M42, M43, M226 +// +#define GET_PIN_MAP_PIN(index) index +#define GET_PIN_MAP_INDEX(pin) pin +#define PARSED_PIN_INDEX(code, dval) parser.intval(code, dval) -// Reset reason -uint8_t HAL_get_reset_source(); +// FastIO +bool is_output(pin_t pin); -FORCE_INLINE void _delay_ms(const int delay_ms) { delay(delay_ms); } +// ------------------------ +// Class Utilities +// ------------------------ +#pragma GCC diagnostic push #if GCC_VERSION <= 50000 - #pragma GCC diagnostic push #pragma GCC diagnostic ignored "-Wunused-function" #endif extern "C" uint32_t freeMemory(); -#if GCC_VERSION <= 50000 - #pragma GCC diagnostic pop -#endif +#pragma GCC diagnostic pop -// ADC +// ------------------------ +// MarlinHAL Class +// ------------------------ -void HAL_adc_init(); +class MarlinHAL { +public: -#define HAL_ADC_VREF 3.3 -#define HAL_ADC_RESOLUTION 10 -#define HAL_ADC_FILTERED // turn off ADC oversampling -#define HAL_START_ADC(pin) HAL_adc_start_conversion(pin) -#define HAL_READ_ADC() HAL_adc_get_result() -#define HAL_ADC_READY() true + // Earliest possible init, before setup() + MarlinHAL() {} -#define HAL_ANALOG_SELECT(pin) + static void init() {} // Called early in setup() + static void init_board() {} // Called less early in setup() + static void reboot(); // Restart the firmware from 0x0 -void HAL_adc_start_conversion(const uint8_t adc_pin); -uint16_t HAL_adc_get_result(); + // Interrupts + static bool isr_state() { return !__get_primask(); } + static void isr_on() { __enable_irq(); } + static void isr_off() { __disable_irq(); } -#define GET_PIN_MAP_PIN(index) index -#define GET_PIN_MAP_INDEX(pin) pin -#define PARSED_PIN_INDEX(code, dval) parser.intval(code, dval) + static void delay_ms(const int ms) { delay(ms); } + + // Tasks, called from idle() + static void idletask() {} + + // Reset + static uint8_t get_reset_source(); + static void clear_reset_source(); + + // Free SRAM + static int freeMemory() { return ::freeMemory(); } + + // + // ADC Methods + // + + static int8_t adc_select; + + // Called by Temperature::init once at startup + static void adc_init(); + + // Called by Temperature::init for each sensor at startup + static void adc_enable(const pin_t pin) {} + + // Begin ADC sampling on the given channel + static void adc_start(const pin_t pin); + + // Is the ADC ready for reading? + static bool adc_ready() { return true; } + + // The current value of the ADC register + static uint16_t adc_value(); + + /** + * Set the PWM duty cycle for the pin to the given value. + * No option to invert the duty cycle [default = false] + * No option to change the scale of the provided value to enable finer PWM duty control [default = 255] + */ + static void set_pwm_duty(const pin_t pin, const uint16_t v, const uint16_t=255, const bool=false) { + analogWrite(pin, v); + } -bool is_output(uint8_t pin); +}; diff --git a/Marlin/src/HAL/TEENSY40_41/HAL_SPI.cpp b/Marlin/src/HAL/TEENSY40_41/HAL_SPI.cpp index 8c93049027ab..9dcb812faf0c 100644 --- a/Marlin/src/HAL/TEENSY40_41/HAL_SPI.cpp +++ b/Marlin/src/HAL/TEENSY40_41/HAL_SPI.cpp @@ -26,11 +26,12 @@ #ifdef __IMXRT1062__ +#include "../../inc/MarlinConfig.h" #include "HAL.h" + #include #include #include "spi_pins.h" -#include "../../core/macros.h" static SPISettings spiConfig; @@ -50,12 +51,9 @@ static SPISettings spiConfig; // ------------------------ void spiBegin() { - #ifndef SD_SS_PIN - #error "SD_SS_PIN is not defined!" + #if PIN_EXISTS(SD_SS) + OUT_WRITE(SD_SS_PIN, HIGH); #endif - - OUT_WRITE(SD_SS_PIN, HIGH); - //SET_OUTPUT(SD_SCK_PIN); //SET_INPUT(SD_MISO_PIN); //SET_OUTPUT(SD_MOSI_PIN); @@ -81,7 +79,7 @@ void spiInit(uint8_t spiRate) { case SPI_SPEED_5: clock = 625000; break; case SPI_SPEED_6: clock = 312500; break; default: - clock = 4000000; // Default from the SPI libarary + clock = 4000000; // Default from the SPI library } spiConfig = SPISettings(clock, MSBFIRST, SPI_MODE0); SPI.begin(); @@ -97,7 +95,7 @@ uint8_t spiRec() { //return SPDR; } -void spiRead(uint8_t* buf, uint16_t nbyte) { +void spiRead(uint8_t *buf, uint16_t nbyte) { SPI.beginTransaction(spiConfig); SPI.transfer(buf, nbyte); SPI.endTransaction(); @@ -120,7 +118,7 @@ void spiSend(uint8_t b) { //while (!TEST(SPSR, SPIF)) { /* Intentionally left empty */ } } -void spiSendBlock(uint8_t token, const uint8_t* buf) { +void spiSendBlock(uint8_t token, const uint8_t *buf) { SPI.beginTransaction(spiConfig); SPDR = token; for (uint16_t i = 0; i < 512; i += 2) { diff --git a/Marlin/src/HAL/TEENSY40_41/MarlinSPI.h b/Marlin/src/HAL/TEENSY40_41/MarlinSPI.h new file mode 100644 index 000000000000..0c447ba4cb3d --- /dev/null +++ b/Marlin/src/HAL/TEENSY40_41/MarlinSPI.h @@ -0,0 +1,26 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2021 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + * + */ +#pragma once + +#include + +using MarlinSPI = SPIClass; diff --git a/Marlin/src/HAL/TEENSY40_41/eeprom.cpp b/Marlin/src/HAL/TEENSY40_41/eeprom.cpp index fe2de388a7a0..3cd376edce62 100644 --- a/Marlin/src/HAL/TEENSY40_41/eeprom.cpp +++ b/Marlin/src/HAL/TEENSY40_41/eeprom.cpp @@ -22,14 +22,14 @@ */ #ifdef __IMXRT1062__ -#include "../../inc/MarlinConfig.h" - -#if USE_WIRED_EEPROM - /** * HAL PersistentStore for Teensy 4.0 (IMXRT1062DVL6A) / 4.1 (IMXRT1062DVJ6A) */ +#include "../../inc/MarlinConfig.h" + +#if USE_WIRED_EEPROM + #include "../shared/eeprom_api.h" #include @@ -42,13 +42,13 @@ bool PersistentStore::access_start() { return true; } bool PersistentStore::access_finish() { return true; } bool PersistentStore::write_data(int &pos, const uint8_t *value, size_t size, uint16_t *crc) { + uint16_t written = 0; while (size--) { uint8_t * const p = (uint8_t * const)pos; uint8_t v = *value; - // EEPROM has only ~100,000 write cycles, - // so only write bytes that have changed! - if (v != eeprom_read_byte(p)) { + if (v != eeprom_read_byte(p)) { // EEPROM has only ~100,000 write cycles, so only write bytes that have changed! eeprom_write_byte(p, v); + if (++written & 0x7F) delay(2); else safe_delay(2); // Avoid triggering watchdog during long EEPROM writes if (eeprom_read_byte(p) != v) { SERIAL_ECHO_MSG(STR_ERR_EEPROM_WRITE); return true; diff --git a/Marlin/src/HAL/TEENSY40_41/endstop_interrupts.h b/Marlin/src/HAL/TEENSY40_41/endstop_interrupts.h index a05e911668a7..4c3ddec9f1f1 100644 --- a/Marlin/src/HAL/TEENSY40_41/endstop_interrupts.h +++ b/Marlin/src/HAL/TEENSY40_41/endstop_interrupts.h @@ -63,4 +63,10 @@ void setup_endstop_interrupts() { TERN_(HAS_Z4_MAX, _ATTACH(Z4_MAX_PIN)); TERN_(HAS_Z4_MIN, _ATTACH(Z4_MIN_PIN)); TERN_(HAS_Z_MIN_PROBE_PIN, _ATTACH(Z_MIN_PROBE_PIN)); + TERN_(HAS_I_MAX, _ATTACH(I_MAX_PIN)); + TERN_(HAS_I_MIN, _ATTACH(I_MIN_PIN)); + TERN_(HAS_J_MAX, _ATTACH(J_MAX_PIN)); + TERN_(HAS_J_MIN, _ATTACH(J_MIN_PIN)); + TERN_(HAS_K_MAX, _ATTACH(K_MAX_PIN)); + TERN_(HAS_K_MIN, _ATTACH(K_MIN_PIN)); } diff --git a/Marlin/src/HAL/TEENSY40_41/pinsDebug.h b/Marlin/src/HAL/TEENSY40_41/pinsDebug.h index 4ad62d00fea5..94b85ea56861 100644 --- a/Marlin/src/HAL/TEENSY40_41/pinsDebug.h +++ b/Marlin/src/HAL/TEENSY40_41/pinsDebug.h @@ -2,6 +2,9 @@ * Marlin 3D Printer Firmware * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * * This program is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation, either version 3 of the License, or @@ -30,6 +33,7 @@ #define PRINT_PORT(p) #define PRINT_ARRAY_NAME(x) do{ sprintf_P(buffer, PSTR("%-" STRINGIFY(MAX_NAME_LENGTH) "s"), pin_array[x].name); SERIAL_ECHO(buffer); }while(0) #define PRINT_PIN(p) do{ sprintf_P(buffer, PSTR("%02d"), p); SERIAL_ECHO(buffer); }while(0) +#define PRINT_PIN_ANALOG(p) do{ sprintf_P(buffer, PSTR(" (A%2d) "), DIGITAL_PIN_TO_ANALOG_PIN(pin)); SERIAL_ECHO(buffer); }while(0) #define GET_ARRAY_PIN(p) pin_array[p].pin #define GET_ARRAY_IS_DIGITAL(p) pin_array[p].is_digital #define VALID_PIN(pin) (pin >= 0 && pin < (int8_t)NUMBER_PINS_TOTAL ? 1 : 0) diff --git a/Marlin/src/HAL/TEENSY40_41/timers.cpp b/Marlin/src/HAL/TEENSY40_41/timers.cpp index 81c9b08c17a5..ed99f65d6e26 100644 --- a/Marlin/src/HAL/TEENSY40_41/timers.cpp +++ b/Marlin/src/HAL/TEENSY40_41/timers.cpp @@ -30,7 +30,7 @@ void HAL_timer_start(const uint8_t timer_num, const uint32_t frequency) { switch (timer_num) { - case 0: + case MF_TIMER_STEP: CCM_CSCMR1 &= ~CCM_CSCMR1_PERCLK_CLK_SEL; // turn off 24mhz mode CCM_CCGR1 |= CCM_CCGR1_GPT1_BUS(CCM_CCGR_ON); @@ -48,7 +48,7 @@ void HAL_timer_start(const uint8_t timer_num, const uint32_t frequency) { attachInterruptVector(IRQ_GPT1, &stepTC_Handler); NVIC_SET_PRIORITY(IRQ_GPT1, 16); break; - case 1: + case MF_TIMER_TEMP: CCM_CSCMR1 &= ~CCM_CSCMR1_PERCLK_CLK_SEL; // turn off 24mhz mode CCM_CCGR0 |= CCM_CCGR0_GPT2_BUS(CCM_CCGR_ON); @@ -71,19 +71,15 @@ void HAL_timer_start(const uint8_t timer_num, const uint32_t frequency) { void HAL_timer_enable_interrupt(const uint8_t timer_num) { switch (timer_num) { - case 0: - NVIC_ENABLE_IRQ(IRQ_GPT1); - break; - case 1: - NVIC_ENABLE_IRQ(IRQ_GPT2); - break; + case MF_TIMER_STEP: NVIC_ENABLE_IRQ(IRQ_GPT1); break; + case MF_TIMER_TEMP: NVIC_ENABLE_IRQ(IRQ_GPT2); break; } } void HAL_timer_disable_interrupt(const uint8_t timer_num) { switch (timer_num) { - case 0: NVIC_DISABLE_IRQ(IRQ_GPT1); break; - case 1: NVIC_DISABLE_IRQ(IRQ_GPT2); break; + case MF_TIMER_STEP: NVIC_DISABLE_IRQ(IRQ_GPT1); break; + case MF_TIMER_TEMP: NVIC_DISABLE_IRQ(IRQ_GPT2); break; } // We NEED memory barriers to ensure Interrupts are actually disabled! @@ -93,20 +89,16 @@ void HAL_timer_disable_interrupt(const uint8_t timer_num) { bool HAL_timer_interrupt_enabled(const uint8_t timer_num) { switch (timer_num) { - case 0: return (NVIC_IS_ENABLED(IRQ_GPT1)); - case 1: return (NVIC_IS_ENABLED(IRQ_GPT2)); + case MF_TIMER_STEP: return (NVIC_IS_ENABLED(IRQ_GPT1)); + case MF_TIMER_TEMP: return (NVIC_IS_ENABLED(IRQ_GPT2)); } return false; } void HAL_timer_isr_prologue(const uint8_t timer_num) { switch (timer_num) { - case 0: - GPT1_SR = GPT_IR_OF1IE; // clear OF3 bit - break; - case 1: - GPT2_SR = GPT_IR_OF1IE; // clear OF3 bit - break; + case MF_TIMER_STEP: GPT1_SR = GPT_IR_OF1IE; break; // clear OF3 bit + case MF_TIMER_TEMP: GPT2_SR = GPT_IR_OF1IE; break; // clear OF3 bit } asm volatile("dsb"); } diff --git a/Marlin/src/HAL/TEENSY40_41/timers.h b/Marlin/src/HAL/TEENSY40_41/timers.h index 556333d7f408..77fe0953d3bd 100644 --- a/Marlin/src/HAL/TEENSY40_41/timers.h +++ b/Marlin/src/HAL/TEENSY40_41/timers.h @@ -43,14 +43,14 @@ typedef uint32_t hal_timer_t; #define GPT1_TIMER_RATE (GPT_TIMER_RATE / GPT1_TIMER_PRESCALE) // 75MHz #define GPT2_TIMER_RATE (GPT_TIMER_RATE / GPT2_TIMER_PRESCALE) // 15MHz -#ifndef STEP_TIMER_NUM - #define STEP_TIMER_NUM 0 // Timer Index for Stepper +#ifndef MF_TIMER_STEP + #define MF_TIMER_STEP 0 // Timer Index for Stepper #endif -#ifndef PULSE_TIMER_NUM - #define PULSE_TIMER_NUM STEP_TIMER_NUM +#ifndef MF_TIMER_PULSE + #define MF_TIMER_PULSE MF_TIMER_STEP #endif -#ifndef TEMP_TIMER_NUM - #define TEMP_TIMER_NUM 1 // Timer Index for Temperature +#ifndef MF_TIMER_TEMP + #define MF_TIMER_TEMP 1 // Timer Index for Temperature #endif #define TEMP_TIMER_RATE 1000000 @@ -64,12 +64,12 @@ typedef uint32_t hal_timer_t; #define PULSE_TIMER_PRESCALE STEPPER_TIMER_PRESCALE #define PULSE_TIMER_TICKS_PER_US STEPPER_TIMER_TICKS_PER_US -#define ENABLE_STEPPER_DRIVER_INTERRUPT() HAL_timer_enable_interrupt(STEP_TIMER_NUM) -#define DISABLE_STEPPER_DRIVER_INTERRUPT() HAL_timer_disable_interrupt(STEP_TIMER_NUM) -#define STEPPER_ISR_ENABLED() HAL_timer_interrupt_enabled(STEP_TIMER_NUM) +#define ENABLE_STEPPER_DRIVER_INTERRUPT() HAL_timer_enable_interrupt(MF_TIMER_STEP) +#define DISABLE_STEPPER_DRIVER_INTERRUPT() HAL_timer_disable_interrupt(MF_TIMER_STEP) +#define STEPPER_ISR_ENABLED() HAL_timer_interrupt_enabled(MF_TIMER_STEP) -#define ENABLE_TEMPERATURE_INTERRUPT() HAL_timer_enable_interrupt(TEMP_TIMER_NUM) -#define DISABLE_TEMPERATURE_INTERRUPT() HAL_timer_disable_interrupt(TEMP_TIMER_NUM) +#define ENABLE_TEMPERATURE_INTERRUPT() HAL_timer_enable_interrupt(MF_TIMER_TEMP) +#define DISABLE_TEMPERATURE_INTERRUPT() HAL_timer_disable_interrupt(MF_TIMER_TEMP) #ifndef HAL_STEP_TIMER_ISR #define HAL_STEP_TIMER_ISR() extern "C" void stepTC_Handler() // GPT1_Handler() @@ -87,27 +87,23 @@ void HAL_timer_start(const uint8_t timer_num, const uint32_t frequency); FORCE_INLINE static void HAL_timer_set_compare(const uint8_t timer_num, const hal_timer_t compare) { switch (timer_num) { - case 0: - GPT1_OCR1 = compare - 1; - break; - case 1: - GPT2_OCR1 = compare - 1; - break; + case MF_TIMER_STEP: GPT1_OCR1 = compare - 1; break; + case MF_TIMER_TEMP: GPT2_OCR1 = compare - 1; break; } } FORCE_INLINE static hal_timer_t HAL_timer_get_compare(const uint8_t timer_num) { switch (timer_num) { - case 0: return GPT1_OCR1; - case 1: return GPT2_OCR1; + case MF_TIMER_STEP: return GPT1_OCR1; + case MF_TIMER_TEMP: return GPT2_OCR1; } return 0; } FORCE_INLINE static hal_timer_t HAL_timer_get_count(const uint8_t timer_num) { switch (timer_num) { - case 0: return GPT1_CNT; - case 1: return GPT2_CNT; + case MF_TIMER_STEP: return GPT1_CNT; + case MF_TIMER_TEMP: return GPT2_CNT; } return 0; } @@ -118,4 +114,4 @@ bool HAL_timer_interrupt_enabled(const uint8_t timer_num); void HAL_timer_isr_prologue(const uint8_t timer_num); //void HAL_timer_isr_epilogue(const uint8_t timer_num) {} -#define HAL_timer_isr_epilogue(TIMER_NUM) +#define HAL_timer_isr_epilogue(T) NOOP diff --git a/Marlin/src/HAL/platforms.h b/Marlin/src/HAL/platforms.h index e0617bdf7fa5..28fe28e1094f 100644 --- a/Marlin/src/HAL/platforms.h +++ b/Marlin/src/HAL/platforms.h @@ -38,11 +38,16 @@ #elif defined(__STM32F1__) || defined(TARGET_STM32F1) #define HAL_PATH(PATH, NAME) XSTR(PATH/STM32F1/NAME) #elif defined(ARDUINO_ARCH_STM32) + #ifndef HAL_STM32 + #define HAL_STM32 + #endif #define HAL_PATH(PATH, NAME) XSTR(PATH/STM32/NAME) #elif defined(ARDUINO_ARCH_ESP32) #define HAL_PATH(PATH, NAME) XSTR(PATH/ESP32/NAME) #elif defined(__PLAT_LINUX__) #define HAL_PATH(PATH, NAME) XSTR(PATH/LINUX/NAME) +#elif defined(__PLAT_NATIVE_SIM__) + #define HAL_PATH(PATH, NAME) XSTR(PATH/NATIVE_SIM/NAME) #elif defined(__SAMD51__) #define HAL_PATH(PATH, NAME) XSTR(PATH/SAMD51/NAME) #else diff --git a/Marlin/src/HAL/shared/Delay.cpp b/Marlin/src/HAL/shared/Delay.cpp index 129698fd306d..c64376d25d9a 100644 --- a/Marlin/src/HAL/shared/Delay.cpp +++ b/Marlin/src/HAL/shared/Delay.cpp @@ -19,6 +19,7 @@ * along with this program. If not, see . * */ + #include "Delay.h" #include "../../inc/MarlinConfig.h" @@ -107,59 +108,61 @@ #if ENABLED(MARLIN_DEV_MODE) void dump_delay_accuracy_check() { - auto report_call_time = [](PGM_P const name, PGM_P const unit, const uint32_t cycles, const uint32_t total, const bool do_flush=true) { + auto report_call_time = [](FSTR_P const name, FSTR_P const unit, const uint32_t cycles, const uint32_t total, const bool do_flush=true) { SERIAL_ECHOPGM("Calling "); - SERIAL_ECHOPGM_P(name); - SERIAL_ECHOLNPAIR(" for ", cycles); - SERIAL_ECHOPGM_P(unit); - SERIAL_ECHOLNPAIR(" took: ", total); - SERIAL_ECHOPGM_P(unit); + SERIAL_ECHOF(name); + SERIAL_ECHOLNPGM(" for ", cycles); + SERIAL_ECHOF(unit); + SERIAL_ECHOLNPGM(" took: ", total); + SERIAL_CHAR(' '); + SERIAL_ECHOF(unit); if (do_flush) SERIAL_FLUSHTX(); }; uint32_t s, e; - SERIAL_ECHOLNPAIR("Computed delay calibration value: ", ASM_CYCLES_PER_ITERATION); + SERIAL_ECHOLNPGM("Computed delay calibration value: ", ASM_CYCLES_PER_ITERATION); SERIAL_FLUSH(); // Display the results of the calibration above constexpr uint32_t testValues[] = { 1, 5, 10, 20, 50, 100, 150, 200, 350, 500, 750, 1000 }; for (auto i : testValues) { s = micros(); DELAY_US(i); e = micros(); - report_call_time(PSTR("delay"), PSTR("us"), i, e - s); + report_call_time(F("delay"), F("us"), i, e - s); } if (HW_REG(_DWT_CTRL)) { + static FSTR_P cyc = F("cycles"); + static FSTR_P dcd = F("DELAY_CYCLES directly "); + for (auto i : testValues) { s = HW_REG(_DWT_CYCCNT); DELAY_CYCLES(i); e = HW_REG(_DWT_CYCCNT); - report_call_time(PSTR("runtime delay"), PSTR("cycles"), i, e - s); + report_call_time(F("runtime delay"), cyc, i, e - s); } // Measure the delay to call a real function compared to a function pointer s = HW_REG(_DWT_CYCCNT); delay_dwt(1); e = HW_REG(_DWT_CYCCNT); - report_call_time(PSTR("delay_dwt"), PSTR("cycles"), 1, e - s); - - static PGMSTR(dcd, "DELAY_CYCLES directly "); + report_call_time(F("delay_dwt"), cyc, 1, e - s); s = HW_REG(_DWT_CYCCNT); DELAY_CYCLES( 1); e = HW_REG(_DWT_CYCCNT); - report_call_time(dcd, PSTR("cycles"), 1, e - s, false); + report_call_time(dcd, cyc, 1, e - s, false); s = HW_REG(_DWT_CYCCNT); DELAY_CYCLES( 5); e = HW_REG(_DWT_CYCCNT); - report_call_time(dcd, PSTR("cycles"), 5, e - s, false); + report_call_time(dcd, cyc, 5, e - s, false); s = HW_REG(_DWT_CYCCNT); DELAY_CYCLES(10); e = HW_REG(_DWT_CYCCNT); - report_call_time(dcd, PSTR("cycles"), 10, e - s, false); + report_call_time(dcd, cyc, 10, e - s, false); s = HW_REG(_DWT_CYCCNT); DELAY_CYCLES(20); e = HW_REG(_DWT_CYCCNT); - report_call_time(dcd, PSTR("cycles"), 20, e - s, false); + report_call_time(dcd, cyc, 20, e - s, false); s = HW_REG(_DWT_CYCCNT); DELAY_CYCLES(50); e = HW_REG(_DWT_CYCCNT); - report_call_time(dcd, PSTR("cycles"), 50, e - s, false); + report_call_time(dcd, cyc, 50, e - s, false); s = HW_REG(_DWT_CYCCNT); DELAY_CYCLES(100); e = HW_REG(_DWT_CYCCNT); - report_call_time(dcd, PSTR("cycles"), 100, e - s, false); + report_call_time(dcd, cyc, 100, e - s, false); s = HW_REG(_DWT_CYCCNT); DELAY_CYCLES(200); e = HW_REG(_DWT_CYCCNT); - report_call_time(dcd, PSTR("cycles"), 200, e - s, false); + report_call_time(dcd, cyc, 200, e - s, false); } } #endif // MARLIN_DEV_MODE @@ -169,7 +172,7 @@ void calibrate_delay_loop() {} #if ENABLED(MARLIN_DEV_MODE) - void dump_delay_accuracy_check() { SERIAL_ECHOPGM_P(PSTR("N/A on this platform")); } + void dump_delay_accuracy_check() { SERIAL_ECHOPGM("N/A on this platform"); } #endif #endif diff --git a/Marlin/src/HAL/shared/Delay.h b/Marlin/src/HAL/shared/Delay.h index dc1f158b44ae..a6795a78eaf4 100644 --- a/Marlin/src/HAL/shared/Delay.h +++ b/Marlin/src/HAL/shared/Delay.h @@ -92,53 +92,83 @@ void calibrate_delay_loop(); #define DELAY_CYCLES(X) do { SmartDelay _smrtdly_X(X); } while(0) + #if GCC_VERSION <= 70000 + #define DELAY_CYCLES_VAR(X) DelayCycleFnc(X) + #else + #define DELAY_CYCLES_VAR DELAY_CYCLES + #endif + // For delay in microseconds, no smart delay selection is required, directly call the delay function // Teensy compiler is too old and does not accept smart delay compile-time / run-time selection correctly #define DELAY_US(x) DelayCycleFnc((x) * ((F_CPU) / 1000000UL)) #elif defined(__AVR__) - - #define nop() __asm__ __volatile__("nop;\n\t":::) - - FORCE_INLINE static void __delay_4cycles(uint8_t cy) { - __asm__ __volatile__( - L("1") - A("dec %[cnt]") - A("nop") - A("brne 1b") - : [cnt] "+r"(cy) // output: +r means input+output - : // input: - : "cc" // clobbers: - ); + FORCE_INLINE static void __delay_up_to_3c(uint8_t cycles) { + switch (cycles) { + case 3: + __asm__ __volatile__(A("RJMP .+0") A("NOP")); + break; + case 2: + __asm__ __volatile__(A("RJMP .+0")); + break; + case 1: + __asm__ __volatile__(A("NOP")); + break; + } } // Delay in cycles - FORCE_INLINE static void DELAY_CYCLES(uint16_t x) { - - if (__builtin_constant_p(x)) { - #define MAXNOPS 4 - - if (x <= (MAXNOPS)) { - switch (x) { case 4: nop(); case 3: nop(); case 2: nop(); case 1: nop(); } + FORCE_INLINE static void DELAY_CYCLES(uint16_t cycles) { + if (__builtin_constant_p(cycles)) { + if (cycles <= 3) { + __delay_up_to_3c(cycles); + } + else if (cycles == 4) { + __delay_up_to_3c(2); + __delay_up_to_3c(2); } else { - const uint32_t rem = (x) % (MAXNOPS); - switch (rem) { case 3: nop(); case 2: nop(); case 1: nop(); } - if ((x = (x) / (MAXNOPS))) - __delay_4cycles(x); // if need more then 4 nop loop is more optimal + cycles -= 1 + 4; // Compensate for the first LDI (1) and the first round (4) + __delay_up_to_3c(cycles % 4); + + cycles /= 4; + // The following code burns [1 + 4 * (rounds+1)] cycles + uint16_t dummy; + __asm__ __volatile__( + // "manually" load counter from constants, otherwise the compiler may optimize this part away + A("LDI %A[rounds], %[l]") // 1c + A("LDI %B[rounds], %[h]") // 1c (compensating the non branching BRCC) + L("1") + A("SBIW %[rounds], 1") // 2c + A("BRCC 1b") // 2c when branching, else 1c (end of loop) + : // Outputs ... + [rounds] "=w" (dummy) // Restrict to a wo (=) 16 bit register pair (w) + : // Inputs ... + [l] "M" (cycles%256), // Restrict to 0..255 constant (M) + [h] "M" (cycles/256) // Restrict to 0..255 constant (M) + :// Clobbers ... + "cc" // Indicate we are modifying flags like Carry (cc) + ); } - - #undef MAXNOPS } - else if ((x >>= 2)) - __delay_4cycles(x); + else { + __asm__ __volatile__( + L("1") + A("SBIW %[cycles], 4") // 2c + A("BRCC 1b") // 2c when branching, else 1c (end of loop) + : [cycles] "+w" (cycles) // output: Restrict to a rw (+) 16 bit register pair (w) + : // input: - + : "cc" // clobbers: We are modifying flags like Carry (cc) + ); + } } - #undef nop // Delay in microseconds #define DELAY_US(x) DELAY_CYCLES((x) * ((F_CPU) / 1000000UL)) -#elif defined(__PLAT_LINUX__) || defined(ESP32) + #define DELAY_CYCLES_VAR DELAY_CYCLES + +#elif defined(ESP32) || defined(__PLAT_LINUX__) || defined(__PLAT_NATIVE_SIM__) // DELAY_CYCLES specified inside platform @@ -150,8 +180,40 @@ void calibrate_delay_loop(); #endif -// Delay in nanoseconds -#define DELAY_NS(x) DELAY_CYCLES((x) * ((F_CPU) / 1000000UL) / 1000UL) +/************************************************************** + * Delay in nanoseconds. Requires the F_CPU macro. + * These macros follow avr-libc delay conventions. + * + * For AVR there are three possible operation modes, due to its + * slower clock speeds and thus coarser delay resolution. For + * example, when F_CPU = 16000000 the resolution is 62.5ns. + * + * Round up (default) + * Round up the delay according to the CPU clock resolution. + * e.g., 100 will give a delay of 2 cycles (125ns). + * + * Round down (DELAY_NS_ROUND_DOWN) + * Round down the delay according to the CPU clock resolution. + * e.g., 100 will be rounded down to 1 cycle (62.5ns). + * + * Nearest (DELAY_NS_ROUND_CLOSEST) + * Round the delay to the nearest number of clock cycles. + * e.g., 165 will be rounded up to 3 cycles (187.5ns) because + * it's closer to the requested delay than 2 cycle (125ns). + */ +#ifndef __AVR__ + #undef DELAY_NS_ROUND_DOWN + #undef DELAY_NS_ROUND_CLOSEST +#endif +#if ENABLED(DELAY_NS_ROUND_DOWN) + #define _NS_TO_CYCLES(x) ( (x) * ((F_CPU) / 1000000UL) / 1000UL) // floor +#elif ENABLED(DELAY_NS_ROUND_CLOSEST) + #define _NS_TO_CYCLES(x) (((x) * ((F_CPU) / 1000000UL) + 500) / 1000UL) // round +#else + #define _NS_TO_CYCLES(x) (((x) * ((F_CPU) / 1000000UL) + 999) / 1000UL) // "ceil" +#endif +#define DELAY_NS(x) DELAY_CYCLES(_NS_TO_CYCLES(x)) +#define DELAY_NS_VAR(x) DELAY_CYCLES_VAR(_NS_TO_CYCLES(x)) diff --git a/Marlin/src/HAL/shared/HAL.cpp b/Marlin/src/HAL/shared/HAL.cpp new file mode 100644 index 000000000000..4d92aedd9a1f --- /dev/null +++ b/Marlin/src/HAL/shared/HAL.cpp @@ -0,0 +1,36 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2022 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + * + */ + +/** + * HAL/shared/HAL.cpp + */ + +#include "../../inc/MarlinConfig.h" + +MarlinHAL hal; + +#if ENABLED(SOFT_RESET_VIA_SERIAL) + + // Global for use by e_parser.h + void HAL_reboot() { hal.reboot(); } + +#endif diff --git a/Marlin/src/HAL/shared/HAL_MinSerial.h b/Marlin/src/HAL/shared/HAL_MinSerial.h index 04643c1ab36d..3089b8aa0618 100644 --- a/Marlin/src/HAL/shared/HAL_MinSerial.h +++ b/Marlin/src/HAL/shared/HAL_MinSerial.h @@ -42,7 +42,7 @@ struct MinSerial { HAL_min_serial_out(ch); } // Send String through UART - static void TX(const char* s) { while (*s) TX(*s++); } + static void TX(const char *s) { while (*s) TX(*s++); } // Send a digit through UART static void TXDigit(uint32_t d) { if (d < 10) TX((char)(d+'0')); diff --git a/Marlin/src/HAL/shared/HAL_SPI.h b/Marlin/src/HAL/shared/HAL_SPI.h index 59af5548064c..6611f9ec4e0f 100644 --- a/Marlin/src/HAL/shared/HAL_SPI.h +++ b/Marlin/src/HAL/shared/HAL_SPI.h @@ -71,10 +71,10 @@ void spiSend(uint8_t b); uint8_t spiRec(); // Read from SPI into buffer -void spiRead(uint8_t* buf, uint16_t nbyte); +void spiRead(uint8_t *buf, uint16_t nbyte); // Write token and then write from 512 byte buffer to SPI (for SD card) -void spiSendBlock(uint8_t token, const uint8_t* buf); +void spiSendBlock(uint8_t token, const uint8_t *buf); // Begin SPI transaction, set clock, bit order, data mode void spiBeginTransaction(uint32_t spiClock, uint8_t bitOrder, uint8_t dataMode); @@ -87,7 +87,7 @@ void spiBeginTransaction(uint32_t spiClock, uint8_t bitOrder, uint8_t dataMode); void spiSend(uint32_t chan, byte b); // Write buffer to specified SPI channel -void spiSend(uint32_t chan, const uint8_t* buf, size_t n); +void spiSend(uint32_t chan, const uint8_t *buf, size_t n); // Read single byte from specified SPI channel uint8_t spiRec(uint32_t chan); diff --git a/Marlin/src/HAL/shared/HAL_spi_L6470.cpp b/Marlin/src/HAL/shared/HAL_spi_L6470.cpp index bd85dbe7bd7e..5d4ce89b2748 100644 --- a/Marlin/src/HAL/shared/HAL_spi_L6470.cpp +++ b/Marlin/src/HAL/shared/HAL_spi_L6470.cpp @@ -92,9 +92,9 @@ uint8_t L64XX_Marlin::transfer_single(uint8_t data, int16_t ss_pin) { // First device in chain has data sent last extDigitalWrite(ss_pin, LOW); - DISABLE_ISRS(); // Disable interrupts during SPI transfer (can't allow partial command to chips) + hal.isr_off(); // Disable interrupts during SPI transfer (can't allow partial command to chips) const uint8_t data_out = L6470_SpiTransfer_Mode_3(data); - ENABLE_ISRS(); // Enable interrupts + hal.isr_on(); // Enable interrupts extDigitalWrite(ss_pin, HIGH); return data_out; @@ -107,9 +107,9 @@ uint8_t L64XX_Marlin::transfer_chain(uint8_t data, int16_t ss_pin, uint8_t chain extDigitalWrite(ss_pin, LOW); for (uint8_t i = L64XX::chain[0]; !L64xxManager.spi_abort && i >= 1; i--) { // Send data unless aborted - DISABLE_ISRS(); // Disable interrupts during SPI transfer (can't allow partial command to chips) + hal.isr_off(); // Disable interrupts during SPI transfer (can't allow partial command to chips) const uint8_t temp = L6470_SpiTransfer_Mode_3(uint8_t(i == chain_position ? data : dSPIN_NOP)); - ENABLE_ISRS(); // Enable interrupts + hal.isr_on(); // Enable interrupts if (i == chain_position) data_out = temp; } diff --git a/Marlin/src/HAL/shared/Marduino.h b/Marlin/src/HAL/shared/Marduino.h index d0ee6ecc9d43..0e2a021a3c4e 100644 --- a/Marlin/src/HAL/shared/Marduino.h +++ b/Marlin/src/HAL/shared/Marduino.h @@ -39,7 +39,7 @@ #define DISABLED(V...) DO(DIS,&&,V) #undef _BV -#define _BV(b) (1UL << (b)) +#define _BV(b) (1 << (b)) #ifndef SBI #define SBI(A,B) (A |= _BV(B)) #endif @@ -82,4 +82,15 @@ #define UNUSED(x) ((void)(x)) #endif +#ifndef FORCE_INLINE + #define FORCE_INLINE __attribute__((always_inline)) inline +#endif + #include "progmem.h" + +class __FlashStringHelper; +typedef const __FlashStringHelper* FSTR_P; +#ifndef FPSTR + #define FPSTR(S) (reinterpret_cast(S)) +#endif +#define FTOP(S) (reinterpret_cast(S)) diff --git a/Marlin/src/HAL/shared/backtrace/backtrace.cpp b/Marlin/src/HAL/shared/backtrace/backtrace.cpp index 8320f475096a..ad88de8385ac 100644 --- a/Marlin/src/HAL/shared/backtrace/backtrace.cpp +++ b/Marlin/src/HAL/shared/backtrace/backtrace.cpp @@ -29,7 +29,7 @@ #include // Dump a backtrace entry -static bool UnwReportOut(void* ctx, const UnwReport* bte) { +static bool UnwReportOut(void *ctx, const UnwReport *bte) { int *p = (int*)ctx; (*p)++; @@ -44,7 +44,7 @@ static bool UnwReportOut(void* ctx, const UnwReport* bte) { } #ifdef UNW_DEBUG - void UnwPrintf(const char* format, ...) { + void UnwPrintf(const char *format, ...) { char dest[256]; va_list argptr; va_start(argptr, format); diff --git a/Marlin/src/HAL/shared/backtrace/unwarm.h b/Marlin/src/HAL/shared/backtrace/unwarm.h index 86dc98c073e1..edae90650e29 100644 --- a/Marlin/src/HAL/shared/backtrace/unwarm.h +++ b/Marlin/src/HAL/shared/backtrace/unwarm.h @@ -4,7 +4,7 @@ * This program is PUBLIC DOMAIN. * This means that there is no copyright and anyone is able to take a copy * for free and use it as they wish, with or without modifications, and in - * any context, commerically or otherwise. The only limitation is that I + * any context, commercially or otherwise. The only limitation is that I * don't guarantee that the software is fit for any purpose or accept any * liability for its use or misuse - this software is without warranty. *************************************************************************** diff --git a/Marlin/src/HAL/shared/backtrace/unwarmbytab.h b/Marlin/src/HAL/shared/backtrace/unwarmbytab.h index e2f80db2a5b8..53aeca259439 100644 --- a/Marlin/src/HAL/shared/backtrace/unwarmbytab.h +++ b/Marlin/src/HAL/shared/backtrace/unwarmbytab.h @@ -5,7 +5,7 @@ * This program is PUBLIC DOMAIN. * This means that there is no copyright and anyone is able to take a copy * for free and use it as they wish, with or without modifications, and in - * any context, commerically or otherwise. The only limitation is that I + * any context, commercially or otherwise. The only limitation is that I * don't guarantee that the software is fit for any purpose or accept any * liability for its use or misuse - this software is without warranty. *************************************************************************** diff --git a/Marlin/src/HAL/shared/backtrace/unwarmmem.cpp b/Marlin/src/HAL/shared/backtrace/unwarmmem.cpp index a40d8540ec38..24023200e155 100644 --- a/Marlin/src/HAL/shared/backtrace/unwarmmem.cpp +++ b/Marlin/src/HAL/shared/backtrace/unwarmmem.cpp @@ -5,7 +5,7 @@ * This program is PUBLIC DOMAIN. * This means that there is no copyright and anyone is able to take a copy * for free and use it as they wish, with or without modifications, and in - * any context, commerically or otherwise. The only limitation is that I + * any context, commercially or otherwise. The only limitation is that I * don't guarantee that the software is fit for any purpose or accept any * liability for its use or misuse - this software is without warranty. *************************************************************************** diff --git a/Marlin/src/HAL/shared/backtrace/unwarmmem.h b/Marlin/src/HAL/shared/backtrace/unwarmmem.h index 1340bbdf0a25..eb4579a76107 100644 --- a/Marlin/src/HAL/shared/backtrace/unwarmmem.h +++ b/Marlin/src/HAL/shared/backtrace/unwarmmem.h @@ -5,7 +5,7 @@ * This program is PUBLIC DOMAIN. * This means that there is no copyright and anyone is able to take a copy * for free and use it as they wish, with or without modifications, and in - * any context, commerically or otherwise. The only limitation is that I + * any context, commercially or otherwise. The only limitation is that I * don't guarantee that the software is fit for any purpose or accept any * liability for its use or misuse - this software is without warranty. *************************************************************************** diff --git a/Marlin/src/HAL/shared/backtrace/unwinder.cpp b/Marlin/src/HAL/shared/backtrace/unwinder.cpp index 0f88e2a7f75b..aedfa2404d65 100644 --- a/Marlin/src/HAL/shared/backtrace/unwinder.cpp +++ b/Marlin/src/HAL/shared/backtrace/unwinder.cpp @@ -28,7 +28,7 @@ extern "C" const UnwTabEntry __exidx_end[]; // Detect if unwind information is present or not static int HasUnwindTableInfo() { - // > 16 because there are default entries we can't supress + // > 16 because there are default entries we can't suppress return ((char*)(&__exidx_end) - (char*)(&__exidx_start)) > 16 ? 1 : 0; } diff --git a/Marlin/src/HAL/shared/backtrace/unwinder.h b/Marlin/src/HAL/shared/backtrace/unwinder.h index 157808d540da..9280e2f36e89 100644 --- a/Marlin/src/HAL/shared/backtrace/unwinder.h +++ b/Marlin/src/HAL/shared/backtrace/unwinder.h @@ -5,7 +5,7 @@ * This program is PUBLIC DOMAIN. * This means that there is no copyright and anyone is able to take a copy * for free and use it as they wish, with or without modifications, and in - * any context, commerically or otherwise. The only limitation is that I + * any context, commercially or otherwise. The only limitation is that I * don't guarantee that the software is fit for any purpose or accept any * liability for its use or misuse - this software is without warranty. **************************************************************************/ @@ -114,7 +114,7 @@ typedef struct { * report function maybe called again in future. If false is returned, * unwinding will stop with UnwindStart() returning UNWIND_TRUNCATED. */ -typedef bool (*UnwindReportFunc)(void* data, const UnwReport* bte); +typedef bool (*UnwindReportFunc)(void *data, const UnwReport *bte); /** Structure that holds memory callback function pointers. */ diff --git a/Marlin/src/HAL/shared/backtrace/unwmemaccess.cpp b/Marlin/src/HAL/shared/backtrace/unwmemaccess.cpp index 2bde1e208d95..a4151b38c20e 100644 --- a/Marlin/src/HAL/shared/backtrace/unwmemaccess.cpp +++ b/Marlin/src/HAL/shared/backtrace/unwmemaccess.cpp @@ -41,7 +41,7 @@ #define START_FLASH_ADDR 0x00000000 #define END_FLASH_ADDR 0x00080000 -#elif defined(__STM32F1__) || defined(STM32F1xx) || defined(STM32F0xx) +#elif defined(__STM32F1__) || defined(STM32F1xx) || defined(STM32F0xx) || defined(STM32G0xx) // For STM32F103ZET6/STM32F103VET6/STM32F0xx // SRAM (0x20000000 - 0x20010000) (64kb) diff --git a/Marlin/src/HAL/shared/backtrace/unwmemaccess.h b/Marlin/src/HAL/shared/backtrace/unwmemaccess.h index 562ab3f05da9..b911e343dc46 100644 --- a/Marlin/src/HAL/shared/backtrace/unwmemaccess.h +++ b/Marlin/src/HAL/shared/backtrace/unwmemaccess.h @@ -5,7 +5,7 @@ * This program is PUBLIC DOMAIN. * This means that there is no copyright and anyone is able to take a copy * for free and use it as they wish, with or without modifications, and in - * any context, commerically or otherwise. The only limitation is that I + * any context, commercially or otherwise. The only limitation is that I * don't guarantee that the software is fit for any purpose or accept any * liability for its use or misuse - this software is without warranty. *************************************************************************** diff --git a/Marlin/src/HAL/shared/cpu_exception/exception_arm.cpp b/Marlin/src/HAL/shared/cpu_exception/exception_arm.cpp index ae9600038a3f..a106ed2b05e5 100644 --- a/Marlin/src/HAL/shared/cpu_exception/exception_arm.cpp +++ b/Marlin/src/HAL/shared/cpu_exception/exception_arm.cpp @@ -101,7 +101,7 @@ struct __attribute__((packed)) ContextSavedFrame { uint32_t ELR; }; -#if DISABLED(STM32F0xx) +#if NONE(STM32F0xx, STM32G0xx) extern "C" __attribute__((naked)) void CommonHandler_ASM() { __asm__ __volatile__ ( @@ -320,9 +320,9 @@ void hook_cpu_exceptions() { // So we'll simply mask the top 8 bits of the first handler as an hint of being in the flash or not -that's poor and will // probably break if the flash happens to be more than 128MB, but in this case, we are not magician, we need help from outside. - unsigned long * vecAddr = (unsigned long*)get_vtor(); - SERIAL_ECHO("Vector table addr: "); - SERIAL_PRINTLN(get_vtor(), HEX); + unsigned long *vecAddr = (unsigned long*)get_vtor(); + SERIAL_ECHOPGM("Vector table addr: "); + SERIAL_PRINTLN(get_vtor(), PrintBase::Hex); #ifdef VECTOR_TABLE_SIZE uint32_t vec_size = VECTOR_TABLE_SIZE; @@ -345,11 +345,11 @@ void hook_cpu_exceptions() { // We failed to find a valid vector table size, let's abort hooking up if (vec_size == VECTOR_TABLE_SENTINEL) return; // Poor method that's wasting RAM here, but allocating with malloc and alignment would be worst - // 128 bytes alignement is required for writing the VTOR register + // 128 bytes alignment is required for writing the VTOR register alignas(128) static unsigned long vectable[VECTOR_TABLE_SENTINEL]; - SERIAL_ECHO("Detected vector table size: "); - SERIAL_PRINTLN(vec_size, HEX); + SERIAL_ECHOPGM("Detected vector table size: "); + SERIAL_PRINTLN(vec_size, PrintBase::Hex); #endif uint32_t defaultFaultHandler = vecAddr[(unsigned)7]; @@ -372,7 +372,7 @@ void hook_cpu_exceptions() { HW_REG(0xE000ED08) = (unsigned long)vectable | _BV32(29); // 29th bit is for telling the CPU the table is now in SRAM (should be present already) - SERIAL_ECHOLN("Installed fault handlers"); + SERIAL_ECHOLNPGM("Installed fault handlers"); #endif } diff --git a/Marlin/src/HAL/shared/eeprom_api.h b/Marlin/src/HAL/shared/eeprom_api.h index 6445f7a4aae4..cd744f82dc79 100644 --- a/Marlin/src/HAL/shared/eeprom_api.h +++ b/Marlin/src/HAL/shared/eeprom_api.h @@ -45,11 +45,11 @@ class PersistentStore { // Read one or more bytes of data and update the CRC // Return 'true' on read error - static bool read_data(int &pos, uint8_t* value, size_t size, uint16_t *crc, const bool writing=true); + static bool read_data(int &pos, uint8_t *value, size_t size, uint16_t *crc, const bool writing=true); // Write one or more bytes of data // Return 'true' on write error - static inline bool write_data(const int pos, const uint8_t* value, const size_t size=sizeof(uint8_t)) { + static bool write_data(const int pos, const uint8_t *value, const size_t size=sizeof(uint8_t)) { int data_pos = pos; uint16_t crc = 0; return write_data(data_pos, value, size, &crc); @@ -57,11 +57,11 @@ class PersistentStore { // Write a single byte of data // Return 'true' on write error - static inline bool write_data(const int pos, const uint8_t value) { return write_data(pos, &value); } + static bool write_data(const int pos, const uint8_t value) { return write_data(pos, &value); } // Read one or more bytes of data // Return 'true' on read error - static inline bool read_data(const int pos, uint8_t* value, const size_t size=1) { + static bool read_data(const int pos, uint8_t *value, const size_t size=1) { int data_pos = pos; uint16_t crc = 0; return read_data(data_pos, value, size, &crc); diff --git a/Marlin/src/HAL/shared/eeprom_if.h b/Marlin/src/HAL/shared/eeprom_if.h index e44da801dfbf..e496de2a03c6 100644 --- a/Marlin/src/HAL/shared/eeprom_if.h +++ b/Marlin/src/HAL/shared/eeprom_if.h @@ -25,5 +25,5 @@ // EEPROM // void eeprom_init(); -void eeprom_write_byte(uint8_t *pos, unsigned char value); +void eeprom_write_byte(uint8_t *pos, uint8_t value); uint8_t eeprom_read_byte(uint8_t *pos); diff --git a/Marlin/src/HAL/shared/eeprom_if_i2c.cpp b/Marlin/src/HAL/shared/eeprom_if_i2c.cpp index da70af277246..6b559e234b4c 100644 --- a/Marlin/src/HAL/shared/eeprom_if_i2c.cpp +++ b/Marlin/src/HAL/shared/eeprom_if_i2c.cpp @@ -30,11 +30,17 @@ #if ENABLED(I2C_EEPROM) #include "eeprom_if.h" -#include + +#if ENABLED(SOFT_I2C_EEPROM) + #include + SlowSoftWire Wire = SlowSoftWire(I2C_SDA_PIN, I2C_SCL_PIN, true); +#else + #include +#endif void eeprom_init() { Wire.begin( - #if PINS_EXIST(I2C_SCL, I2C_SDA) + #if PINS_EXIST(I2C_SCL, I2C_SDA) && DISABLED(SOFT_I2C_EEPROM) uint8_t(I2C_SDA_PIN), uint8_t(I2C_SCL_PIN) #endif ); @@ -55,12 +61,28 @@ static constexpr uint8_t eeprom_device_address = I2C_ADDRESS(EEPROM_DEVICE_ADDRE // Public functions // ------------------------ -void eeprom_write_byte(uint8_t *pos, unsigned char value) { +#define SMALL_EEPROM (MARLIN_EEPROM_SIZE <= 2048) + +// Combine Address high bits into the device address on <=16Kbit (2K) and >512Kbit (64K) EEPROMs. +// Note: MARLIN_EEPROM_SIZE is specified in bytes, whereas EEPROM model numbers refer to bits. +// e.g., The "16" in BL24C16 indicates a 16Kbit (2KB) size. +static uint8_t _eeprom_calc_device_address(uint8_t * const pos) { const unsigned eeprom_address = (unsigned)pos; + return (SMALL_EEPROM || MARLIN_EEPROM_SIZE > 65536) + ? uint8_t(eeprom_device_address | ((eeprom_address >> (SMALL_EEPROM ? 8 : 16)) & 0x07)) + : eeprom_device_address; +} - Wire.beginTransmission(eeprom_device_address); - Wire.write(int(eeprom_address >> 8)); // MSB - Wire.write(int(eeprom_address & 0xFF)); // LSB +static void _eeprom_begin(uint8_t * const pos) { + const unsigned eeprom_address = (unsigned)pos; + Wire.beginTransmission(_eeprom_calc_device_address(pos)); + if (!SMALL_EEPROM) + Wire.write(uint8_t((eeprom_address >> 8) & 0xFF)); // Address High, if needed + Wire.write(uint8_t(eeprom_address & 0xFF)); // Address Low +} + +void eeprom_write_byte(uint8_t *pos, uint8_t value) { + _eeprom_begin(pos); Wire.write(value); Wire.endTransmission(); @@ -70,13 +92,9 @@ void eeprom_write_byte(uint8_t *pos, unsigned char value) { } uint8_t eeprom_read_byte(uint8_t *pos) { - const unsigned eeprom_address = (unsigned)pos; - - Wire.beginTransmission(eeprom_device_address); - Wire.write(int(eeprom_address >> 8)); // MSB - Wire.write(int(eeprom_address & 0xFF)); // LSB + _eeprom_begin(pos); Wire.endTransmission(); - Wire.requestFrom(eeprom_device_address, (byte)1); + Wire.requestFrom(_eeprom_calc_device_address(pos), (byte)1); return Wire.available() ? Wire.read() : 0xFF; } diff --git a/Marlin/src/HAL/shared/eeprom_if_spi.cpp b/Marlin/src/HAL/shared/eeprom_if_spi.cpp index a341fef9de97..72c35addcb45 100644 --- a/Marlin/src/HAL/shared/eeprom_if_spi.cpp +++ b/Marlin/src/HAL/shared/eeprom_if_spi.cpp @@ -43,44 +43,41 @@ void eeprom_init() {} #define EEPROM_WRITE_DELAY 7 #endif -uint8_t eeprom_read_byte(uint8_t* pos) { - uint8_t v; - uint8_t eeprom_temp[3]; - - // set read location - // begin transmission from device - eeprom_temp[0] = CMD_READ; - eeprom_temp[1] = ((unsigned)pos>>8) & 0xFF; // addr High - eeprom_temp[2] = (unsigned)pos& 0xFF; // addr Low - WRITE(SPI_EEPROM1_CS, HIGH); - WRITE(SPI_EEPROM1_CS, LOW); +static void _eeprom_begin(uint8_t * const pos, const uint8_t cmd) { + const uint8_t eeprom_temp[3] = { + cmd, + (unsigned(pos) >> 8) & 0xFF, // Address High + unsigned(pos) & 0xFF // Address Low + }; + WRITE(SPI_EEPROM1_CS_PIN, HIGH); // Usually free already + WRITE(SPI_EEPROM1_CS_PIN, LOW); // Activate the Bus spiSend(SPI_CHAN_EEPROM1, eeprom_temp, 3); + // Leave the Bus in-use +} + +uint8_t eeprom_read_byte(uint8_t *pos) { + _eeprom_begin(pos, CMD_READ); // Set read location and begin transmission + + const uint8_t v = spiRec(SPI_CHAN_EEPROM1); // After READ a value sits on the Bus + + WRITE(SPI_EEPROM1_CS_PIN, HIGH); // Done with device - v = spiRec(SPI_CHAN_EEPROM1); - WRITE(SPI_EEPROM1_CS, HIGH); return v; } -void eeprom_write_byte(uint8_t* pos, uint8_t value) { - uint8_t eeprom_temp[3]; - - /*write enable*/ - eeprom_temp[0] = CMD_WREN; - WRITE(SPI_EEPROM1_CS, LOW); - spiSend(SPI_CHAN_EEPROM1, eeprom_temp, 1); - WRITE(SPI_EEPROM1_CS, HIGH); - delay(1); - - /*write addr*/ - eeprom_temp[0] = CMD_WRITE; - eeprom_temp[1] = ((unsigned)pos>>8) & 0xFF; //addr High - eeprom_temp[2] = (unsigned)pos & 0xFF; //addr Low - WRITE(SPI_EEPROM1_CS, LOW); - spiSend(SPI_CHAN_EEPROM1, eeprom_temp, 3); +void eeprom_write_byte(uint8_t *pos, uint8_t value) { + const uint8_t eeprom_temp = CMD_WREN; + WRITE(SPI_EEPROM1_CS_PIN, LOW); + spiSend(SPI_CHAN_EEPROM1, &eeprom_temp, 1); // Write Enable + + WRITE(SPI_EEPROM1_CS_PIN, HIGH); // Done with the Bus + delay(1); // For a small amount of time + + _eeprom_begin(pos, CMD_WRITE); // Set write address and begin transmission - spiSend(SPI_CHAN_EEPROM1, value); - WRITE(SPI_EEPROM1_CS, HIGH); - delay(EEPROM_WRITE_DELAY); // wait for page write to complete + spiSend(SPI_CHAN_EEPROM1, value); // Send the value to be written + WRITE(SPI_EEPROM1_CS_PIN, HIGH); // Done with the Bus + delay(EEPROM_WRITE_DELAY); // Give page write time to complete } #endif // USE_SHARED_EEPROM diff --git a/Marlin/src/HAL/shared/math_32bit.h b/Marlin/src/HAL/shared/math_32bit.h index 87e9e6406ee4..1fb233e3e896 100644 --- a/Marlin/src/HAL/shared/math_32bit.h +++ b/Marlin/src/HAL/shared/math_32bit.h @@ -26,6 +26,6 @@ /** * Math helper functions for 32 bit CPUs */ -static FORCE_INLINE uint32_t MultiU32X24toH32(uint32_t longIn1, uint32_t longIn2) { +FORCE_INLINE static uint32_t MultiU32X24toH32(uint32_t longIn1, uint32_t longIn2) { return ((uint64_t)longIn1 * longIn2 + 0x00800000) >> 24; } diff --git a/Marlin/src/HAL/shared/progmem.h b/Marlin/src/HAL/shared/progmem.h index 539d02705e97..4cd7663df9af 100644 --- a/Marlin/src/HAL/shared/progmem.h +++ b/Marlin/src/HAL/shared/progmem.h @@ -38,7 +38,8 @@ #define PSTR(str) (str) #endif #ifndef F -#define F(str) (str) +class __FlashStringHelper; +#define F(str) (reinterpret_cast(PSTR(str))) #endif #ifndef _SFR_BYTE #define _SFR_BYTE(n) (n) @@ -110,7 +111,7 @@ #define strrchr_P(str, c) strrchr((str), (c)) #endif #ifndef strsep_P -#define strsep_P(strp, delim) strsep((strp), (delim)) +#define strsep_P(pstr, delim) strsep((pstr), (delim)) #endif #ifndef strspn_P #define strspn_P(str, chrs) strspn((str), (chrs)) diff --git a/Marlin/src/MarlinCore.cpp b/Marlin/src/MarlinCore.cpp index 689650c6ab45..482ad95cac5a 100644 --- a/Marlin/src/MarlinCore.cpp +++ b/Marlin/src/MarlinCore.cpp @@ -30,10 +30,6 @@ #include "MarlinCore.h" -#if ENABLED(MARLIN_DEV_MODE) - #warning "WARNING! Disable MARLIN_DEV_MODE for the final build!" -#endif - #include "HAL/shared/Delay.h" #include "HAL/shared/esp_wifi.h" #include "HAL/shared/cpu_exception/exception_hook.h" @@ -68,20 +64,21 @@ #endif #if HAS_TFT_LVGL_UI - #include "lcd/extui/lib/mks_ui/tft_lvgl_configuration.h" - #include "lcd/extui/lib/mks_ui/draw_ui.h" - #include "lcd/extui/lib/mks_ui/mks_hardware_test.h" + #include "lcd/extui/mks_ui/tft_lvgl_configuration.h" + #include "lcd/extui/mks_ui/draw_ui.h" + #include "lcd/extui/mks_ui/mks_hardware.h" #include #endif -#if ENABLED(DWIN_CREALITY_LCD) - #include "lcd/dwin/e3v2/dwin.h" - #include "lcd/dwin/dwin_lcd.h" - #include "lcd/dwin/e3v2/rotary_encoder.h" -#endif - -#if ENABLED(EXTENSIBLE_UI) - #include "lcd/extui/ui_api.h" +#if HAS_DWIN_E3V2 + #include "lcd/e3v2/common/encoder.h" + #if ENABLED(DWIN_CREALITY_LCD) + #include "lcd/e3v2/creality/dwin.h" + #elif ENABLED(DWIN_CREALITY_LCD_ENHANCED) + #include "lcd/e3v2/proui/dwin.h" + #elif ENABLED(DWIN_CREALITY_LCD_JYERSUI) + #include "lcd/e3v2/jyersui/dwin.h" + #endif #endif #if HAS_ETHERNET @@ -136,7 +133,7 @@ #include "module/servo.h" #endif -#if ENABLED(HAS_MOTOR_CURRENT_DAC) +#if HAS_MOTOR_CURRENT_DAC #include "feature/dac/stepper_dac.h" #endif @@ -167,6 +164,8 @@ #if ENABLED(DELTA) #include "module/delta.h" +#elif ENABLED(POLARGRAPH) + #include "module/polargraph.h" #elif IS_SCARA #include "module/scara.h" #endif @@ -211,14 +210,20 @@ #include "feature/fanmux.h" #endif -#if DO_SWITCH_EXTRUDER || ANY(SWITCHING_NOZZLE, PARKING_EXTRUDER, MAGNETIC_PARKING_EXTRUDER, ELECTROMAGNETIC_SWITCHING_TOOLHEAD, SWITCHING_TOOLHEAD) - #include "module/tool_change.h" +#include "module/tool_change.h" + +#if HAS_FANCHECK + #include "feature/fancheck.h" #endif #if ENABLED(USE_CONTROLLER_FAN) #include "feature/controllerfan.h" #endif +#if HAS_PRUSA_MMU1 + #include "feature/mmu/mmu.h" +#endif + #if HAS_PRUSA_MMU2 #include "feature/mmu/mmu2.h" #endif @@ -232,7 +237,19 @@ #endif #if ENABLED(DGUS_LCD_UI_MKS) - #include "lcd/extui/lib/dgus/DGUSScreenHandler.h" + #include "lcd/extui/dgus/DGUSScreenHandler.h" +#endif + +#if HAS_DRIVER_SAFE_POWER_PROTECT + #include "feature/stepper_driver_safety.h" +#endif + +#if ENABLED(PSU_CONTROL) + #include "feature/power.h" +#endif + +#if ENABLED(EASYTHREED_UI) + #include "feature/easythreed_ui.h" #endif PGMSTR(M112_KILL_STR, "M112 Shutdown"); @@ -281,67 +298,37 @@ bool wait_for_heatup = true; #pragma GCC diagnostic push #pragma GCC diagnostic ignored "-Wnarrowing" +#ifndef RUNTIME_ONLY_ANALOG_TO_DIGITAL + template + constexpr pin_t OnlyPins<_SP_END, D...>::table[sizeof...(D)]; +#endif + bool pin_is_protected(const pin_t pin) { - static const pin_t sensitive_pins[] PROGMEM = SENSITIVE_PINS; - LOOP_L_N(i, COUNT(sensitive_pins)) { - pin_t sensitive_pin; - memcpy_P(&sensitive_pin, &sensitive_pins[i], sizeof(pin_t)); - if (pin == sensitive_pin) return true; + #ifdef RUNTIME_ONLY_ANALOG_TO_DIGITAL + static const pin_t sensitive_pins[] PROGMEM = { SENSITIVE_PINS }; + const size_t pincount = COUNT(sensitive_pins); + #else + static constexpr size_t pincount = OnlyPins::size; + static const pin_t (&sensitive_pins)[pincount] PROGMEM = OnlyPins::table; + #endif + LOOP_L_N(i, pincount) { + const pin_t * const pptr = &sensitive_pins[i]; + if (pin == (sizeof(pin_t) == 2 ? (pin_t)pgm_read_word(pptr) : (pin_t)pgm_read_byte(pptr))) return true; } return false; } #pragma GCC diagnostic pop -void enable_e_steppers() { - #define _ENA_E(N) ENABLE_AXIS_E##N(); - REPEAT(E_STEPPERS, _ENA_E) -} - -void enable_all_steppers() { - TERN_(AUTO_POWER_CONTROL, powerManager.power_on()); - ENABLE_AXIS_X(); - ENABLE_AXIS_Y(); - ENABLE_AXIS_Z(); - enable_e_steppers(); - - TERN_(EXTENSIBLE_UI, ExtUI::onSteppersEnabled()); -} - -void disable_e_steppers() { - #define _DIS_E(N) DISABLE_AXIS_E##N(); - REPEAT(E_STEPPERS, _DIS_E) -} - -void disable_e_stepper(const uint8_t e) { - #define _CASE_DIS_E(N) case N: DISABLE_AXIS_E##N(); break; - switch (e) { - REPEAT(EXTRUDERS, _CASE_DIS_E) - } -} - -void disable_all_steppers() { - DISABLE_AXIS_X(); - DISABLE_AXIS_Y(); - DISABLE_AXIS_Z(); - disable_e_steppers(); - - TERN_(EXTENSIBLE_UI, ExtUI::onSteppersDisabled()); -} - /** - * A Print Job exists when the timer is running or SD printing + * A Print Job exists when the timer is running or SD is printing */ -bool printJobOngoing() { - return print_job_timer.isRunning() || IS_SD_PRINTING(); -} +bool printJobOngoing() { return print_job_timer.isRunning() || IS_SD_PRINTING(); } /** - * Printing is active when the print job timer is running + * Printing is active when a job is underway but not paused */ -bool printingIsActive() { - return !did_pause_print && (print_job_timer.isRunning() || IS_SD_PRINTING()); -} +bool printingIsActive() { return !did_pause_print && printJobOngoing(); } /** * Printing is paused according to SD or host indicators @@ -366,7 +353,7 @@ void startOrResumeJob() { inline void abortSDPrinting() { IF_DISABLED(NO_SD_AUTOSTART, card.autofile_cancel()); - card.endFilePrint(TERN_(SD_RESORT, true)); + card.abortFilePrintNow(TERN_(SD_RESORT, true)); queue.clear(); quickstop_stepper(); @@ -382,15 +369,15 @@ void startOrResumeJob() { TERN_(POWER_LOSS_RECOVERY, recovery.purge()); #ifdef EVENT_GCODE_SD_ABORT - queue.inject_P(PSTR(EVENT_GCODE_SD_ABORT)); + queue.inject(F(EVENT_GCODE_SD_ABORT)); #endif TERN_(PASSWORD_AFTER_SD_PRINT_ABORT, password.lock_machine()); } inline void finishSDPrinting() { - if (queue.enqueue_one_P(PSTR("M1001"))) { - marlin_state = MF_RUNNING; + if (queue.enqueue_one(F("M1001"))) { // Keep trying until it gets queued + marlin_state = MF_RUNNING; // Signal to stop trying TERN_(PASSWORD_AFTER_SD_PRINT_END, password.lock_machine()); TERN_(DGUS_LCD_UI_MKS, ScreenHandler.SDPrintingFinished()); } @@ -411,19 +398,18 @@ void startOrResumeJob() { * - Check if an idle but hot extruder needs filament extruded (EXTRUDER_RUNOUT_PREVENT) * - Pulse FET_SAFETY_PIN if it exists */ -inline void manage_inactivity(const bool ignore_stepper_queue=false) { +inline void manage_inactivity(const bool no_stepper_sleep=false) { queue.get_available_commands(); const millis_t ms = millis(); - // Prevent steppers timing-out in the middle of M600 - // unless PAUSE_PARK_NO_STEPPER_TIMEOUT is disabled - const bool parked_or_ignoring = ignore_stepper_queue + // Prevent steppers timing-out + const bool do_reset_timeout = no_stepper_sleep || TERN0(PAUSE_PARK_NO_STEPPER_TIMEOUT, did_pause_print); // Reset both the M18/M84 activity timeout and the M85 max 'kill' timeout - if (parked_or_ignoring) gcode.reset_stepper_timeout(ms); + if (do_reset_timeout) gcode.reset_stepper_timeout(ms); if (gcode.stepper_max_timed_out(ms)) { SERIAL_ERROR_MSG(STR_KILL_INACTIVE_TIME, parser.command_ptr); @@ -439,15 +425,18 @@ inline void manage_inactivity(const bool ignore_stepper_queue=false) { // activity timeout and the M85 max 'kill' timeout if (planner.has_blocks_queued()) gcode.reset_stepper_timeout(ms); - else if (!parked_or_ignoring && gcode.stepper_inactive_timeout()) { + else if (!do_reset_timeout && gcode.stepper_inactive_timeout()) { if (!already_shutdown_steppers) { already_shutdown_steppers = true; // L6470 SPI will consume 99% of free time without this // Individual axes will be disabled if configured - if (ENABLED(DISABLE_INACTIVE_X)) DISABLE_AXIS_X(); - if (ENABLED(DISABLE_INACTIVE_Y)) DISABLE_AXIS_Y(); - if (ENABLED(DISABLE_INACTIVE_Z)) DISABLE_AXIS_Z(); - if (ENABLED(DISABLE_INACTIVE_E)) disable_e_steppers(); + TERN_(DISABLE_INACTIVE_X, stepper.disable_axis(X_AXIS)); + TERN_(DISABLE_INACTIVE_Y, stepper.disable_axis(Y_AXIS)); + TERN_(DISABLE_INACTIVE_Z, stepper.disable_axis(Z_AXIS)); + TERN_(DISABLE_INACTIVE_I, stepper.disable_axis(I_AXIS)); + TERN_(DISABLE_INACTIVE_J, stepper.disable_axis(J_AXIS)); + TERN_(DISABLE_INACTIVE_K, stepper.disable_axis(K_AXIS)); + TERN_(DISABLE_INACTIVE_E, stepper.disable_e_steppers()); TERN_(AUTO_BED_LEVELING_UBL, ubl.steppers_were_disabled()); } @@ -456,7 +445,8 @@ inline void manage_inactivity(const bool ignore_stepper_queue=false) { already_shutdown_steppers = false; } - #if PIN_EXISTS(CHDK) // Check if pin should be set to LOW (after M240 set it HIGH) + #if ENABLED(PHOTO_GCODE) && PIN_EXISTS(CHDK) + // Check if CHDK should be set to LOW (after M240 set it HIGH) extern millis_t chdk_timeout; if (chdk_timeout && ELAPSED(ms, chdk_timeout)) { chdk_timeout = 0; @@ -485,15 +475,18 @@ inline void manage_inactivity(const bool ignore_stepper_queue=false) { } #endif + #if HAS_FREEZE_PIN + Stepper::frozen = !READ(FREEZE_PIN); + #endif + #if HAS_HOME // Handle a standalone HOME button constexpr millis_t HOME_DEBOUNCE_DELAY = 1000UL; static millis_t next_home_key_ms; // = 0 if (!IS_SD_PRINTING() && !READ(HOME_PIN)) { // HOME_PIN goes LOW when pressed - const millis_t ms = millis(); if (ELAPSED(ms, next_home_key_ms)) { next_home_key_ms = ms + HOME_DEBOUNCE_DELAY; - LCD_MESSAGEPGM(MSG_AUTO_HOME); + LCD_MESSAGE(MSG_AUTO_HOME); queue.inject_P(G28_STR); } } @@ -502,120 +495,175 @@ inline void manage_inactivity(const bool ignore_stepper_queue=false) { #if ENABLED(CUSTOM_USER_BUTTONS) // Handle a custom user button if defined const bool printer_not_busy = !printingIsActive(); - #define HAS_CUSTOM_USER_BUTTON(N) (PIN_EXISTS(BUTTON##N) && defined(BUTTON##N##_HIT_STATE) && defined(BUTTON##N##_GCODE) && defined(BUTTON##N##_DESC)) - #define CHECK_CUSTOM_USER_BUTTON(N) do{ \ + #define HAS_CUSTOM_USER_BUTTON(N) (PIN_EXISTS(BUTTON##N) && defined(BUTTON##N##_HIT_STATE) && defined(BUTTON##N##_GCODE)) + #define HAS_BETTER_USER_BUTTON(N) HAS_CUSTOM_USER_BUTTON(N) && defined(BUTTON##N##_DESC) + #define _CHECK_CUSTOM_USER_BUTTON(N, CODE) do{ \ constexpr millis_t CUB_DEBOUNCE_DELAY_##N = 250UL; \ static millis_t next_cub_ms_##N; \ if (BUTTON##N##_HIT_STATE == READ(BUTTON##N##_PIN) \ && (ENABLED(BUTTON##N##_WHEN_PRINTING) || printer_not_busy)) { \ - const millis_t ms = millis(); \ if (ELAPSED(ms, next_cub_ms_##N)) { \ next_cub_ms_##N = ms + CUB_DEBOUNCE_DELAY_##N; \ - if (strlen(BUTTON##N##_DESC)) \ - LCD_MESSAGEPGM_P(PSTR(BUTTON##N##_DESC)); \ - queue.inject_P(PSTR(BUTTON##N##_GCODE)); \ + CODE; \ + queue.inject(F(BUTTON##N##_GCODE)); \ + TERN_(HAS_MARLINUI_MENU, ui.quick_feedback()); \ } \ } \ }while(0) - #if HAS_CUSTOM_USER_BUTTON(1) + #define CHECK_CUSTOM_USER_BUTTON(N) _CHECK_CUSTOM_USER_BUTTON(N, NOOP) + #define CHECK_BETTER_USER_BUTTON(N) _CHECK_CUSTOM_USER_BUTTON(N, if (strlen(BUTTON##N##_DESC)) LCD_MESSAGE_F(BUTTON##N##_DESC)) + + #if HAS_BETTER_USER_BUTTON(1) + CHECK_BETTER_USER_BUTTON(1); + #elif HAS_CUSTOM_USER_BUTTON(1) CHECK_CUSTOM_USER_BUTTON(1); #endif - #if HAS_CUSTOM_USER_BUTTON(2) + #if HAS_BETTER_USER_BUTTON(2) + CHECK_BETTER_USER_BUTTON(2); + #elif HAS_CUSTOM_USER_BUTTON(2) CHECK_CUSTOM_USER_BUTTON(2); #endif - #if HAS_CUSTOM_USER_BUTTON(3) + #if HAS_BETTER_USER_BUTTON(3) + CHECK_BETTER_USER_BUTTON(3); + #elif HAS_CUSTOM_USER_BUTTON(3) CHECK_CUSTOM_USER_BUTTON(3); #endif - #if HAS_CUSTOM_USER_BUTTON(4) + #if HAS_BETTER_USER_BUTTON(4) + CHECK_BETTER_USER_BUTTON(4); + #elif HAS_CUSTOM_USER_BUTTON(4) CHECK_CUSTOM_USER_BUTTON(4); #endif - #if HAS_CUSTOM_USER_BUTTON(5) + #if HAS_BETTER_USER_BUTTON(5) + CHECK_BETTER_USER_BUTTON(5); + #elif HAS_CUSTOM_USER_BUTTON(5) CHECK_CUSTOM_USER_BUTTON(5); #endif - #if HAS_CUSTOM_USER_BUTTON(6) + #if HAS_BETTER_USER_BUTTON(6) + CHECK_BETTER_USER_BUTTON(6); + #elif HAS_CUSTOM_USER_BUTTON(6) CHECK_CUSTOM_USER_BUTTON(6); #endif - #if HAS_CUSTOM_USER_BUTTON(7) + #if HAS_BETTER_USER_BUTTON(7) + CHECK_BETTER_USER_BUTTON(7); + #elif HAS_CUSTOM_USER_BUTTON(7) CHECK_CUSTOM_USER_BUTTON(7); #endif - #if HAS_CUSTOM_USER_BUTTON(8) + #if HAS_BETTER_USER_BUTTON(8) + CHECK_BETTER_USER_BUTTON(8); + #elif HAS_CUSTOM_USER_BUTTON(8) CHECK_CUSTOM_USER_BUTTON(8); #endif - #if HAS_CUSTOM_USER_BUTTON(9) + #if HAS_BETTER_USER_BUTTON(9) + CHECK_BETTER_USER_BUTTON(9); + #elif HAS_CUSTOM_USER_BUTTON(9) CHECK_CUSTOM_USER_BUTTON(9); #endif - #if HAS_CUSTOM_USER_BUTTON(10) + #if HAS_BETTER_USER_BUTTON(10) + CHECK_BETTER_USER_BUTTON(10); + #elif HAS_CUSTOM_USER_BUTTON(10) CHECK_CUSTOM_USER_BUTTON(10); #endif - #if HAS_CUSTOM_USER_BUTTON(11) + #if HAS_BETTER_USER_BUTTON(11) + CHECK_BETTER_USER_BUTTON(11); + #elif HAS_CUSTOM_USER_BUTTON(11) CHECK_CUSTOM_USER_BUTTON(11); #endif - #if HAS_CUSTOM_USER_BUTTON(12) + #if HAS_BETTER_USER_BUTTON(12) + CHECK_BETTER_USER_BUTTON(12); + #elif HAS_CUSTOM_USER_BUTTON(12) CHECK_CUSTOM_USER_BUTTON(12); #endif - #if HAS_CUSTOM_USER_BUTTON(13) + #if HAS_BETTER_USER_BUTTON(13) + CHECK_BETTER_USER_BUTTON(13); + #elif HAS_CUSTOM_USER_BUTTON(13) CHECK_CUSTOM_USER_BUTTON(13); #endif - #if HAS_CUSTOM_USER_BUTTON(14) + #if HAS_BETTER_USER_BUTTON(14) + CHECK_BETTER_USER_BUTTON(14); + #elif HAS_CUSTOM_USER_BUTTON(14) CHECK_CUSTOM_USER_BUTTON(14); #endif - #if HAS_CUSTOM_USER_BUTTON(15) + #if HAS_BETTER_USER_BUTTON(15) + CHECK_BETTER_USER_BUTTON(15); + #elif HAS_CUSTOM_USER_BUTTON(15) CHECK_CUSTOM_USER_BUTTON(15); #endif - #if HAS_CUSTOM_USER_BUTTON(16) + #if HAS_BETTER_USER_BUTTON(16) + CHECK_BETTER_USER_BUTTON(16); + #elif HAS_CUSTOM_USER_BUTTON(16) CHECK_CUSTOM_USER_BUTTON(16); #endif - #if HAS_CUSTOM_USER_BUTTON(17) + #if HAS_BETTER_USER_BUTTON(17) + CHECK_BETTER_USER_BUTTON(17); + #elif HAS_CUSTOM_USER_BUTTON(17) CHECK_CUSTOM_USER_BUTTON(17); #endif - #if HAS_CUSTOM_USER_BUTTON(18) + #if HAS_BETTER_USER_BUTTON(18) + CHECK_BETTER_USER_BUTTON(18); + #elif HAS_CUSTOM_USER_BUTTON(18) CHECK_CUSTOM_USER_BUTTON(18); #endif - #if HAS_CUSTOM_USER_BUTTON(19) + #if HAS_BETTER_USER_BUTTON(19) + CHECK_BETTER_USER_BUTTON(19); + #elif HAS_CUSTOM_USER_BUTTON(19) CHECK_CUSTOM_USER_BUTTON(19); #endif - #if HAS_CUSTOM_USER_BUTTON(20) + #if HAS_BETTER_USER_BUTTON(20) + CHECK_BETTER_USER_BUTTON(20); + #elif HAS_CUSTOM_USER_BUTTON(20) CHECK_CUSTOM_USER_BUTTON(20); #endif - #if HAS_CUSTOM_USER_BUTTON(21) + #if HAS_BETTER_USER_BUTTON(21) + CHECK_BETTER_USER_BUTTON(21); + #elif HAS_CUSTOM_USER_BUTTON(21) CHECK_CUSTOM_USER_BUTTON(21); #endif - #if HAS_CUSTOM_USER_BUTTON(22) + #if HAS_BETTER_USER_BUTTON(22) + CHECK_BETTER_USER_BUTTON(22); + #elif HAS_CUSTOM_USER_BUTTON(22) CHECK_CUSTOM_USER_BUTTON(22); #endif - #if HAS_CUSTOM_USER_BUTTON(23) + #if HAS_BETTER_USER_BUTTON(23) + CHECK_BETTER_USER_BUTTON(23); + #elif HAS_CUSTOM_USER_BUTTON(23) CHECK_CUSTOM_USER_BUTTON(23); #endif - #if HAS_CUSTOM_USER_BUTTON(24) + #if HAS_BETTER_USER_BUTTON(24) + CHECK_BETTER_USER_BUTTON(24); + #elif HAS_CUSTOM_USER_BUTTON(24) CHECK_CUSTOM_USER_BUTTON(24); #endif - #if HAS_CUSTOM_USER_BUTTON(25) + #if HAS_BETTER_USER_BUTTON(25) + CHECK_BETTER_USER_BUTTON(25); + #elif HAS_CUSTOM_USER_BUTTON(25) CHECK_CUSTOM_USER_BUTTON(25); #endif #endif + TERN_(EASYTHREED_UI, easythreed_ui.run()); + TERN_(USE_CONTROLLER_FAN, controllerFan.update()); // Check if fan should be turned on to cool stepper drivers down - TERN_(AUTO_POWER_CONTROL, powerManager.check()); + TERN_(AUTO_POWER_CONTROL, powerManager.check(!ui.on_status_screen() || printJobOngoing() || printingIsPaused())); TERN_(HOTEND_IDLE_TIMEOUT, hotend_idle.check()); #if ENABLED(EXTRUDER_RUNOUT_PREVENT) - if (thermalManager.degHotend(active_extruder) > EXTRUDER_RUNOUT_MINTEMP + if (thermalManager.degHotend(active_extruder) > (EXTRUDER_RUNOUT_MINTEMP) && ELAPSED(ms, gcode.previous_move_ms + SEC_TO_MS(EXTRUDER_RUNOUT_SECONDS)) && !planner.has_blocks_queued() ) { #if ENABLED(SWITCHING_EXTRUDER) bool oldstatus; switch (active_extruder) { - default: oldstatus = E0_ENABLE_READ(); ENABLE_AXIS_E0(); break; + default: oldstatus = stepper.AXIS_IS_ENABLED(E_AXIS, 0); stepper.ENABLE_EXTRUDER(0); break; #if E_STEPPERS > 1 - case 2: case 3: oldstatus = E1_ENABLE_READ(); ENABLE_AXIS_E1(); break; + case 2: case 3: oldstatus = stepper.AXIS_IS_ENABLED(E_AXIS, 1); stepper.ENABLE_EXTRUDER(1); break; #if E_STEPPERS > 2 - case 4: case 5: oldstatus = E2_ENABLE_READ(); ENABLE_AXIS_E2(); break; + case 4: case 5: oldstatus = stepper.AXIS_IS_ENABLED(E_AXIS, 2); stepper.ENABLE_EXTRUDER(2); break; #if E_STEPPERS > 3 - case 6: case 7: oldstatus = E3_ENABLE_READ(); ENABLE_AXIS_E3(); break; + case 6: case 7: oldstatus = stepper.AXIS_IS_ENABLED(E_AXIS, 3); stepper.ENABLE_EXTRUDER(3); break; #endif // E_STEPPERS > 3 #endif // E_STEPPERS > 2 #endif // E_STEPPERS > 1 @@ -624,7 +672,7 @@ inline void manage_inactivity(const bool ignore_stepper_queue=false) { bool oldstatus; switch (active_extruder) { default: - #define _CASE_EN(N) case N: oldstatus = E##N##_ENABLE_READ(); ENABLE_AXIS_E##N(); break; + #define _CASE_EN(N) case N: oldstatus = stepper.AXIS_IS_ENABLED(E_AXIS, N); stepper.ENABLE_EXTRUDER(N); break; REPEAT(E_STEPPERS, _CASE_EN); } #endif @@ -638,17 +686,17 @@ inline void manage_inactivity(const bool ignore_stepper_queue=false) { #if ENABLED(SWITCHING_EXTRUDER) switch (active_extruder) { - default: oldstatus = E0_ENABLE_WRITE(oldstatus); break; + default: if (oldstatus) stepper.ENABLE_EXTRUDER(0); else stepper.DISABLE_EXTRUDER(0); break; #if E_STEPPERS > 1 - case 2: case 3: oldstatus = E1_ENABLE_WRITE(oldstatus); break; + case 2: case 3: if (oldstatus) stepper.ENABLE_EXTRUDER(1); else stepper.DISABLE_EXTRUDER(1); break; #if E_STEPPERS > 2 - case 4: case 5: oldstatus = E2_ENABLE_WRITE(oldstatus); break; + case 4: case 5: if (oldstatus) stepper.ENABLE_EXTRUDER(2); else stepper.DISABLE_EXTRUDER(2); break; #endif // E_STEPPERS > 2 #endif // E_STEPPERS > 1 } #else // !SWITCHING_EXTRUDER switch (active_extruder) { - #define _CASE_RESTORE(N) case N: E##N##_ENABLE_WRITE(oldstatus); break; + #define _CASE_RESTORE(N) case N: if (oldstatus) stepper.ENABLE_EXTRUDER(N); else stepper.DISABLE_EXTRUDER(N); break; REPEAT(E_STEPPERS, _CASE_RESTORE); } #endif // !SWITCHING_EXTRUDER @@ -714,14 +762,14 @@ inline void manage_inactivity(const bool ignore_stepper_queue=false) { * - Update the Průša MMU2 * - Handle Joystick jogging */ -void idle(TERN_(ADVANCED_PAUSE_FEATURE, bool no_stepper_sleep/*=false*/)) { +void idle(bool no_stepper_sleep/*=false*/) { #if ENABLED(MARLIN_DEV_MODE) static uint16_t idle_depth = 0; - if (++idle_depth > 5) SERIAL_ECHOLNPAIR("idle() call depth: ", idle_depth); + if (++idle_depth > 5) SERIAL_ECHOLNPGM("idle() call depth: ", idle_depth); #endif // Core Marlin activities - manage_inactivity(TERN_(ADVANCED_PAUSE_FEATURE, no_stepper_sleep)); + manage_inactivity(no_stepper_sleep); // Manage Heaters (and Watchdog) thermalManager.manage_heater(); @@ -732,33 +780,37 @@ void idle(TERN_(ADVANCED_PAUSE_FEATURE, bool no_stepper_sleep/*=false*/)) { // Return if setup() isn't completed if (marlin_state == MF_INITIALIZING) goto IDLE_DONE; + // TODO: Still causing errors + (void)check_tool_sensor_stats(active_extruder, true); + // Handle filament runout sensors - TERN_(HAS_FILAMENT_SENSOR, runout.run()); + #if HAS_FILAMENT_SENSOR + if (TERN1(HAS_PRUSA_MMU2, !mmu2.enabled())) + runout.run(); + #endif // Run HAL idle tasks - TERN_(HAL_IDLETASK, HAL_idletask()); + hal.idletask(); // Check network connection TERN_(HAS_ETHERNET, ethernet.check()); // Handle Power-Loss Recovery #if ENABLED(POWER_LOSS_RECOVERY) && PIN_EXISTS(POWER_LOSS) - if (printJobOngoing()) recovery.outage(); + if (IS_SD_PRINTING()) recovery.outage(); #endif // Run StallGuard endstop checks #if ENABLED(SPI_ENDSTOPS) - if (endstops.tmc_spi_homing.any - && TERN1(IMPROVE_HOMING_RELIABILITY, ELAPSED(millis(), sg_guard_period)) - ) LOOP_L_N(i, 4) // Read SGT 4 times per idle loop - if (endstops.tmc_spi_homing_check()) break; + if (endstops.tmc_spi_homing.any && TERN1(IMPROVE_HOMING_RELIABILITY, ELAPSED(millis(), sg_guard_period))) + LOOP_L_N(i, 4) if (endstops.tmc_spi_homing_check()) break; // Read SGT 4 times per idle loop #endif // Handle SD Card insert / remove TERN_(SDSUPPORT, card.manage_media()); // Handle USB Flash Drive insert / remove - TERN_(USB_FLASH_DRIVE_SUPPORT, Sd2Card::idle()); + TERN_(USB_FLASH_DRIVE_SUPPORT, card.diskIODriver()->idle()); // Announce Host Keepalive state (if any) TERN_(HOST_KEEPALIVE_FEATURE, gcode.host_keepalive()); @@ -770,7 +822,7 @@ void idle(TERN_(ADVANCED_PAUSE_FEATURE, bool no_stepper_sleep/*=false*/)) { TERN_(USE_BEEPER, buzzer.tick()); // Handle UI input / draw events - TERN(DWIN_CREALITY_LCD, DWIN_Update(), ui.update()); + TERN(HAS_DWIN_E3V2_BASIC, DWIN_Update(), ui.update()); // Run i2c Position Encoders #if ENABLED(I2C_POSITION_ENCODERS) @@ -790,7 +842,10 @@ void idle(TERN_(ADVANCED_PAUSE_FEATURE, bool no_stepper_sleep/*=false*/)) { #if HAS_AUTO_REPORTING if (!gcode.autoreport_paused) { TERN_(AUTO_REPORT_TEMPERATURES, thermalManager.auto_reporter.tick()); + TERN_(AUTO_REPORT_FANS, fan_check.auto_reporter.tick()); TERN_(AUTO_REPORT_SD_STATUS, card.auto_reporter.tick()); + TERN_(AUTO_REPORT_POSITION, position_auto_reporter.tick()); + TERN_(BUFFER_MONITORING, queue.auto_report_buffer_statistics()); } #endif @@ -815,26 +870,27 @@ void idle(TERN_(ADVANCED_PAUSE_FEATURE, bool no_stepper_sleep/*=false*/)) { * Kill all activity and lock the machine. * After this the machine will need to be reset. */ -void kill(PGM_P const lcd_error/*=nullptr*/, PGM_P const lcd_component/*=nullptr*/, const bool steppers_off/*=false*/) { +void kill(FSTR_P const lcd_error/*=nullptr*/, FSTR_P const lcd_component/*=nullptr*/, const bool steppers_off/*=false*/) { thermalManager.disable_all_heaters(); TERN_(HAS_CUTTER, cutter.kill()); // Full cutter shutdown including ISR control - SERIAL_ERROR_MSG(STR_ERR_KILLED); + // Echo the LCD message to serial for extra context + if (lcd_error) { SERIAL_ECHO_START(); SERIAL_ECHOLNF(lcd_error); } - #if HAS_DISPLAY - ui.kill_screen(lcd_error ?: GET_TEXT(MSG_KILLED), lcd_component ?: NUL_STR); + #if EITHER(HAS_DISPLAY, DWIN_CREALITY_LCD_ENHANCED) + ui.kill_screen(lcd_error ?: GET_TEXT_F(MSG_KILLED), lcd_component ?: FPSTR(NUL_STR)); #else - UNUSED(lcd_error); - UNUSED(lcd_component); + UNUSED(lcd_error); UNUSED(lcd_component); #endif - #if HAS_TFT_LVGL_UI - lv_draw_error_message(lcd_error); - #endif + TERN_(HAS_TFT_LVGL_UI, lv_draw_error_message(lcd_error)); + + // "Error:Printer halted. kill() called!" + SERIAL_ERROR_MSG(STR_ERR_KILLED); #ifdef ACTION_ON_KILL - host_action_kill(); + hostui.kill(); #endif minkill(steppers_off); @@ -856,26 +912,28 @@ void minkill(const bool steppers_off/*=false*/) { TERN_(HAS_CUTTER, cutter.kill()); // Reiterate cutter shutdown // Power off all steppers (for M112) or just the E steppers - steppers_off ? disable_all_steppers() : disable_e_steppers(); + steppers_off ? stepper.disable_all_steppers() : stepper.disable_e_steppers(); - TERN_(PSU_CONTROL, PSU_OFF()); + TERN_(PSU_CONTROL, powerManager.power_off()); TERN_(HAS_SUICIDE, suicide()); - #if HAS_KILL + #if EITHER(HAS_KILL, SOFT_RESET_ON_KILL) - // Wait for kill to be released - while (kill_state()) watchdog_refresh(); + // Wait for both KILL and ENC to be released + while (TERN0(HAS_KILL, kill_state()) || TERN0(SOFT_RESET_ON_KILL, ui.button_pressed())) + watchdog_refresh(); - // Wait for kill to be pressed - while (!kill_state()) watchdog_refresh(); + // Wait for either KILL or ENC to be pressed again + while (TERN1(HAS_KILL, !kill_state()) && TERN1(SOFT_RESET_ON_KILL, !ui.button_pressed())) + watchdog_refresh(); - void (*resetFunc)() = 0; // Declare resetFunc() at address 0 - resetFunc(); // Jump to address 0 + // Reboot the board + hal.reboot(); #else - for (;;) watchdog_refresh(); // Wait for reset + for (;;) watchdog_refresh(); // Wait for RESET button or power-cycle #endif } @@ -889,13 +947,13 @@ void stop() { print_job_timer.stop(); - #if ENABLED(PROBING_FANS_OFF) - if (thermalManager.fans_paused) thermalManager.set_fans_paused(false); // put things back the way they were + #if EITHER(PROBING_FANS_OFF, ADVANCED_PAUSE_FANS_PAUSE) + thermalManager.set_fans_paused(false); // Un-pause fans for safety #endif - if (IsRunning()) { + if (!IsStopped()) { SERIAL_ERROR_MSG(STR_ERR_STOPPED); - LCD_MESSAGEPGM(MSG_STOPPED); + LCD_MESSAGE(MSG_STOPPED); safe_delay(350); // allow enough time for messages to get out before stopping marlin_state = MF_STOPPED; } @@ -926,6 +984,15 @@ inline void tmc_standby_setup() { #if PIN_EXISTS(Z4_STDBY) SET_INPUT_PULLDOWN(Z4_STDBY_PIN); #endif + #if PIN_EXISTS(I_STDBY) + SET_INPUT_PULLDOWN(I_STDBY_PIN); + #endif + #if PIN_EXISTS(J_STDBY) + SET_INPUT_PULLDOWN(J_STDBY_PIN); + #endif + #if PIN_EXISTS(K_STDBY) + SET_INPUT_PULLDOWN(K_STDBY_PIN); + #endif #if PIN_EXISTS(E0_STDBY) SET_INPUT_PULLDOWN(E0_STDBY_PIN); #endif @@ -953,31 +1020,108 @@ inline void tmc_standby_setup() { } /** - * Marlin entry-point: Set up before the program loop - * - Set up the kill pin, filament runout, power hold, custom user buttons - * - Start the serial port + * Marlin Firmware entry-point. Abandon Hope All Ye Who Enter Here. + * Setup before the program loop: + * + * - Call any special pre-init set for the board + * - Put TMC drivers into Low Power Standby mode + * - Init the serial ports (so setup can be debugged) + * - Set up the kill and suicide pins + * - Prepare (disable) board JTAG and Debug ports + * - Init serial for a connected MKS TFT with WiFi + * - Install Marlin custom Exception Handlers, if set. + * - Init Marlin's HAL interfaces (for SPI, i2c, etc.) + * - Init some optional hardware and features: + * • MAX Thermocouple pins + * • Duet Smart Effector + * • Filament Runout Sensor + * • TMC220x Stepper Drivers (Serial) + * • PSU control + * • Power-loss Recovery + * • L64XX Stepper Drivers (SPI) + * • Stepper Driver Reset: DISABLE + * • TMC Stepper Drivers (SPI) + * • Run hal.init_board() for additional pins setup + * • ESP WiFi + * - Get the Reset Reason and report it * - Print startup messages and diagnostics - * - Get EEPROM or default settings - * - Initialize managers for: - * • temperature - * • planner - * • watchdog - * • stepper - * • photo pin - * • servos - * • LCD controller - * • Digipot I2C - * • Z probe sled - * • status LEDs - * • Max7219 + * - Calibrate the HAL DELAY for precise timing + * - Init the buzzer, possibly a custom timer + * - Init more optional hardware: + * • Color LED illumination + * • Neopixel illumination + * • Controller Fan + * • Creality DWIN LCD (show boot image) + * • Tare the Probe if possible + * - Mount the (most likely external) SD Card + * - Load settings from EEPROM (or use defaults) + * - Init the Ethernet Port + * - Init Touch Buttons (for emulated DOGLCD) + * - Adjust the (certainly wrong) current position by the home offset + * - Init the Planner::position (steps) based on current (native) position + * - Initialize more managers and peripherals: + * • Temperatures + * • Print Job Timer + * • Endstops and Endstop Interrupts + * • Stepper ISR - Kind of Important! + * • Servos + * • Servo-based Probe + * • Photograph Pin + * • Laser/Spindle tool Power / PWM + * • Coolant Control + * • Bed Probe + * • Stepper Driver Reset: ENABLE + * • Digipot I2C - Stepper driver current control + * • Stepper DAC - Stepper driver current control + * • Solenoid (probe, or for other use) + * • Home Pin + * • Custom User Buttons + * • Red/Blue Status LEDs + * • Case Light + * • Prusa MMU filament changer + * • Fan Multiplexer + * • Mixing Extruder + * • BLTouch Probe + * • I2C Position Encoders + * • Custom I2C Bus handlers + * • Enhanced tools or extruders: + * • Switching Extruder + * • Switching Nozzle + * • Parking Extruder + * • Magnetic Parking Extruder + * • Switching Toolhead + * • Electromagnetic Switching Toolhead + * • Watchdog Timer - Also Kind of Important! + * • Closed Loop Controller + * - Run Startup Commands, if defined + * - Tell host to close Host Prompts + * - Test Trinamic driver connections + * - Init Prusa MMU2 filament changer + * - Init and test BL24Cxx EEPROM + * - Init Creality DWIN encoder, show faux progress bar + * - Reset Status Message / Show Service Messages + * - Init MAX7219 LED Matrix + * - Init Direct Stepping (Klipper-style motion control) + * - Init TFT LVGL UI (with 3D Graphics) + * - Apply Password Lock - Hold for Authentication + * - Open Touch Screen Calibration screen, if not calibrated + * - Set Marlin to RUNNING State */ void setup() { + #ifdef FASTIO_INIT + FASTIO_INIT(); + #endif + #ifdef BOARD_PREINIT BOARD_PREINIT(); // Low-level init (before serial init) #endif tmc_standby_setup(); // TMC Low Power Standby pins must be set early or they're not usable + // Check startup - does nothing if bootloader sets MCUSR to 0 + const byte mcu = hal.get_reset_source(); + hal.clear_reset_source(); + #if ENABLED(MARLIN_DEV_MODE) auto log_current_ms = [&](PGM_P const msg) { SERIAL_ECHO_START(); @@ -990,14 +1134,25 @@ void setup() { #endif #define SETUP_RUN(C) do{ SETUP_LOG(STRINGIFY(C)); C; }while(0) - MYSERIAL0.begin(BAUDRATE); + MYSERIAL1.begin(BAUDRATE); millis_t serial_connect_timeout = millis() + 1000UL; - while (!MYSERIAL0.connected() && PENDING(millis(), serial_connect_timeout)) { /*nada*/ } + while (!MYSERIAL1.connected() && PENDING(millis(), serial_connect_timeout)) { /*nada*/ } #if HAS_MULTI_SERIAL && !HAS_ETHERNET - MYSERIAL1.begin(BAUDRATE); + #ifndef BAUDRATE_2 + #define BAUDRATE_2 BAUDRATE + #endif + MYSERIAL2.begin(BAUDRATE_2); serial_connect_timeout = millis() + 1000UL; - while (!MYSERIAL1.connected() && PENDING(millis(), serial_connect_timeout)) { /*nada*/ } + while (!MYSERIAL2.connected() && PENDING(millis(), serial_connect_timeout)) { /*nada*/ } + #ifdef SERIAL_PORT_3 + #ifndef BAUDRATE_3 + #define BAUDRATE_3 BAUDRATE + #endif + MYSERIAL3.begin(BAUDRATE_3); + serial_connect_timeout = millis() + 1000UL; + while (!MYSERIAL3.connected() && PENDING(millis(), serial_connect_timeout)) { /*nada*/ } + #endif #endif SERIAL_ECHOLNPGM("start"); @@ -1011,39 +1166,42 @@ void setup() { #endif #endif + #if HAS_FREEZE_PIN + SETUP_LOG("FREEZE_PIN"); + SET_INPUT_PULLUP(FREEZE_PIN); + #endif + #if HAS_SUICIDE SETUP_LOG("SUICIDE_PIN"); - OUT_WRITE(SUICIDE_PIN, !SUICIDE_PIN_INVERTING); + OUT_WRITE(SUICIDE_PIN, !SUICIDE_PIN_STATE); #endif - #if EITHER(DISABLE_DEBUG, DISABLE_JTAG) - // Disable any hardware debug to free up pins for IO - #if ENABLED(DISABLE_DEBUG) && defined(JTAGSWD_DISABLE) - JTAGSWD_DISABLE(); - #elif defined(JTAG_DISABLE) - JTAG_DISABLE(); - #else - #error "DISABLE_(DEBUG|JTAG) is not supported for the selected MCU/Board." - #endif + #ifdef JTAGSWD_RESET + SETUP_LOG("JTAGSWD_RESET"); + JTAGSWD_RESET(); #endif - #if BOTH(HAS_TFT_LVGL_UI, MKS_WIFI_MODULE) - mks_esp_wifi_init(); - WIFISERIAL.begin(WIFI_BAUDRATE); - serial_connect_timeout = millis() + 1000UL; - while (/*!WIFISERIAL && */PENDING(millis(), serial_connect_timeout)) { /*nada*/ } + // Disable any hardware debug to free up pins for IO + #if ENABLED(DISABLE_DEBUG) && defined(JTAGSWD_DISABLE) + delay(10); + SETUP_LOG("JTAGSWD_DISABLE"); + JTAGSWD_DISABLE(); + #elif ENABLED(DISABLE_JTAG) && defined(JTAG_DISABLE) + delay(10); + SETUP_LOG("JTAG_DISABLE"); + JTAG_DISABLE(); #endif TERN_(DYNAMIC_VECTORTABLE, hook_cpu_exceptions()); // If supported, install Marlin exception handlers at runtime - SETUP_RUN(HAL_init()); + SETUP_RUN(hal.init()); // Init and disable SPI thermocouples; this is still needed - #if TEMP_SENSOR_0_IS_MAX_TC - OUT_WRITE(MAX6675_SS_PIN, HIGH); // Disable + #if TEMP_SENSOR_0_IS_MAX_TC || (TEMP_SENSOR_REDUNDANT_IS_MAX_TC && REDUNDANT_TEMP_MATCH(SOURCE, E0)) + OUT_WRITE(TEMP_0_CS_PIN, HIGH); // Disable #endif - #if TEMP_SENSOR_1_IS_MAX_TC - OUT_WRITE(MAX6675_SS2_PIN, HIGH); // Disable + #if TEMP_SENSOR_1_IS_MAX_TC || (TEMP_SENSOR_REDUNDANT_IS_MAX_TC && REDUNDANT_TEMP_MATCH(SOURCE, E1)) + OUT_WRITE(TEMP_1_CS_PIN, HIGH); #endif #if ENABLED(DUET_SMART_EFFECTOR) && PIN_EXISTS(SMART_EFFECTOR_MOD) @@ -1054,10 +1212,13 @@ void setup() { SETUP_RUN(runout.setup()); #endif + #if HAS_TMC220x + SETUP_RUN(tmc_serial_begin()); + #endif + #if ENABLED(PSU_CONTROL) SETUP_LOG("PSU_CONTROL"); - powersupply_on = ENABLED(PSU_DEFAULT_OFF); - if (ENABLED(PSU_DEFAULT_OFF)) PSU_OFF(); else PSU_ON(); + powerManager.init(); #endif #if ENABLED(POWER_LOSS_RECOVERY) @@ -1068,10 +1229,6 @@ void setup() { SETUP_RUN(L64xxManager.init()); // Set up SPI, init drivers #endif - #if HAS_TMC220x - SETUP_RUN(tmc_serial_begin()); - #endif - #if HAS_STEPPER_RESET SETUP_RUN(disableStepperDrivers()); #endif @@ -1083,34 +1240,27 @@ void setup() { SETUP_RUN(tmc_init_cs_pins()); #endif - #ifdef BOARD_INIT - SETUP_LOG("BOARD_INIT"); - BOARD_INIT(); - #endif + SETUP_RUN(hal.init_board()); SETUP_RUN(esp_wifi_init()); - // Check startup - does nothing if bootloader sets MCUSR to 0 - const byte mcu = HAL_get_reset_source(); - if (mcu & RST_POWER_ON) SERIAL_ECHOLNPGM(STR_POWERUP); - if (mcu & RST_EXTERNAL) SERIAL_ECHOLNPGM(STR_EXTERNAL_RESET); + // Report Reset Reason + if (mcu & RST_POWER_ON) SERIAL_ECHOLNPGM(STR_POWERUP); + if (mcu & RST_EXTERNAL) SERIAL_ECHOLNPGM(STR_EXTERNAL_RESET); if (mcu & RST_BROWN_OUT) SERIAL_ECHOLNPGM(STR_BROWNOUT_RESET); - if (mcu & RST_WATCHDOG) SERIAL_ECHOLNPGM(STR_WATCHDOG_RESET); - if (mcu & RST_SOFTWARE) SERIAL_ECHOLNPGM(STR_SOFTWARE_RESET); - HAL_clear_reset_source(); - - SERIAL_ECHOPGM_P(GET_TEXT(MSG_MARLIN)); - SERIAL_CHAR(' '); - SERIAL_ECHOLNPGM(SHORT_BUILD_VERSION); - SERIAL_EOL(); + if (mcu & RST_WATCHDOG) SERIAL_ECHOLNPGM(STR_WATCHDOG_RESET); + if (mcu & RST_SOFTWARE) SERIAL_ECHOLNPGM(STR_SOFTWARE_RESET); + + // Identify myself as Marlin x.x.x + SERIAL_ECHOLNPGM("Marlin " SHORT_BUILD_VERSION); #if defined(STRING_DISTRIBUTION_DATE) && defined(STRING_CONFIG_H_AUTHOR) SERIAL_ECHO_MSG( " Last Updated: " STRING_DISTRIBUTION_DATE " | Author: " STRING_CONFIG_H_AUTHOR ); #endif - SERIAL_ECHO_MSG("Compiled: " __DATE__); - SERIAL_ECHO_MSG(STR_FREE_MEMORY, freeMemory(), STR_PLANNER_BUFFER_BYTES, sizeof(block_t) * (BLOCK_BUFFER_SIZE)); + SERIAL_ECHO_MSG(" Compiled: " __DATE__); + SERIAL_ECHO_MSG(STR_FREE_MEMORY, hal.freeMemory(), STR_PLANNER_BUFFER_BYTES, sizeof(block_t) * (BLOCK_BUFFER_SIZE)); // Some HAL need precise delay adjustment calibrate_delay_loop(); @@ -1133,25 +1283,20 @@ void setup() { SETUP_RUN(controllerFan.setup()); #endif + TERN_(HAS_FANCHECK, fan_check.init()); + // UI must be initialized before EEPROM // (because EEPROM code calls the UI). - #if ENABLED(DWIN_CREALITY_LCD) - delay(800); // Required delay (since boot?) - SERIAL_ECHOPGM("\nDWIN handshake "); - if (DWIN_Handshake()) SERIAL_ECHOLNPGM("ok."); else SERIAL_ECHOLNPGM("error."); - DWIN_Frame_SetDir(1); // Orientation 90° - DWIN_UpdateLCD(); // Show bootscreen (first image) - #else - SETUP_RUN(ui.init()); - #if HAS_WIRED_LCD && ENABLED(SHOW_BOOTSCREEN) - SETUP_RUN(ui.show_bootscreen()); - #endif - SETUP_RUN(ui.reset_status()); // Load welcome message early. (Retained if no errors exist.) - #endif + SETUP_RUN(ui.init()); - #if ENABLED(PROBE_TARE) - SETUP_RUN(probe.tare_init()); + #if PIN_EXISTS(SAFE_POWER) + #if HAS_DRIVER_SAFE_POWER_PROTECT + SETUP_RUN(stepper_driver_backward_check()); + #else + SETUP_LOG("SAFE_POWER"); + OUT_WRITE(SAFE_POWER_PIN, HIGH); + #endif #endif #if BOTH(SDSUPPORT, SDCARD_EEPROM_EMULATION) @@ -1161,12 +1306,21 @@ void setup() { SETUP_RUN(settings.first_load()); // Load data from EEPROM if available (or use defaults) // This also updates variables in the planner, elsewhere + #if BOTH(HAS_WIRED_LCD, SHOW_BOOTSCREEN) + SETUP_RUN(ui.show_bootscreen()); + const millis_t bootscreen_ms = millis(); + #endif + + #if ENABLED(PROBE_TARE) + SETUP_RUN(probe.tare_init()); + #endif + #if HAS_ETHERNET SETUP_RUN(ethernet.init()); #endif #if HAS_TOUCH_BUTTONS - SETUP_RUN(touch.init()); + SETUP_RUN(touchBt.init()); #endif TERN_(HAS_M206_COMMAND, current_position += home_offset); // Init current position based on home_offset @@ -1179,6 +1333,10 @@ void setup() { SETUP_RUN(endstops.init()); // Init endstops and pullups + #if ENABLED(DELTA) && !HAS_SOFTWARE_ENDSTOPS + SETUP_RUN(refresh_delta_clip_start_height()); // Init safe delta height without soft endstops + #endif + SETUP_RUN(stepper.init()); // Init stepper. This enables interrupts! #if HAS_SERVOS @@ -1205,6 +1363,9 @@ void setup() { #endif #if HAS_BED_PROBE + #if PIN_EXISTS(PROBE_ENABLE) + OUT_WRITE(PROBE_ENABLE_PIN, LOW); // Disable + #endif SETUP_RUN(endstops.enable_z_probe(false)); #endif @@ -1216,7 +1377,7 @@ void setup() { SETUP_RUN(digipot_i2c.init()); #endif - #if ENABLED(HAS_MOTOR_CURRENT_DAC) + #if HAS_MOTOR_CURRENT_DAC SETUP_RUN(stepper_dac.init()); #endif @@ -1311,7 +1472,6 @@ void setup() { #if PIN_EXISTS(STAT_LED_RED) OUT_WRITE(STAT_LED_RED_PIN, LOW); // OFF #endif - #if PIN_EXISTS(STAT_LED_BLUE) OUT_WRITE(STAT_LED_BLUE_PIN, LOW); // OFF #endif @@ -1321,10 +1481,7 @@ void setup() { #endif #if HAS_PRUSA_MMU1 - SETUP_LOG("Prusa MMU1"); - SET_OUTPUT(E_MUX0_PIN); - SET_OUTPUT(E_MUX1_PIN); - SET_OUTPUT(E_MUX2_PIN); + SETUP_RUN(mmu_init()); #endif #if HAS_FANMUX @@ -1339,6 +1496,10 @@ void setup() { SETUP_RUN(bltouch.init(/*set_voltage=*/true)); #endif + #if ENABLED(MAGLEV4) + OUT_WRITE(MAGLEV_TRIGGER_PIN, LOW); + #endif + #if ENABLED(I2C_POSITION_ENCODERS) SETUP_RUN(I2CPEM.init()); #endif @@ -1364,24 +1525,18 @@ void setup() { #endif #endif - #if ENABLED(MAGNETIC_PARKING_EXTRUDER) - SETUP_RUN(mpe_settings_init()); - #endif - #if ENABLED(PARKING_EXTRUDER) SETUP_RUN(pe_solenoid_init()); - #endif - - #if ENABLED(SWITCHING_TOOLHEAD) + #elif ENABLED(MAGNETIC_PARKING_EXTRUDER) + SETUP_RUN(mpe_settings_init()); + #elif ENABLED(SWITCHING_TOOLHEAD) SETUP_RUN(swt_init()); - #endif - - #if ENABLED(ELECTROMAGNETIC_SWITCHING_TOOLHEAD) + #elif ENABLED(ELECTROMAGNETIC_SWITCHING_TOOLHEAD) SETUP_RUN(est_init()); #endif #if ENABLED(USE_WATCHDOG) - SETUP_RUN(watchdog_init()); // Reinit watchdog after HAL_get_reset_source call + SETUP_RUN(watchdog_init()); // Reinit watchdog after hal.get_reset_source call #endif #if ENABLED(EXTERNAL_CLOSED_LOOP_CONTROLLER) @@ -1390,15 +1545,19 @@ void setup() { #ifdef STARTUP_COMMANDS SETUP_LOG("STARTUP_COMMANDS"); - queue.inject_P(PSTR(STARTUP_COMMANDS)); + queue.inject(F(STARTUP_COMMANDS)); #endif #if ENABLED(HOST_PROMPT_SUPPORT) - SETUP_RUN(host_action_prompt_end()); + SETUP_RUN(hostui.prompt_end()); #endif #if HAS_TRINAMIC_CONFIG && DISABLED(PSU_DEFAULT_OFF) - SETUP_RUN(test_tmc_connection(true, true, true, true)); + SETUP_RUN(test_tmc_connection()); + #endif + + #if HAS_DRIVER_SAFE_POWER_PROTECT + SETUP_RUN(stepper_driver_backward_report()); #endif #if HAS_PRUSA_MMU2 @@ -1411,14 +1570,16 @@ void setup() { SERIAL_ECHO_TERNARY(err, "BL24CXX Check ", "failed", "succeeded", "!\n"); #endif - #if ENABLED(DWIN_CREALITY_LCD) + #if HAS_DWIN_E3V2_BASIC + SETUP_LOG("E3V2 Init"); Encoder_Configuration(); HMI_Init(); + HMI_SetLanguageCache(); HMI_StartFrame(true); #endif - #if HAS_SERVICE_INTERVALS && DISABLED(DWIN_CREALITY_LCD) - ui.reset_status(true); // Show service messages or keep current status + #if HAS_SERVICE_INTERVALS && !HAS_DWIN_E3V2_BASIC + SETUP_RUN(ui.reset_status(true)); // Show service messages or keep current status #endif #if ENABLED(MAX7219_DEBUG) @@ -1436,12 +1597,24 @@ void setup() { SETUP_RUN(tft_lvgl_init()); #endif + #if BOTH(HAS_WIRED_LCD, SHOW_BOOTSCREEN) + const millis_t elapsed = millis() - bootscreen_ms; + #if ENABLED(MARLIN_DEV_MODE) + SERIAL_ECHOLNPGM("elapsed=", elapsed); + #endif + SETUP_RUN(ui.bootscreen_completion(elapsed)); + #endif + #if ENABLED(PASSWORD_ON_STARTUP) SETUP_RUN(password.lock_machine()); // Will not proceed until correct password provided #endif - #if BOTH(HAS_LCD_MENU, TOUCH_SCREEN_CALIBRATION) && EITHER(TFT_CLASSIC_UI, TFT_COLOR_UI) - ui.check_touch_calibration(); + #if BOTH(HAS_MARLINUI_MENU, TOUCH_SCREEN_CALIBRATION) && EITHER(TFT_CLASSIC_UI, TFT_COLOR_UI) + SETUP_RUN(ui.check_touch_calibration()); + #endif + + #if ENABLED(EASYTHREED_UI) + SETUP_RUN(easythreed_ui.init()); #endif marlin_state = MF_RUNNING; @@ -1473,6 +1646,10 @@ void loop() { queue.advance(); + #if EITHER(POWER_OFF_TIMER, POWER_OFF_WAIT_FOR_COOLDOWN) + powerManager.checkAutoPowerOff(); + #endif + endstops.event_handler(); TERN_(HAS_TFT_LVGL_UI, printer_state_polling()); diff --git a/Marlin/src/MarlinCore.h b/Marlin/src/MarlinCore.h index d43d46bbd8e1..7063c7e2de38 100644 --- a/Marlin/src/MarlinCore.h +++ b/Marlin/src/MarlinCore.h @@ -23,10 +23,6 @@ #include "inc/MarlinConfig.h" -#ifdef DEBUG_GCODE_PARSER - #include "gcode/parser.h" -#endif - #include #include #include @@ -34,42 +30,34 @@ void stop(); // Pass true to keep steppers from timing out -void idle(TERN_(ADVANCED_PAUSE_FEATURE, bool no_stepper_sleep=false)); -inline void idle_no_sleep() { idle(TERN_(ADVANCED_PAUSE_FEATURE, true)); } +void idle(bool no_stepper_sleep=false); +inline void idle_no_sleep() { idle(true); } #if ENABLED(G38_PROBE_TARGET) extern uint8_t G38_move; // Flag to tell the ISR that G38 is in progress, and the type extern bool G38_did_trigger; // Flag from the ISR to indicate the endstop changed #endif -/** - * The axis order in all axis related arrays is X, Y, Z, E - */ -void enable_e_steppers(); -void enable_all_steppers(); -void disable_e_stepper(const uint8_t e); -void disable_e_steppers(); -void disable_all_steppers(); - -void kill(PGM_P const lcd_error=nullptr, PGM_P const lcd_component=nullptr, const bool steppers_off=false); +void kill(FSTR_P const lcd_error=nullptr, FSTR_P const lcd_component=nullptr, const bool steppers_off=false); void minkill(const bool steppers_off=false); // Global State of the firmware enum MarlinState : uint8_t { - MF_INITIALIZING = 0, - MF_RUNNING = _BV(0), - MF_PAUSED = _BV(1), - MF_WAITING = _BV(2), - MF_STOPPED = _BV(3), - MF_SD_COMPLETE = _BV(4), - MF_KILLED = _BV(7) + MF_INITIALIZING = 0, + MF_STOPPED, + MF_KILLED, + MF_RUNNING, + MF_SD_COMPLETE, + MF_PAUSED, + MF_WAITING, }; extern MarlinState marlin_state; -inline bool IsRunning() { return marlin_state == MF_RUNNING; } -inline bool IsStopped() { return marlin_state != MF_RUNNING; } +inline bool IsRunning() { return marlin_state >= MF_RUNNING; } +inline bool IsStopped() { return marlin_state == MF_STOPPED; } bool printingIsActive(); +bool printJobOngoing(); bool printingIsPaused(); void startOrResumeJob(); @@ -80,25 +68,10 @@ extern bool wait_for_heatup; void wait_for_user_response(millis_t ms=0, const bool no_sleep=false); #endif -#if ENABLED(PSU_CONTROL) - extern bool powersupply_on; - #define PSU_PIN_ON() do{ OUT_WRITE(PS_ON_PIN, PSU_ACTIVE_STATE); powersupply_on = true; }while(0) - #define PSU_PIN_OFF() do{ OUT_WRITE(PS_ON_PIN, !PSU_ACTIVE_STATE); powersupply_on = false; }while(0) - #if ENABLED(AUTO_POWER_CONTROL) - #define PSU_ON() powerManager.power_on() - #define PSU_OFF() powerManager.power_off() - #define PSU_OFF_SOON() powerManager.power_off_soon() - #else - #define PSU_ON() PSU_PIN_ON() - #define PSU_OFF() PSU_PIN_OFF() - #define PSU_OFF_SOON PSU_OFF - #endif -#endif - bool pin_is_protected(const pin_t pin); #if HAS_SUICIDE - inline void suicide() { OUT_WRITE(SUICIDE_PIN, SUICIDE_PIN_INVERTING); } + inline void suicide() { OUT_WRITE(SUICIDE_PIN, SUICIDE_PIN_STATE); } #endif #if HAS_KILL diff --git a/Marlin/src/core/boards.h b/Marlin/src/core/boards.h index cfde52ddb918..4435452005ba 100644 --- a/Marlin/src/core/boards.h +++ b/Marlin/src/core/boards.h @@ -68,8 +68,8 @@ #define BOARD_MKS_GEN_13 1112 // MKS GEN v1.3 or 1.4 #define BOARD_MKS_GEN_L 1113 // MKS GEN L #define BOARD_KFB_2 1114 // BigTreeTech or BIQU KFB2.0 -#define BOARD_ZRIB_V20 1115 // zrib V2.0 control board (Chinese knock off RAMPS replica) -#define BOARD_ZRIB_V52 1116 // zrib V5.2 control board (Chinese knock off RAMPS replica) +#define BOARD_ZRIB_V20 1115 // zrib V2.0 (Chinese RAMPS replica) +#define BOARD_ZRIB_V52 1116 // zrib V5.2 (Chinese RAMPS replica) #define BOARD_FELIX2 1117 // Felix 2.0+ Electronics Board (RAMPS like) #define BOARD_RIGIDBOARD 1118 // Invent-A-Part RigidBoard #define BOARD_RIGIDBOARD_V2 1119 // Invent-A-Part RigidBoard V2 @@ -87,7 +87,7 @@ #define BOARD_FORMBOT_RAPTOR 1131 // Formbot Raptor #define BOARD_FORMBOT_RAPTOR2 1132 // Formbot Raptor 2 #define BOARD_BQ_ZUM_MEGA_3D 1133 // bq ZUM Mega 3D -#define BOARD_MAKEBOARD_MINI 1134 // MakeBoard Mini v2.1.2 is a control board sold by MicroMake +#define BOARD_MAKEBOARD_MINI 1134 // MakeBoard Mini v2.1.2 by MicroMake #define BOARD_TRIGORILLA_13 1135 // TriGorilla Anycubic version 1.3-based on RAMPS EFB #define BOARD_TRIGORILLA_14 1136 // ... Ver 1.4 #define BOARD_TRIGORILLA_14_11 1137 // ... Rev 1.1 (new servo pin order) @@ -115,6 +115,8 @@ #define BOARD_RAMPS_S_12_EFFB 1159 // Ramps S 1.2 by Sakul.cz (Power outputs: Hotend, Fan0, Fan1, Bed) #define BOARD_LONGER3D_LK1_PRO 1160 // Longer LK1 PRO / Alfawise U20 Pro (PRO version) #define BOARD_LONGER3D_LKx_PRO 1161 // Longer LKx PRO / Alfawise Uxx Pro (PRO version) +#define BOARD_ZRIB_V53 1162 // Zonestar zrib V5.3 (Chinese RAMPS replica) +#define BOARD_PXMALION_CORE_I3 1163 // Pxmalion Core I3 // // RAMBo and derivatives @@ -146,17 +148,22 @@ #define BOARD_ELEFU_3 1311 // Elefu Ra Board (v3) #define BOARD_LEAPFROG 1312 // Leapfrog #define BOARD_MEGACONTROLLER 1313 // Mega controller -#define BOARD_GT2560_REV_A 1314 // Geeetech GT2560 Rev. A -#define BOARD_GT2560_REV_A_PLUS 1315 // Geeetech GT2560 Rev. A+ (with auto level probe) -#define BOARD_GT2560_V3 1316 // Geeetech GT2560 Rev B for A10(M/D) -#define BOARD_GT2560_V3_MC2 1317 // Geeetech GT2560 Rev B for Mecreator2 -#define BOARD_GT2560_V3_A20 1318 // Geeetech GT2560 Rev B for A20(M/D) -#define BOARD_EINSTART_S 1319 // Einstart retrofit -#define BOARD_WANHAO_ONEPLUS 1320 // Wanhao 0ne+ i3 Mini -#define BOARD_LEAPFROG_XEED2015 1321 // Leapfrog Xeed 2015 -#define BOARD_PICA_REVB 1322 // PICA Shield (original version) -#define BOARD_PICA 1323 // PICA Shield (rev C or later) -#define BOARD_INTAMSYS40 1324 // Intamsys 4.0 (Funmat HT) +#define BOARD_GT2560_REV_A 1314 // Geeetech GT2560 Rev A +#define BOARD_GT2560_REV_A_PLUS 1315 // Geeetech GT2560 Rev A+ (with auto level probe) +#define BOARD_GT2560_REV_B 1316 // Geeetech GT2560 Rev B +#define BOARD_GT2560_V3 1317 // Geeetech GT2560 Rev B for A10(M/T/D) +#define BOARD_GT2560_V4 1318 // Geeetech GT2560 Rev B for A10(M/T/D) +#define BOARD_GT2560_V3_MC2 1319 // Geeetech GT2560 Rev B for Mecreator2 +#define BOARD_GT2560_V3_A20 1320 // Geeetech GT2560 Rev B for A20(M/T/D) +#define BOARD_EINSTART_S 1321 // Einstart retrofit +#define BOARD_WANHAO_ONEPLUS 1322 // Wanhao 0ne+ i3 Mini +#define BOARD_LEAPFROG_XEED2015 1323 // Leapfrog Xeed 2015 +#define BOARD_PICA_REVB 1324 // PICA Shield (original version) +#define BOARD_PICA 1325 // PICA Shield (rev C or later) +#define BOARD_INTAMSYS40 1326 // Intamsys 4.0 (Funmat HT) +#define BOARD_MALYAN_M180 1327 // Malyan M180 Mainboard Version 2 (no display function, direct gcode only) +#define BOARD_GT2560_V4_A20 1328 // Geeetech GT2560 Rev B for A20(M/T/D) +#define BOARD_PROTONEER_CNC_SHIELD_V3 1329 // Mega controller & Protoneer CNC Shield V3.00 // // ATmega1281, ATmega2561 @@ -174,8 +181,8 @@ #define BOARD_MELZI 1502 // Melzi #define BOARD_MELZI_V2 1503 // Melzi V2 #define BOARD_MELZI_MAKR3D 1504 // Melzi with ATmega1284 (MaKr3d version) -#define BOARD_MELZI_CREALITY 1505 // Melzi Creality3D board (for CR-10 etc) -#define BOARD_MELZI_MALYAN 1506 // Melzi Malyan M150 board +#define BOARD_MELZI_CREALITY 1505 // Melzi Creality3D (for CR-10 etc) +#define BOARD_MELZI_MALYAN 1506 // Melzi Malyan M150 #define BOARD_MELZI_TRONXY 1507 // Tronxy X5S #define BOARD_STB_11 1508 // STB V1.1 #define BOARD_AZTEEG_X1 1509 // Azteeg X1 @@ -194,8 +201,8 @@ #define BOARD_GEN7_12 1605 // Gen7 v1.1, v1.2 #define BOARD_GEN7_13 1606 // Gen7 v1.3 #define BOARD_GEN7_14 1607 // Gen7 v1.4 -#define BOARD_OMCA_A 1608 // Alpha OMCA board -#define BOARD_OMCA 1609 // Final OMCA board +#define BOARD_OMCA_A 1608 // Alpha OMCA +#define BOARD_OMCA 1609 // Final OMCA #define BOARD_SETHI 1610 // Sethi 3D_1 // @@ -226,7 +233,7 @@ #define BOARD_SELENA_COMPACT 2008 // Selena Compact (Power outputs: Hotend0, Hotend1, Bed0, Bed1, Fan0, Fan1) #define BOARD_BIQU_B300_V1_0 2009 // BIQU B300_V1.0 (Power outputs: Hotend0, Fan, Bed, SPI Driver) #define BOARD_MKS_SGEN_L 2010 // MKS-SGen-L (Power outputs: Hotend0, Hotend1, Bed, Fan) -#define BOARD_GMARSH_X6_REV1 2011 // GMARSH X6 board, revision 1 prototype +#define BOARD_GMARSH_X6_REV1 2011 // GMARSH X6, revision 1 prototype #define BOARD_BTT_SKR_V1_1 2012 // BigTreeTech SKR v1.1 (Power outputs: Hotend0, Hotend1, Fan, Bed) #define BOARD_BTT_SKR_V1_3 2013 // BigTreeTech SKR v1.3 (Power outputs: Hotend0, Hotend1, Fan, Bed) #define BOARD_BTT_SKR_V1_4 2014 // BigTreeTech SKR v1.4 (Power outputs: Hotend0, Hotend1, Fan, Bed) @@ -246,7 +253,7 @@ #define BOARD_BTT_SKR_V1_4_TURBO 2508 // BigTreeTech SKR v1.4 TURBO (Power outputs: Hotend0, Hotend1, Fan, Bed) #define BOARD_MKS_SGEN_L_V2 2509 // MKS SGEN_L V2 (Power outputs: Hotend0, Hotend1, Bed, Fan) #define BOARD_BTT_SKR_E3_TURBO 2510 // BigTreeTech SKR E3 Turbo (Power outputs: Hotend0, Hotend1, Bed, Fan0, Fan1) -#define BOARD_FLY_CDY 2511 // FLY_CDY (Power outputs: Hotend0, Hotend1, Hotend2, Bed, Fan0, Fan1, Fan2) +#define BOARD_FLY_CDY 2511 // FLYmaker FLY CDY (Power outputs: Hotend0, Hotend1, Hotend2, Bed, Fan0, Fan1, Fan2) // // SAM3X8E ARM Cortex M3 @@ -279,6 +286,7 @@ #define BOARD_ARCHIM2 3024 // UltiMachine Archim2 (with TMC2130 drivers) #define BOARD_ALLIGATOR 3025 // Alligator Board R2 #define BOARD_CNCONTROLS_15D 3026 // Cartesio CN Controls V15 on DUE +#define BOARD_KRATOS32 3027 // K.3D Kratos32 (Arduino Due Shield) // // SAM3X8C ARM Cortex M3 @@ -294,52 +302,67 @@ #define BOARD_MALYAN_M200_V2 4000 // STM32F070CB controller #define BOARD_MALYAN_M300 4001 // STM32F070-based delta #define BOARD_STM32F103RE 4002 // STM32F103RE Libmaple-based STM32F1 controller -#define BOARD_MALYAN_M200 4003 // STM32C8T6 Libmaple-based STM32F1 controller +#define BOARD_MALYAN_M200 4003 // STM32C8 Libmaple-based STM32F1 controller #define BOARD_STM3R_MINI 4004 // STM32F103RE Libmaple-based STM32F1 controller -#define BOARD_GTM32_PRO_VB 4005 // STM32F103VET6 controller -#define BOARD_GTM32_MINI 4006 // STM32F103VET6 controller -#define BOARD_GTM32_MINI_A30 4007 // STM32F103VET6 controller -#define BOARD_GTM32_REV_B 4008 // STM32F103VET6 controller +#define BOARD_GTM32_PRO_VB 4005 // STM32F103VE controller +#define BOARD_GTM32_MINI 4006 // STM32F103VE controller +#define BOARD_GTM32_MINI_A30 4007 // STM32F103VE controller +#define BOARD_GTM32_REV_B 4008 // STM32F103VE controller #define BOARD_MORPHEUS 4009 // STM32F103C8 / STM32F103CB Libmaple-based STM32F1 controller -#define BOARD_CHITU3D 4010 // Chitu3D (STM32F103RET6) -#define BOARD_MKS_ROBIN 4011 // MKS Robin (STM32F103ZET6) -#define BOARD_MKS_ROBIN_MINI 4012 // MKS Robin Mini (STM32F103VET6) -#define BOARD_MKS_ROBIN_NANO 4013 // MKS Robin Nano (STM32F103VET6) -#define BOARD_MKS_ROBIN_NANO_V2 4014 // MKS Robin Nano V2 (STM32F103VET6) -#define BOARD_MKS_ROBIN_LITE 4015 // MKS Robin Lite/Lite2 (STM32F103RCT6) -#define BOARD_MKS_ROBIN_LITE3 4016 // MKS Robin Lite3 (STM32F103RCT6) -#define BOARD_MKS_ROBIN_PRO 4017 // MKS Robin Pro (STM32F103ZET6) -#define BOARD_MKS_ROBIN_E3 4018 // MKS Robin E3 (STM32F103RCT6) -#define BOARD_MKS_ROBIN_E3_V1_1 4019 // MKS Robin E3 V1.1 (STM32F103RCT6) -#define BOARD_MKS_ROBIN_E3D 4020 // MKS Robin E3D (STM32F103RCT6) -#define BOARD_MKS_ROBIN_E3D_V1_1 4021 // MKS Robin E3D V1.1 (STM32F103RCT6) -#define BOARD_MKS_ROBIN_E3P 4022 // MKS Robin E3p (STM32F103VET6) +#define BOARD_CHITU3D 4010 // Chitu3D (STM32F103RE) +#define BOARD_MKS_ROBIN 4011 // MKS Robin (STM32F103ZE) +#define BOARD_MKS_ROBIN_MINI 4012 // MKS Robin Mini (STM32F103VE) +#define BOARD_MKS_ROBIN_NANO 4013 // MKS Robin Nano (STM32F103VE) +#define BOARD_MKS_ROBIN_NANO_V2 4014 // MKS Robin Nano V2 (STM32F103VE) +#define BOARD_MKS_ROBIN_LITE 4015 // MKS Robin Lite/Lite2 (STM32F103RC) +#define BOARD_MKS_ROBIN_LITE3 4016 // MKS Robin Lite3 (STM32F103RC) +#define BOARD_MKS_ROBIN_PRO 4017 // MKS Robin Pro (STM32F103ZE) +#define BOARD_MKS_ROBIN_E3 4018 // MKS Robin E3 (STM32F103RC) +#define BOARD_MKS_ROBIN_E3_V1_1 4019 // MKS Robin E3 V1.1 (STM32F103RC) +#define BOARD_MKS_ROBIN_E3D 4020 // MKS Robin E3D (STM32F103RC) +#define BOARD_MKS_ROBIN_E3D_V1_1 4021 // MKS Robin E3D V1.1 (STM32F103RC) +#define BOARD_MKS_ROBIN_E3P 4022 // MKS Robin E3p (STM32F103VE) #define BOARD_BTT_SKR_MINI_V1_1 4023 // BigTreeTech SKR Mini v1.1 (STM32F103RC) #define BOARD_BTT_SKR_MINI_E3_V1_0 4024 // BigTreeTech SKR Mini E3 (STM32F103RC) #define BOARD_BTT_SKR_MINI_E3_V1_2 4025 // BigTreeTech SKR Mini E3 V1.2 (STM32F103RC) -#define BOARD_BTT_SKR_MINI_E3_V2_0 4026 // BigTreeTech SKR Mini E3 V2.0 (STM32F103RC) -#define BOARD_BTT_SKR_MINI_MZ_V1_0 4027 // BigTreeTech SKR Mini MZ V1.0 (STM32F103RC) -#define BOARD_BTT_SKR_E3_DIP 4028 // BigTreeTech SKR E3 DIP V1.0 (STM32F103RC / STM32F103RE) -#define BOARD_BTT_SKR_CR6 4029 // BigTreeTech SKR CR6 v1.0 (STM32F103RE) -#define BOARD_JGAURORA_A5S_A1 4030 // JGAurora A5S A1 (STM32F103ZET6) -#define BOARD_FYSETC_AIO_II 4031 // FYSETC AIO_II -#define BOARD_FYSETC_CHEETAH 4032 // FYSETC Cheetah -#define BOARD_FYSETC_CHEETAH_V12 4033 // FYSETC Cheetah V1.2 -#define BOARD_LONGER3D_LK 4034 // Alfawise U20/U20+/U30 (Longer3D LK1/2) / STM32F103VET6 -#define BOARD_CCROBOT_MEEB_3DP 4035 // ccrobot-online.com MEEB_3DP (STM32F103RC) -#define BOARD_CHITU3D_V5 4036 // Chitu3D TronXY X5SA V5 Board -#define BOARD_CHITU3D_V6 4037 // Chitu3D TronXY X5SA V6 Board -#define BOARD_CREALITY_V4 4038 // Creality v4.x (STM32F103RE) -#define BOARD_CREALITY_V427 4039 // Creality v4.2.7 (STM32F103RE) -#define BOARD_CREALITY_V4210 4040 // Creality v4.2.10 (STM32F103RE) as found in the CR-30 -#define BOARD_CREALITY_V431 4041 // Creality v4.3.1 (STM32F103RE) -#define BOARD_CREALITY_V452 4042 // Creality v4.5.2 (STM32F103RE) -#define BOARD_CREALITY_V453 4043 // Creality v4.5.3 (STM32F103RE) -#define BOARD_TRIGORILLA_PRO 4044 // Trigorilla Pro (STM32F103ZET6) -#define BOARD_FLY_MINI 4045 // FLY MINI (STM32F103RCT6) -#define BOARD_FLSUN_HISPEED 4046 // FLSUN HiSpeedV1 (STM32F103VET6) -#define BOARD_BEAST 4047 // STM32F103RET6 Libmaple-based controller -#define BOARD_MINGDA_MPX_ARM_MINI 4048 // STM32F103ZET6 Mingda MD-16 +#define BOARD_BTT_SKR_MINI_E3_V2_0 4026 // BigTreeTech SKR Mini E3 V2.0 (STM32F103RC / STM32F103RE) +#define BOARD_BTT_SKR_MINI_E3_V3_0 4027 // BigTreeTech SKR Mini E3 V3.0 (STM32G0B1RE) +#define BOARD_BTT_SKR_MINI_MZ_V1_0 4028 // BigTreeTech SKR Mini MZ V1.0 (STM32F103RC) +#define BOARD_BTT_SKR_E3_DIP 4029 // BigTreeTech SKR E3 DIP V1.0 (STM32F103RC / STM32F103RE) +#define BOARD_BTT_SKR_CR6 4030 // BigTreeTech SKR CR6 v1.0 (STM32F103RE) +#define BOARD_JGAURORA_A5S_A1 4031 // JGAurora A5S A1 (STM32F103ZE) +#define BOARD_FYSETC_AIO_II 4032 // FYSETC AIO_II +#define BOARD_FYSETC_CHEETAH 4033 // FYSETC Cheetah +#define BOARD_FYSETC_CHEETAH_V12 4034 // FYSETC Cheetah V1.2 +#define BOARD_LONGER3D_LK 4035 // Alfawise U20/U20+/U30 (Longer3D LK1/2) / STM32F103VE +#define BOARD_CCROBOT_MEEB_3DP 4036 // ccrobot-online.com MEEB_3DP (STM32F103RC) +#define BOARD_CHITU3D_V5 4037 // Chitu3D TronXY X5SA V5 Board +#define BOARD_CHITU3D_V6 4038 // Chitu3D TronXY X5SA V6 Board +#define BOARD_CHITU3D_V9 4039 // Chitu3D TronXY X5SA V9 Board +#define BOARD_CREALITY_V4 4040 // Creality v4.x (STM32F103RC / STM32F103RE) +#define BOARD_CREALITY_V422 4041 // Creality v4.2.2 (STM32F103RC / STM32F103RE) +#define BOARD_CREALITY_V423 4042 // Creality v4.2.3 (STM32F103RC / STM32F103RE) +#define BOARD_CREALITY_V427 4043 // Creality v4.2.7 (STM32F103RC / STM32F103RE) +#define BOARD_CREALITY_V4210 4044 // Creality v4.2.10 (STM32F103RC / STM32F103RE) as found in the CR-30 +#define BOARD_CREALITY_V431 4045 // Creality v4.3.1 (STM32F103RC / STM32F103RE) +#define BOARD_CREALITY_V431_A 4046 // Creality v4.3.1a (STM32F103RC / STM32F103RE) +#define BOARD_CREALITY_V431_B 4047 // Creality v4.3.1b (STM32F103RC / STM32F103RE) +#define BOARD_CREALITY_V431_C 4048 // Creality v4.3.1c (STM32F103RC / STM32F103RE) +#define BOARD_CREALITY_V431_D 4049 // Creality v4.3.1d (STM32F103RC / STM32F103RE) +#define BOARD_CREALITY_V452 4050 // Creality v4.5.2 (STM32F103RC / STM32F103RE) +#define BOARD_CREALITY_V453 4051 // Creality v4.5.3 (STM32F103RC / STM32F103RE) +#define BOARD_CREALITY_V24S1 4052 // Creality v2.4.S1 (STM32F103RC / STM32F103RE) v101 as found in the Ender 7 +#define BOARD_CREALITY_V24S1_301 4053 // Creality v2.4.S1_301 (STM32F103RC / STM32F103RE) as found in the Ender 3 S1 +#define BOARD_TRIGORILLA_PRO 4054 // Trigorilla Pro (STM32F103ZE) +#define BOARD_FLY_MINI 4055 // FLYmaker FLY MINI (STM32F103RC) +#define BOARD_FLSUN_HISPEED 4056 // FLSUN HiSpeedV1 (STM32F103VE) +#define BOARD_BEAST 4057 // STM32F103RE Libmaple-based controller +#define BOARD_MINGDA_MPX_ARM_MINI 4058 // STM32F103ZE Mingda MD-16 +#define BOARD_GTM32_PRO_VD 4059 // STM32F103VE controller +#define BOARD_ZONESTAR_ZM3E2 4060 // Zonestar ZM3E2 (STM32F103RC) +#define BOARD_ZONESTAR_ZM3E4 4061 // Zonestar ZM3E4 V1 (STM32F103VC) +#define BOARD_ZONESTAR_ZM3E4V2 4062 // Zonestar ZM3E4 V2 (STM32F103VC) +#define BOARD_ERYONE_ERY32_MINI 4063 // Eryone Ery32 mini (STM32F103VE) // // ARM Cortex-M4F @@ -353,29 +376,44 @@ // #define BOARD_ARMED 4200 // Arm'ed STM32F4-based controller -#define BOARD_RUMBA32_V1_0 4201 // RUMBA32 STM32F446VET6 based controller from Aus3D -#define BOARD_RUMBA32_V1_1 4202 // RUMBA32 STM32F446VET6 based controller from Aus3D -#define BOARD_RUMBA32_MKS 4203 // RUMBA32 STM32F446VET6 based controller from Makerbase -#define BOARD_BLACK_STM32F407VE 4204 // BLACK_STM32F407VE -#define BOARD_BLACK_STM32F407ZE 4205 // BLACK_STM32F407ZE -#define BOARD_STEVAL_3DP001V1 4206 // STEVAL-3DP001V1 3D PRINTER BOARD -#define BOARD_BTT_SKR_PRO_V1_1 4207 // BigTreeTech SKR Pro v1.1 (STM32F407ZG) -#define BOARD_BTT_SKR_PRO_V1_2 4208 // BigTreeTech SKR Pro v1.2 (STM32F407ZG) -#define BOARD_BTT_BTT002_V1_0 4209 // BigTreeTech BTT002 v1.0 (STM32F407VG) -#define BOARD_BTT_GTR_V1_0 4210 // BigTreeTech GTR v1.0 (STM32F407IGT) -#define BOARD_LERDGE_K 4211 // Lerdge K (STM32F407ZG) -#define BOARD_LERDGE_S 4212 // Lerdge S (STM32F407VE) -#define BOARD_LERDGE_X 4213 // Lerdge X (STM32F407VE) -#define BOARD_VAKE403D 4214 // VAkE 403D (STM32F446VET6) -#define BOARD_FYSETC_S6 4215 // FYSETC S6 board -#define BOARD_FYSETC_S6_V2_0 4216 // FYSETC S6 v2.0 board -#define BOARD_FLYF407ZG 4217 // FLYF407ZG board (STM32F407ZG) -#define BOARD_MKS_ROBIN2 4218 // MKS_ROBIN2 (STM32F407ZE) -#define BOARD_MKS_ROBIN_PRO_V2 4219 // MKS Robin Pro V2 (STM32F407VE) -#define BOARD_MKS_ROBIN_NANO_V3 4220 // MKS Robin Nano V3 (STM32F407VG) -#define BOARD_ANET_ET4 4221 // ANET ET4 V1.x (STM32F407VGT6) -#define BOARD_ANET_ET4P 4222 // ANET ET4P V1.x (STM32F407VGT6) -#define BOARD_FYSETC_CHEETAH_V20 4223 // FYSETC Cheetah V2.0 +#define BOARD_RUMBA32_V1_0 4201 // RUMBA32 STM32F446VE based controller from Aus3D +#define BOARD_RUMBA32_V1_1 4202 // RUMBA32 STM32F446VE based controller from Aus3D +#define BOARD_RUMBA32_MKS 4203 // RUMBA32 STM32F446VE based controller from Makerbase +#define BOARD_RUMBA32_BTT 4204 // RUMBA32 STM32F446VE based controller from BIGTREETECH +#define BOARD_BLACK_STM32F407VE 4205 // BLACK_STM32F407VE +#define BOARD_BLACK_STM32F407ZE 4206 // BLACK_STM32F407ZE +#define BOARD_STEVAL_3DP001V1 4207 // STEVAL-3DP001V1 3D PRINTER BOARD +#define BOARD_BTT_SKR_PRO_V1_1 4208 // BigTreeTech SKR Pro v1.1 (STM32F407ZG) +#define BOARD_BTT_SKR_PRO_V1_2 4209 // BigTreeTech SKR Pro v1.2 (STM32F407ZG) +#define BOARD_BTT_BTT002_V1_0 4210 // BigTreeTech BTT002 v1.0 (STM32F407VG) +#define BOARD_BTT_E3_RRF 4211 // BigTreeTech E3 RRF (STM32F407VG) +#define BOARD_BTT_SKR_V2_0_REV_A 4212 // BigTreeTech SKR v2.0 Rev A (STM32F407VG) +#define BOARD_BTT_SKR_V2_0_REV_B 4213 // BigTreeTech SKR v2.0 Rev B (STM32F407VG/STM32F429VG) +#define BOARD_BTT_GTR_V1_0 4214 // BigTreeTech GTR v1.0 (STM32F407IGT) +#define BOARD_BTT_OCTOPUS_V1_0 4215 // BigTreeTech Octopus v1.0 (STM32F446ZE) +#define BOARD_BTT_OCTOPUS_V1_1 4216 // BigTreeTech Octopus v1.1 (STM32F446ZE) +#define BOARD_BTT_OCTOPUS_PRO_V1_0 4217 // BigTreeTech Octopus Pro v1.0 (STM32F446ZE/STM32F429ZG) +#define BOARD_LERDGE_K 4218 // Lerdge K (STM32F407ZG) +#define BOARD_LERDGE_S 4219 // Lerdge S (STM32F407VE) +#define BOARD_LERDGE_X 4220 // Lerdge X (STM32F407VE) +#define BOARD_VAKE403D 4221 // VAkE 403D (STM32F446VE) +#define BOARD_FYSETC_S6 4222 // FYSETC S6 (STM32F446VE) +#define BOARD_FYSETC_S6_V2_0 4223 // FYSETC S6 v2.0 (STM32F446VE) +#define BOARD_FYSETC_SPIDER 4224 // FYSETC Spider (STM32F446VE) +#define BOARD_FLYF407ZG 4225 // FLYmaker FLYF407ZG (STM32F407ZG) +#define BOARD_MKS_ROBIN2 4226 // MKS_ROBIN2 (STM32F407ZE) +#define BOARD_MKS_ROBIN_PRO_V2 4227 // MKS Robin Pro V2 (STM32F407VE) +#define BOARD_MKS_ROBIN_NANO_V3 4228 // MKS Robin Nano V3 (STM32F407VG) +#define BOARD_MKS_MONSTER8 4229 // MKS Monster8 (STM32F407VG) +#define BOARD_ANET_ET4 4230 // ANET ET4 V1.x (STM32F407VG) +#define BOARD_ANET_ET4P 4231 // ANET ET4P V1.x (STM32F407VG) +#define BOARD_FYSETC_CHEETAH_V20 4232 // FYSETC Cheetah V2.0 +#define BOARD_TH3D_EZBOARD_V2 4233 // TH3D EZBoard v2.0 +#define BOARD_INDEX_REV03 4234 // Index PnP Controller REV03 (STM32F407VE/VG) +#define BOARD_MKS_ROBIN_NANO_V1_3_F4 4235 // MKS Robin Nano V1.3 and MKS Robin Nano-S V1.3 (STM32F407VE) +#define BOARD_MKS_EAGLE 4236 // MKS Eagle (STM32F407VE) +#define BOARD_ARTILLERY_RUBY 4237 // Artillery Ruby (STM32F401RC) +#define BOARD_FYSETC_SPIDER_V2_2 4238 // FYSETC Spider V2.2 (STM32F446VE) // // ARM Cortex M7 @@ -385,22 +423,29 @@ #define BOARD_TEENSY41 5001 // Teensy 4.1 #define BOARD_T41U5XBB 5002 // T41U5XBB Teensy 4.1 breakout board #define BOARD_NUCLEO_F767ZI 5003 // ST NUCLEO-F767ZI Dev Board +#define BOARD_BTT_SKR_SE_BX 5004 // BigTreeTech SKR SE BX (STM32H743II) // // Espressif ESP32 WiFi // #define BOARD_ESPRESSIF_ESP32 6000 // Generic ESP32 -#define BOARD_MRR_ESPA 6001 // MRR ESPA board based on ESP32 (native pins only) -#define BOARD_MRR_ESPE 6002 // MRR ESPE board based on ESP32 (with I2S stepper stream) +#define BOARD_MRR_ESPA 6001 // MRR ESPA based on ESP32 (native pins only) +#define BOARD_MRR_ESPE 6002 // MRR ESPE based on ESP32 (with I2S stepper stream) #define BOARD_E4D_BOX 6003 // E4d@BOX -#define BOARD_FYSETC_E4 6004 // FYSETC E4 +#define BOARD_RESP32_CUSTOM 6004 // Rutilea ESP32 custom board +#define BOARD_FYSETC_E4 6005 // FYSETC E4 +#define BOARD_PANDA_ZHU 6006 // Panda_ZHU +#define BOARD_PANDA_M4 6007 // Panda_M4 +#define BOARD_MKS_TINYBEE 6008 // MKS TinyBee based on ESP32 (with I2S stepper stream) // // SAMD51 ARM Cortex M4 // #define BOARD_AGCM4_RAMPS_144 6100 // RAMPS 1.4.4 +#define BOARD_BRICOLEMON_V1_0 6101 // Bricolemon +#define BOARD_BRICOLEMON_LITE_V1_0 6102 // Bricolemon Lite // // Custom board diff --git a/Marlin/src/core/bug_on.h b/Marlin/src/core/bug_on.h index 8869be8d284f..7f1243ed40b6 100644 --- a/Marlin/src/core/bug_on.h +++ b/Marlin/src/core/bug_on.h @@ -20,17 +20,19 @@ */ #pragma once -// We need SERIAL_ECHOPAIR and macros.h +// We need SERIAL_ECHOPGM and macros.h #include "serial.h" #if ENABLED(POSTMORTEM_DEBUGGING) // Useful macro for stopping the CPU on an unexpected condition - // This is used like SERIAL_ECHOPAIR, that is: a key-value call of the local variables you want + // This is used like SERIAL_ECHOPGM, that is: a key-value call of the local variables you want // to dump to the serial port before stopping the CPU. - #define BUG_ON(V...) do { SERIAL_ECHOPAIR(ONLY_FILENAME, __LINE__, ": "); SERIAL_ECHOLNPAIR(V); SERIAL_FLUSHTX(); *(char*)0 = 42; } while(0) + // \/ Don't replace by SERIAL_ECHOPGM since ONLY_FILENAME cannot be transformed to a PGM string on Arduino and it breaks building + #define BUG_ON(V...) do { SERIAL_ECHO(ONLY_FILENAME); SERIAL_ECHO(__LINE__); SERIAL_ECHOLNPGM(": "); SERIAL_ECHOLNPGM(V); SERIAL_FLUSHTX(); *(char*)0 = 42; } while(0) #elif ENABLED(MARLIN_DEV_MODE) // Don't stop the CPU here, but at least dump the bug on the serial port - #define BUG_ON(V...) do { SERIAL_ECHOPAIR(ONLY_FILENAME, __LINE__, ": BUG!\n"); SERIAL_ECHOLNPAIR(V); SERIAL_FLUSHTX(); } while(0) + // \/ Don't replace by SERIAL_ECHOPGM since ONLY_FILENAME cannot be transformed to a PGM string on Arduino and it breaks building + #define BUG_ON(V...) do { SERIAL_ECHO(ONLY_FILENAME); SERIAL_ECHO(__LINE__); SERIAL_ECHOLNPGM(": BUG!"); SERIAL_ECHOLNPGM(V); SERIAL_FLUSHTX(); } while(0) #else // Release mode, let's ignore the bug #define BUG_ON(V...) NOOP diff --git a/Marlin/src/core/debug_out.h b/Marlin/src/core/debug_out.h index d93decf7acc2..eb1c91e50786 100644 --- a/Marlin/src/core/debug_out.h +++ b/Marlin/src/core/debug_out.h @@ -27,7 +27,6 @@ // #undef DEBUG_SECTION -#undef DEBUG_PRINT_P #undef DEBUG_ECHO_START #undef DEBUG_ERROR_START #undef DEBUG_CHAR @@ -37,12 +36,12 @@ #undef DEBUG_ECHOLN #undef DEBUG_ECHOPGM #undef DEBUG_ECHOLNPGM -#undef DEBUG_ECHOPAIR -#undef DEBUG_ECHOPAIR_P +#undef DEBUG_ECHOF +#undef DEBUG_ECHOLNF +#undef DEBUG_ECHOPGM_P +#undef DEBUG_ECHOLNPGM_P #undef DEBUG_ECHOPAIR_F #undef DEBUG_ECHOPAIR_F_P -#undef DEBUG_ECHOLNPAIR -#undef DEBUG_ECHOLNPAIR_P #undef DEBUG_ECHOLNPAIR_F #undef DEBUG_ECHOLNPAIR_F_P #undef DEBUG_ECHO_MSG @@ -57,9 +56,8 @@ #if DEBUG_OUT #include "debug_section.h" - #define DEBUG_SECTION(N,S,D) SectionLog N(PSTR(S),D) + #define DEBUG_SECTION(N,S,D) SectionLog N(F(S),D) - #define DEBUG_ECHOPGM_P(P) SERIAL_ECHOPGM_P(P) #define DEBUG_ECHO_START SERIAL_ECHO_START #define DEBUG_ERROR_START SERIAL_ERROR_START #define DEBUG_CHAR SERIAL_CHAR @@ -69,12 +67,14 @@ #define DEBUG_ECHOLN SERIAL_ECHOLN #define DEBUG_ECHOPGM SERIAL_ECHOPGM #define DEBUG_ECHOLNPGM SERIAL_ECHOLNPGM - #define DEBUG_ECHOPAIR SERIAL_ECHOPAIR - #define DEBUG_ECHOPAIR_P SERIAL_ECHOPAIR_P + #define DEBUG_ECHOF SERIAL_ECHOF + #define DEBUG_ECHOLNF SERIAL_ECHOLNF + #define DEBUG_ECHOPGM SERIAL_ECHOPGM + #define DEBUG_ECHOPGM_P SERIAL_ECHOPGM_P #define DEBUG_ECHOPAIR_F SERIAL_ECHOPAIR_F #define DEBUG_ECHOPAIR_F_P SERIAL_ECHOPAIR_F_P - #define DEBUG_ECHOLNPAIR SERIAL_ECHOLNPAIR - #define DEBUG_ECHOLNPAIR_P SERIAL_ECHOLNPAIR_P + #define DEBUG_ECHOLNPGM SERIAL_ECHOLNPGM + #define DEBUG_ECHOLNPGM_P SERIAL_ECHOLNPGM_P #define DEBUG_ECHOLNPAIR_F SERIAL_ECHOLNPAIR_F #define DEBUG_ECHOLNPAIR_F_P SERIAL_ECHOLNPAIR_F_P #define DEBUG_ECHO_MSG SERIAL_ECHO_MSG @@ -89,7 +89,6 @@ #else #define DEBUG_SECTION(...) NOOP - #define DEBUG_ECHOPGM_P(P) NOOP #define DEBUG_ECHO_START() NOOP #define DEBUG_ERROR_START() NOOP #define DEBUG_CHAR(...) NOOP @@ -99,12 +98,12 @@ #define DEBUG_ECHOLN(...) NOOP #define DEBUG_ECHOPGM(...) NOOP #define DEBUG_ECHOLNPGM(...) NOOP - #define DEBUG_ECHOPAIR(...) NOOP - #define DEBUG_ECHOPAIR_P(...) NOOP + #define DEBUG_ECHOF(...) NOOP + #define DEBUG_ECHOLNF(...) NOOP + #define DEBUG_ECHOPGM_P(...) NOOP + #define DEBUG_ECHOLNPGM_P(...) NOOP #define DEBUG_ECHOPAIR_F(...) NOOP #define DEBUG_ECHOPAIR_F_P(...) NOOP - #define DEBUG_ECHOLNPAIR(...) NOOP - #define DEBUG_ECHOLNPAIR_P(...) NOOP #define DEBUG_ECHOLNPAIR_F(...) NOOP #define DEBUG_ECHOLNPAIR_F_P(...) NOOP #define DEBUG_ECHO_MSG(...) NOOP diff --git a/Marlin/src/core/debug_section.h b/Marlin/src/core/debug_section.h index 2862d35af132..6e23d9e4edb6 100644 --- a/Marlin/src/core/debug_section.h +++ b/Marlin/src/core/debug_section.h @@ -26,24 +26,24 @@ class SectionLog { public: - SectionLog(PGM_P const msg=nullptr, bool inbug=true) { - the_msg = msg; - if ((debug = inbug)) echo_msg(PSTR(">>>")); + SectionLog(FSTR_P const fmsg=nullptr, bool inbug=true) { + the_msg = fmsg; + if ((debug = inbug)) echo_msg(F(">>>")); } - ~SectionLog() { if (debug) echo_msg(PSTR("<<<")); } + ~SectionLog() { if (debug) echo_msg(F("<<<")); } private: - PGM_P the_msg; + FSTR_P the_msg; bool debug; - void echo_msg(PGM_P const pre) { - SERIAL_ECHOPGM_P(pre); + void echo_msg(FSTR_P const fpre) { + SERIAL_ECHOF(fpre); if (the_msg) { SERIAL_CHAR(' '); - SERIAL_ECHOPGM_P(the_msg); + SERIAL_ECHOF(the_msg); } SERIAL_CHAR(' '); - print_xyz(current_position); + print_pos(current_position); } }; diff --git a/Marlin/src/core/drivers.h b/Marlin/src/core/drivers.h index 3a0e620923ad..0a76410274bb 100644 --- a/Marlin/src/core/drivers.h +++ b/Marlin/src/core/drivers.h @@ -60,6 +60,9 @@ #define AXIS_DRIVER_TYPE_X(T) _AXIS_DRIVER_TYPE(X,T) #define AXIS_DRIVER_TYPE_Y(T) _AXIS_DRIVER_TYPE(Y,T) #define AXIS_DRIVER_TYPE_Z(T) _AXIS_DRIVER_TYPE(Z,T) +#define AXIS_DRIVER_TYPE_I(T) _AXIS_DRIVER_TYPE(I,T) +#define AXIS_DRIVER_TYPE_J(T) _AXIS_DRIVER_TYPE(J,T) +#define AXIS_DRIVER_TYPE_K(T) _AXIS_DRIVER_TYPE(K,T) #define AXIS_DRIVER_TYPE_X2(T) (EITHER(X_DUAL_STEPPER_DRIVERS, DUAL_X_CARRIAGE) && _AXIS_DRIVER_TYPE(X2,T)) #define AXIS_DRIVER_TYPE_Y2(T) (ENABLED(Y_DUAL_STEPPER_DRIVERS) && _AXIS_DRIVER_TYPE(Y2,T)) @@ -83,6 +86,7 @@ #define HAS_E_DRIVER(T) (0 RREPEAT2(E_STEPPERS, _OR_ADTE, T)) #define HAS_DRIVER(T) ( AXIS_DRIVER_TYPE_X(T) || AXIS_DRIVER_TYPE_Y(T) || AXIS_DRIVER_TYPE_Z(T) \ + || AXIS_DRIVER_TYPE_I(T) || AXIS_DRIVER_TYPE_J(T) || AXIS_DRIVER_TYPE_K(T) \ || AXIS_DRIVER_TYPE_X2(T) || AXIS_DRIVER_TYPE_Y2(T) || AXIS_DRIVER_TYPE_Z2(T) \ || AXIS_DRIVER_TYPE_Z3(T) || AXIS_DRIVER_TYPE_Z4(T) || HAS_E_DRIVER(T) ) @@ -153,9 +157,11 @@ #define _OR_EAH(N,T) || AXIS_HAS_##T(E##N) #define E_AXIS_HAS(T) (0 _OR_EAH(0,T) _OR_EAH(1,T) _OR_EAH(2,T) _OR_EAH(3,T) _OR_EAH(4,T) _OR_EAH(5,T) _OR_EAH(6,T) _OR_EAH(7,T)) -#define ANY_AXIS_HAS(T) ( AXIS_HAS_##T(X) || AXIS_HAS_##T(Y) || AXIS_HAS_##T(Z) \ - || AXIS_HAS_##T(X2) || AXIS_HAS_##T(Y2) || AXIS_HAS_##T(Z2) \ - || AXIS_HAS_##T(Z3) || AXIS_HAS_##T(Z4) || E_AXIS_HAS(T) ) +#define ANY_AXIS_HAS(T) ( AXIS_HAS_##T(X) || AXIS_HAS_##T(X2) \ + || AXIS_HAS_##T(Y) || AXIS_HAS_##T(Y2) \ + || AXIS_HAS_##T(Z) || AXIS_HAS_##T(Z2) || AXIS_HAS_##T(Z3) || AXIS_HAS_##T(Z4) \ + || AXIS_HAS_##T(I) || AXIS_HAS_##T(J) || AXIS_HAS_##T(K) \ + || E_AXIS_HAS(T) ) #if ANY_AXIS_HAS(STEALTHCHOP) #define HAS_STEALTHCHOP 1 diff --git a/Marlin/src/core/language.h b/Marlin/src/core/language.h index c9c3fd015341..8e0784f70d76 100644 --- a/Marlin/src/core/language.h +++ b/Marlin/src/core/language.h @@ -48,8 +48,8 @@ // cz Czech // da Danish // de German -// el Greek -// el_gr Greek (Greece) +// el Greek (Greece) +// el_CY Greek (Cyprus) // en English // es Spanish // eu Basque-Euskera @@ -105,6 +105,7 @@ #define STR_ENQUEUEING "enqueueing \"" #define STR_POWERUP "PowerUp" +#define STR_POWEROFF "PowerOff" #define STR_EXTERNAL_RESET " External Reset" #define STR_BROWNOUT_RESET " Brown out Reset" #define STR_WATCHDOG_RESET " Watchdog Reset" @@ -130,6 +131,7 @@ #define STR_COUNT_A " Count A:" #define STR_WATCHDOG_FIRED "Watchdog timeout. Reset required." #define STR_ERR_KILLED "Printer halted. kill() called!" +#define STR_FLOWMETER_FAULT "Coolant flow fault. Flowmeter safety is active. Attention required." #define STR_ERR_STOPPED "Printer stopped due to errors. Fix the error and use M999 to restart. (Temperature is reset. Set it after restarting)" #define STR_ERR_SERIAL_MISMATCH "Serial status mismatch" #define STR_BUSY_PROCESSING "busy: processing" @@ -139,25 +141,8 @@ #define STR_RESEND "Resend: " #define STR_UNKNOWN_COMMAND "Unknown command: \"" #define STR_ACTIVE_EXTRUDER "Active Extruder: " -#define STR_X_MIN "x_min" -#define STR_X_MAX "x_max" -#define STR_X2_MIN "x2_min" -#define STR_X2_MAX "x2_max" -#define STR_Y_MIN "y_min" -#define STR_Y_MAX "y_max" -#define STR_Y2_MIN "y2_min" -#define STR_Y2_MAX "y2_max" -#define STR_Z_MIN "z_min" -#define STR_Z_MAX "z_max" -#define STR_Z2_MIN "z2_min" -#define STR_Z2_MAX "z2_max" -#define STR_Z3_MIN "z3_min" -#define STR_Z3_MAX "z3_max" -#define STR_Z4_MIN "z4_min" -#define STR_Z4_MAX "z4_max" -#define STR_Z_PROBE "z_probe" -#define STR_PROBE_EN "probe_en" -#define STR_FILAMENT_RUNOUT_SENSOR "filament" +#define STR_ERR_FANSPEED "Fan speed E" + #define STR_PROBE_OFFSET "Probe Offset" #define STR_SKEW_MIN "min_skew_factor: " #define STR_SKEW_MAX "max_skew_factor: " @@ -175,15 +160,14 @@ #define STR_OFF "OFF" #define STR_ENDSTOP_HIT "TRIGGERED" #define STR_ENDSTOP_OPEN "open" -#define STR_HOTEND_OFFSET "Hotend offsets:" #define STR_DUPLICATION_MODE "Duplication mode: " -#define STR_SOFT_ENDSTOPS "Soft endstops: " #define STR_SOFT_MIN " Min: " #define STR_SOFT_MAX " Max: " #define STR_SAVED_POS "Position saved" #define STR_RESTORING_POS "Restoring position" #define STR_INVALID_POS_SLOT "Invalid slot. Total: " +#define STR_DONE "Done." #define STR_SD_CANT_OPEN_SUBDIR "Cannot open subdir " #define STR_SD_INIT_FAIL "No SD card" @@ -247,11 +231,17 @@ #define STR_HEATER_BED "bed" #define STR_HEATER_CHAMBER "chamber" +#define STR_COOLER "cooler" +#define STR_MOTHERBOARD "motherboard" +#define STR_PROBE "probe" +#define STR_REDUNDANT "redundant " +#define STR_LASER_TEMP "laser temperature" #define STR_STOPPED_HEATER ", system stopped! Heater_ID: " #define STR_REDUNDANCY "Heater switched off. Temperature difference between temp sensors is too high !" #define STR_T_HEATING_FAILED "Heating failed" #define STR_T_THERMAL_RUNAWAY "Thermal Runaway" +#define STR_T_MALFUNCTION "Thermal Malfunction" #define STR_T_MAXTEMP "MAXTEMP triggered" #define STR_T_MINTEMP "MINTEMP triggered" #define STR_ERR_PROBING_FAILED "Probing Failed" @@ -265,7 +255,7 @@ #define STR_DEBUG_ERRORS "ERRORS" #define STR_DEBUG_DRYRUN "DRYRUN" #define STR_DEBUG_COMMUNICATION "COMMUNICATION" -#define STR_DEBUG_LEVELING "LEVELING" +#define STR_DEBUG_DETAIL "DETAIL" #define STR_PRINTER_LOCKED "Printer locked! (Unlock with M511 or LCD)" #define STR_WRONG_PASSWORD "Incorrect Password" @@ -274,14 +264,82 @@ #define STR_REMINDER_SAVE_SETTINGS "Remember to save!" #define STR_PASSWORD_SET "Password is " -// LCD Menu Messages +// Settings Report Strings +#define STR_Z_AUTO_ALIGN "Z Auto-Align" +#define STR_BACKLASH_COMPENSATION "Backlash compensation" +#define STR_S_SEG_PER_SEC "S" +#define STR_DELTA_SETTINGS "Delta (L R H S XYZ ABC)" +#define STR_SCARA_SETTINGS "SCARA" +#define STR_POLARGRAPH_SETTINGS "Polargraph" +#define STR_SCARA_P_T_Z "P T Z" +#define STR_ENDSTOP_ADJUSTMENT "Endstop adjustment" +#define STR_SKEW_FACTOR "Skew Factor" +#define STR_FILAMENT_SETTINGS "Filament settings" +#define STR_MAX_ACCELERATION "Max Acceleration (units/s2)" +#define STR_MAX_FEEDRATES "Max feedrates (units/s)" +#define STR_ACCELERATION_P_R_T "Acceleration (units/s2) (P R T)" +#define STR_TOOL_CHANGING "Tool-changing" +#define STR_HOTEND_OFFSETS "Hotend offsets" +#define STR_SERVO_ANGLES "Servo Angles" +#define STR_HOTEND_PID "Hotend PID" +#define STR_BED_PID "Bed PID" +#define STR_CHAMBER_PID "Chamber PID" +#define STR_STEPS_PER_UNIT "Steps per unit" +#define STR_LINEAR_ADVANCE "Linear Advance" +#define STR_CONTROLLER_FAN "Controller Fan" +#define STR_STEPPER_MOTOR_CURRENTS "Stepper motor currents" +#define STR_RETRACT_S_F_Z "Retract (S F Z)" +#define STR_RECOVER_S_F "Recover (S F)" +#define STR_AUTO_RETRACT_S "Auto-Retract (S)" +#define STR_FILAMENT_LOAD_UNLOAD "Filament load/unload" +#define STR_POWER_LOSS_RECOVERY "Power-loss recovery" +#define STR_FILAMENT_RUNOUT_SENSOR "Filament runout sensor" +#define STR_DRIVER_STEPPING_MODE "Driver stepping mode" +#define STR_STEPPER_DRIVER_CURRENT "Stepper driver current" +#define STR_HYBRID_THRESHOLD "Hybrid Threshold" +#define STR_STALLGUARD_THRESHOLD "StallGuard threshold" +#define STR_HOME_OFFSET "Home offset" +#define STR_SOFT_ENDSTOPS "Soft endstops" +#define STR_MATERIAL_HEATUP "Material heatup parameters" +#define STR_LCD_CONTRAST "LCD Contrast" +#define STR_LCD_BRIGHTNESS "LCD Brightness" +#define STR_UI_LANGUAGE "UI Language" +#define STR_Z_PROBE_OFFSET "Z-Probe Offset" +#define STR_TEMPERATURE_UNITS "Temperature Units" +#define STR_USER_THERMISTORS "User thermistors" +#define STR_DELAYED_POWEROFF "Delayed poweroff" -#define LANGUAGE_DATA_INCL_(M) STRINGIFY_(fontdata/langdata_##M.h) -#define LANGUAGE_DATA_INCL(M) LANGUAGE_DATA_INCL_(M) +// +// Endstop Names used by Endstops::report_states +// +#define STR_X_MIN "x_min" +#define STR_X_MAX "x_max" +#define STR_X2_MIN "x2_min" +#define STR_X2_MAX "x2_max" -#define LANGUAGE_INCL_(M) STRINGIFY_(../lcd/language/language_##M.h) -#define LANGUAGE_INCL(M) LANGUAGE_INCL_(M) +#if HAS_Y_AXIS + #define STR_Y_MIN "y_min" + #define STR_Y_MAX "y_max" + #define STR_Y2_MIN "y2_min" + #define STR_Y2_MAX "y2_max" +#endif + +#if HAS_Z_AXIS + #define STR_Z_MIN "z_min" + #define STR_Z_MAX "z_max" + #define STR_Z2_MIN "z2_min" + #define STR_Z2_MAX "z2_max" + #define STR_Z3_MIN "z3_min" + #define STR_Z3_MAX "z3_max" + #define STR_Z4_MIN "z4_min" + #define STR_Z4_MAX "z4_max" +#endif +#define STR_Z_PROBE "z_probe" +#define STR_PROBE_EN "probe_en" +#define STR_FILAMENT "filament" + +// General axis names #define STR_X "X" #define STR_Y "Y" #define STR_Z "Z" @@ -301,10 +359,90 @@ #define STR_Z3 "Z3" #define STR_Z4 "Z4" -#define LCD_STR_A STR_A -#define LCD_STR_B STR_B -#define LCD_STR_C STR_C -#define LCD_STR_E STR_E +// Extra Axis and Endstop Names +#if HAS_I_AXIS + #if AXIS4_NAME == 'A' + #define STR_I "A" + #define STR_I_MIN "a_min" + #define STR_I_MAX "a_max" + #elif AXIS4_NAME == 'B' + #define STR_I "B" + #define STR_I_MIN "b_min" + #define STR_I_MAX "b_max" + #elif AXIS4_NAME == 'C' + #define STR_I "C" + #define STR_I_MIN "c_min" + #define STR_I_MAX "c_max" + #elif AXIS4_NAME == 'U' + #define STR_I "U" + #define STR_I_MIN "u_min" + #define STR_I_MAX "u_max" + #elif AXIS4_NAME == 'V' + #define STR_I "V" + #define STR_I_MIN "v_min" + #define STR_I_MAX "v_max" + #elif AXIS4_NAME == 'W' + #define STR_I "W" + #define STR_I_MIN "w_min" + #define STR_I_MAX "w_max" + #else + #error "AXIS4_NAME can only be one of 'A', 'B', 'C', 'U', 'V', or 'W'." + #endif +#else + #define STR_I "" +#endif + +#if HAS_J_AXIS + #if AXIS5_NAME == 'B' + #define STR_J "B" + #define STR_J_MIN "b_min" + #define STR_J_MAX "b_max" + #elif AXIS5_NAME == 'C' + #define STR_J "C" + #define STR_J_MIN "c_min" + #define STR_J_MAX "c_max" + #elif AXIS5_NAME == 'U' + #define STR_J "U" + #define STR_J_MIN "u_min" + #define STR_J_MAX "u_max" + #elif AXIS5_NAME == 'V' + #define STR_J "V" + #define STR_J_MIN "v_min" + #define STR_J_MAX "v_max" + #elif AXIS5_NAME == 'W' + #define STR_J "W" + #define STR_J_MIN "w_min" + #define STR_J_MAX "w_max" + #else + #error "AXIS5_NAME can only be one of 'B', 'C', 'U', 'V', or 'W'." + #endif +#else + #define STR_J "" +#endif + +#if HAS_K_AXIS + #if AXIS6_NAME == 'C' + #define STR_K "C" + #define STR_K_MIN "c_min" + #define STR_K_MAX "c_max" + #elif AXIS6_NAME == 'U' + #define STR_K "U" + #define STR_K_MIN "u_min" + #define STR_K_MAX "u_max" + #elif AXIS6_NAME == 'V' + #define STR_K "V" + #define STR_K_MIN "v_min" + #define STR_K_MAX "v_max" + #elif AXIS6_NAME == 'W' + #define STR_K "W" + #define STR_K_MIN "w_min" + #define STR_K_MAX "w_max" + #else + #error "AXIS6_NAME can only be one of 'C', 'U', 'V', or 'W'." + #endif +#else + #define STR_K "" +#endif #if EITHER(HAS_MARLINUI_HD44780, IS_TFTGLCD_PANEL) @@ -354,34 +492,42 @@ */ #if ENABLED(NUMBER_TOOLS_FROM_0) #define LCD_FIRST_TOOL 0 - #define LCD_STR_N0 "0" - #define LCD_STR_N1 "1" - #define LCD_STR_N2 "2" - #define LCD_STR_N3 "3" - #define LCD_STR_N4 "4" - #define LCD_STR_N5 "5" - #define LCD_STR_N6 "6" - #define LCD_STR_N7 "7" + #define STR_N0 "0" + #define STR_N1 "1" + #define STR_N2 "2" + #define STR_N3 "3" + #define STR_N4 "4" + #define STR_N5 "5" + #define STR_N6 "6" + #define STR_N7 "7" #else #define LCD_FIRST_TOOL 1 - #define LCD_STR_N0 "1" - #define LCD_STR_N1 "2" - #define LCD_STR_N2 "3" - #define LCD_STR_N3 "4" - #define LCD_STR_N4 "5" - #define LCD_STR_N5 "6" - #define LCD_STR_N6 "7" - #define LCD_STR_N7 "8" + #define STR_N0 "1" + #define STR_N1 "2" + #define STR_N2 "3" + #define STR_N3 "4" + #define STR_N4 "5" + #define STR_N5 "6" + #define STR_N6 "7" + #define STR_N7 "8" #endif -#define LCD_STR_E0 "E" LCD_STR_N0 -#define LCD_STR_E1 "E" LCD_STR_N1 -#define LCD_STR_E2 "E" LCD_STR_N2 -#define LCD_STR_E3 "E" LCD_STR_N3 -#define LCD_STR_E4 "E" LCD_STR_N4 -#define LCD_STR_E5 "E" LCD_STR_N5 -#define LCD_STR_E6 "E" LCD_STR_N6 -#define LCD_STR_E7 "E" LCD_STR_N7 +#define STR_E0 STR_E STR_N0 +#define STR_E1 STR_E STR_N1 +#define STR_E2 STR_E STR_N2 +#define STR_E3 STR_E STR_N3 +#define STR_E4 STR_E STR_N4 +#define STR_E5 STR_E STR_N5 +#define STR_E6 STR_E STR_N6 +#define STR_E7 STR_E STR_N7 + +// Include localized LCD Menu Messages + +#define LANGUAGE_DATA_INCL_(M) STRINGIFY_(fontdata/langdata_##M.h) +#define LANGUAGE_DATA_INCL(M) LANGUAGE_DATA_INCL_(M) + +#define LANGUAGE_INCL_(M) STRINGIFY_(../lcd/language/language_##M.h) +#define LANGUAGE_INCL(M) LANGUAGE_INCL_(M) // Use superscripts, if possible. Evaluated at point of use. #define SUPERSCRIPT_TWO TERN(NOT_EXTENDED_ISO10646_1_5X7, "^2", "²") diff --git a/Marlin/src/core/macros.h b/Marlin/src/core/macros.h index d7043ba5234d..31808586cf0d 100644 --- a/Marlin/src/core/macros.h +++ b/Marlin/src/core/macros.h @@ -33,23 +33,38 @@ #define _AXIS(A) (A##_AXIS) -#define _XMIN_ 100 -#define _YMIN_ 200 -#define _ZMIN_ 300 -#define _XMAX_ 101 -#define _YMAX_ 201 -#define _ZMAX_ 301 -#define _XDIAG_ 102 -#define _YDIAG_ 202 -#define _ZDIAG_ 302 -#define _E0DIAG_ 400 -#define _E1DIAG_ 401 -#define _E2DIAG_ 402 -#define _E3DIAG_ 403 -#define _E4DIAG_ 404 -#define _E5DIAG_ 405 -#define _E6DIAG_ 406 -#define _E7DIAG_ 407 +#define _XSTOP_ 0x01 +#define _YSTOP_ 0x02 +#define _ZSTOP_ 0x03 +#define _ISTOP_ 0x04 +#define _JSTOP_ 0x05 +#define _KSTOP_ 0x06 +#define _XMIN_ 0x11 +#define _YMIN_ 0x12 +#define _ZMIN_ 0x13 +#define _IMIN_ 0x14 +#define _JMIN_ 0x15 +#define _KMIN_ 0x16 +#define _XMAX_ 0x21 +#define _YMAX_ 0x22 +#define _ZMAX_ 0x23 +#define _IMAX_ 0x24 +#define _JMAX_ 0x25 +#define _KMAX_ 0x26 +#define _XDIAG_ 0x31 +#define _YDIAG_ 0x32 +#define _ZDIAG_ 0x33 +#define _IDIAG_ 0x34 +#define _JDIAG_ 0x35 +#define _KDIAG_ 0x36 +#define _E0DIAG_ 0xE0 +#define _E1DIAG_ 0xE1 +#define _E2DIAG_ 0xE2 +#define _E3DIAG_ 0xE3 +#define _E4DIAG_ 0xE4 +#define _E5DIAG_ 0xE5 +#define _E6DIAG_ 0xE6 +#define _E7DIAG_ 0xE7 #define _FORCE_INLINE_ __attribute__((__always_inline__)) __inline__ #define FORCE_INLINE __attribute__((always_inline)) inline @@ -116,13 +131,13 @@ #ifdef __cplusplus // C++11 solution that is standards compliant. - template static inline constexpr void NOLESS(V& v, const N n) { + template static constexpr void NOLESS(V& v, const N n) { if (n > v) v = n; } - template static inline constexpr void NOMORE(V& v, const N n) { + template static constexpr void NOMORE(V& v, const N n) { if (n < v) v = n; } - template static inline constexpr void LIMIT(V& v, const N1 n1, const N2 n2) { + template static constexpr void LIMIT(V& v, const N1 n1, const N2 n2) { if (n1 > v) v = n1; else if (n2 < v) v = n2; } @@ -151,7 +166,7 @@ #endif -// Macros to chain up to 14 conditions +// Macros to chain up to 40 conditions #define _DO_1(W,C,A) (_##W##_1(A)) #define _DO_2(W,C,A,B) (_##W##_1(A) C _##W##_1(B)) #define _DO_3(W,C,A,V...) (_##W##_1(A) C _DO_2(W,C,V)) @@ -167,6 +182,31 @@ #define _DO_13(W,C,A,V...) (_##W##_1(A) C _DO_12(W,C,V)) #define _DO_14(W,C,A,V...) (_##W##_1(A) C _DO_13(W,C,V)) #define _DO_15(W,C,A,V...) (_##W##_1(A) C _DO_14(W,C,V)) +#define _DO_16(W,C,A,V...) (_##W##_1(A) C _DO_15(W,C,V)) +#define _DO_17(W,C,A,V...) (_##W##_1(A) C _DO_16(W,C,V)) +#define _DO_18(W,C,A,V...) (_##W##_1(A) C _DO_17(W,C,V)) +#define _DO_19(W,C,A,V...) (_##W##_1(A) C _DO_18(W,C,V)) +#define _DO_20(W,C,A,V...) (_##W##_1(A) C _DO_19(W,C,V)) +#define _DO_21(W,C,A,V...) (_##W##_1(A) C _DO_20(W,C,V)) +#define _DO_22(W,C,A,V...) (_##W##_1(A) C _DO_21(W,C,V)) +#define _DO_23(W,C,A,V...) (_##W##_1(A) C _DO_22(W,C,V)) +#define _DO_24(W,C,A,V...) (_##W##_1(A) C _DO_23(W,C,V)) +#define _DO_25(W,C,A,V...) (_##W##_1(A) C _DO_24(W,C,V)) +#define _DO_26(W,C,A,V...) (_##W##_1(A) C _DO_25(W,C,V)) +#define _DO_27(W,C,A,V...) (_##W##_1(A) C _DO_26(W,C,V)) +#define _DO_28(W,C,A,V...) (_##W##_1(A) C _DO_27(W,C,V)) +#define _DO_29(W,C,A,V...) (_##W##_1(A) C _DO_28(W,C,V)) +#define _DO_30(W,C,A,V...) (_##W##_1(A) C _DO_29(W,C,V)) +#define _DO_31(W,C,A,V...) (_##W##_1(A) C _DO_30(W,C,V)) +#define _DO_32(W,C,A,V...) (_##W##_1(A) C _DO_31(W,C,V)) +#define _DO_33(W,C,A,V...) (_##W##_1(A) C _DO_32(W,C,V)) +#define _DO_34(W,C,A,V...) (_##W##_1(A) C _DO_33(W,C,V)) +#define _DO_35(W,C,A,V...) (_##W##_1(A) C _DO_34(W,C,V)) +#define _DO_36(W,C,A,V...) (_##W##_1(A) C _DO_35(W,C,V)) +#define _DO_37(W,C,A,V...) (_##W##_1(A) C _DO_36(W,C,V)) +#define _DO_38(W,C,A,V...) (_##W##_1(A) C _DO_37(W,C,V)) +#define _DO_39(W,C,A,V...) (_##W##_1(A) C _DO_38(W,C,V)) +#define _DO_40(W,C,A,V...) (_##W##_1(A) C _DO_39(W,C,V)) #define __DO_N(W,C,N,V...) _DO_##N(W,C,V) #define _DO_N(W,C,N,V...) __DO_N(W,C,N,V) #define DO(W,C,V...) (_DO_N(W,C,NUM_ARGS(V),V)) @@ -187,14 +227,28 @@ #define DISABLED(V...) DO(DIS,&&,V) #define COUNT_ENABLED(V...) DO(ENA,+,V) -#define TERN(O,A,B) _TERN(_ENA_1(O),B,A) // OPTION converted to '0' or '1' -#define TERN0(O,A) _TERN(_ENA_1(O),0,A) // OPTION converted to A or '0' -#define TERN1(O,A) _TERN(_ENA_1(O),1,A) // OPTION converted to A or '1' -#define TERN_(O,A) _TERN(_ENA_1(O),,A) // OPTION converted to A or '' +#define TERN(O,A,B) _TERN(_ENA_1(O),B,A) // OPTION ? 'A' : 'B' +#define TERN0(O,A) _TERN(_ENA_1(O),0,A) // OPTION ? 'A' : '0' +#define TERN1(O,A) _TERN(_ENA_1(O),1,A) // OPTION ? 'A' : '1' +#define TERN_(O,A) _TERN(_ENA_1(O),,A) // OPTION ? 'A' : '' #define _TERN(E,V...) __TERN(_CAT(T_,E),V) // Prepend 'T_' to get 'T_0' or 'T_1' #define __TERN(T,V...) ___TERN(_CAT(_NO,T),V) // Prepend '_NO' to get '_NOT_0' or '_NOT_1' #define ___TERN(P,V...) THIRD(P,V) // If first argument has a comma, A. Else B. +#define _OPTITEM(A...) A, +#define OPTITEM(O,A...) TERN_(O,DEFER4(_OPTITEM)(A)) +#define _OPTARG(A...) , A +#define OPTARG(O,A...) TERN_(O,DEFER4(_OPTARG)(A)) +#define _OPTCODE(A) A; +#define OPTCODE(O,A) TERN_(O,DEFER4(_OPTCODE)(A)) + +// Macros to avoid 'f + 0.0' which is not always optimized away. Minus included for symmetry. +// Compiler flags -fno-signed-zeros -ffinite-math-only also cover 'f * 1.0', 'f - f', etc. +#define PLUS_TERN0(O,A) _TERN(_ENA_1(O),,+ (A)) // OPTION ? '+ (A)' : '' +#define MINUS_TERN0(O,A) _TERN(_ENA_1(O),,- (A)) // OPTION ? '- (A)' : '' +#define SUM_TERN(O,B,A) ((B) PLUS_TERN0(O,A)) // ((B) (OPTION ? '+ (A)' : '')) +#define DIFF_TERN(O,B,A) ((B) MINUS_TERN0(O,A)) // ((B) (OPTION ? '- (A)' : '')) + #define IF_ENABLED TERN_ #define IF_DISABLED(O,A) TERN(O,,A) @@ -230,7 +284,52 @@ memcpy(&a[0],&b[0],_MIN(sizeof(a),sizeof(b))); \ }while(0) +#define CODE_16( A,B,C,D,E,F,G,H,I,J,K,L,M,N,O,P,...) A; B; C; D; E; F; G; H; I; J; K; L; M; N; O; P +#define CODE_15( A,B,C,D,E,F,G,H,I,J,K,L,M,N,O,...) A; B; C; D; E; F; G; H; I; J; K; L; M; N; O +#define CODE_14( A,B,C,D,E,F,G,H,I,J,K,L,M,N,...) A; B; C; D; E; F; G; H; I; J; K; L; M; N +#define CODE_13( A,B,C,D,E,F,G,H,I,J,K,L,M,...) A; B; C; D; E; F; G; H; I; J; K; L; M +#define CODE_12( A,B,C,D,E,F,G,H,I,J,K,L,...) A; B; C; D; E; F; G; H; I; J; K; L +#define CODE_11( A,B,C,D,E,F,G,H,I,J,K,...) A; B; C; D; E; F; G; H; I; J; K +#define CODE_10( A,B,C,D,E,F,G,H,I,J,...) A; B; C; D; E; F; G; H; I; J +#define CODE_9( A,B,C,D,E,F,G,H,I,...) A; B; C; D; E; F; G; H; I +#define CODE_8( A,B,C,D,E,F,G,H,...) A; B; C; D; E; F; G; H +#define CODE_7( A,B,C,D,E,F,G,...) A; B; C; D; E; F; G +#define CODE_6( A,B,C,D,E,F,...) A; B; C; D; E; F +#define CODE_5( A,B,C,D,E,...) A; B; C; D; E +#define CODE_4( A,B,C,D,...) A; B; C; D +#define CODE_3( A,B,C,...) A; B; C +#define CODE_2( A,B,...) A; B +#define CODE_1( A,...) A +#define CODE_0(...) +#define _CODE_N(N,V...) CODE_##N(V) +#define CODE_N(N,V...) _CODE_N(N,V) + +#define GANG_16(A,B,C,D,E,F,G,H,I,J,K,L,M,N,O,P,...) A B C D E F G H I J K L M N O P +#define GANG_15(A,B,C,D,E,F,G,H,I,J,K,L,M,N,O,...) A B C D E F G H I J K L M N O +#define GANG_14(A,B,C,D,E,F,G,H,I,J,K,L,M,N,...) A B C D E F G H I J K L M N +#define GANG_13(A,B,C,D,E,F,G,H,I,J,K,L,M...) A B C D E F G H I J K L M +#define GANG_12(A,B,C,D,E,F,G,H,I,J,K,L...) A B C D E F G H I J K L +#define GANG_11(A,B,C,D,E,F,G,H,I,J,K,...) A B C D E F G H I J K +#define GANG_10(A,B,C,D,E,F,G,H,I,J,...) A B C D E F G H I J +#define GANG_9( A,B,C,D,E,F,G,H,I,...) A B C D E F G H I +#define GANG_8( A,B,C,D,E,F,G,H,...) A B C D E F G H +#define GANG_7( A,B,C,D,E,F,G,...) A B C D E F G +#define GANG_6( A,B,C,D,E,F,...) A B C D E F +#define GANG_5( A,B,C,D,E,...) A B C D E +#define GANG_4( A,B,C,D,...) A B C D +#define GANG_3( A,B,C,...) A B C +#define GANG_2( A,B,...) A B +#define GANG_1( A,...) A +#define GANG_0(...) +#define _GANG_N(N,V...) GANG_##N(V) +#define GANG_N(N,V...) _GANG_N(N,V) +#define GANG_N_1(N,K) _GANG_N(N,K,K,K,K,K,K,K,K,K,K,K,K,K,K,K,K) + // Macros for initializing arrays +#define LIST_20(A,B,C,D,E,F,G,H,I,J,K,L,M,N,O,P,Q,R,S,T,...) A,B,C,D,E,F,G,H,I,J,K,L,M,N,O,P,Q,R,S,T +#define LIST_19(A,B,C,D,E,F,G,H,I,J,K,L,M,N,O,P,Q,R,S,...) A,B,C,D,E,F,G,H,I,J,K,L,M,N,O,P,Q,R,S +#define LIST_18(A,B,C,D,E,F,G,H,I,J,K,L,M,N,O,P,Q,R,...) A,B,C,D,E,F,G,H,I,J,K,L,M,N,O,P,Q,R +#define LIST_17(A,B,C,D,E,F,G,H,I,J,K,L,M,N,O,P,Q,...) A,B,C,D,E,F,G,H,I,J,K,L,M,N,O,P,Q #define LIST_16(A,B,C,D,E,F,G,H,I,J,K,L,M,N,O,P,...) A,B,C,D,E,F,G,H,I,J,K,L,M,N,O,P #define LIST_15(A,B,C,D,E,F,G,H,I,J,K,L,M,N,O,...) A,B,C,D,E,F,G,H,I,J,K,L,M,N,O #define LIST_14(A,B,C,D,E,F,G,H,I,J,K,L,M,N,...) A,B,C,D,E,F,G,H,I,J,K,L,M,N @@ -247,10 +346,13 @@ #define LIST_3( A,B,C,...) A,B,C #define LIST_2( A,B,...) A,B #define LIST_1( A,...) A +#define LIST_0(...) #define _LIST_N(N,V...) LIST_##N(V) #define LIST_N(N,V...) _LIST_N(N,V) +#define LIST_N_1(N,K) _LIST_N(N,K,K,K,K,K,K,K,K,K,K,K,K,K,K,K,K) #define ARRAY_N(N,V...) { _LIST_N(N,V) } +#define ARRAY_N_1(N,K) { LIST_N_1(N,K) } #define _JOIN_1(O) (O) #define JOIN_N(N,C,V...) (DO(JOIN,C,LIST_N(N,V))) @@ -266,7 +368,7 @@ #undef ABS #ifdef __cplusplus - template static inline constexpr const T ABS(const T v) { return v >= 0 ? v : -v; } + template static constexpr const T ABS(const T v) { return v >= 0 ? v : -v; } #else #define ABS(a) ({__typeof__(a) _a = (a); _a >= 0 ? _a : -_a;}) #endif @@ -294,8 +396,12 @@ #define HYPOT(x,y) SQRT(HYPOT2(x,y)) // Use NUM_ARGS(__VA_ARGS__) to get the number of variadic arguments -#define _NUM_ARGS(_,Z,Y,X,W,V,U,T,S,R,Q,P,O,N,M,L,K,J,I,H,G,F,E,D,C,B,A,OUT,...) OUT -#define NUM_ARGS(V...) _NUM_ARGS(0,V,26,25,24,23,22,21,20,19,18,17,16,15,14,13,12,11,10,9,8,7,6,5,4,3,2,1,0) +#define _NUM_ARGS(_,n,m,l,k,j,i,h,g,f,e,d,c,b,a,Z,Y,X,W,V,U,T,S,R,Q,P,O,N,M,L,K,J,I,H,G,F,E,D,C,B,A,OUT,...) OUT +#define NUM_ARGS(V...) _NUM_ARGS(0,V,40,39,38,37,36,35,34,33,32,31,30,29,28,27,26,25,24,23,22,21,20,19,18,17,16,15,14,13,12,11,10,9,8,7,6,5,4,3,2,1,0) + +// Use TWO_ARGS(__VA_ARGS__) to get whether there are 1, 2, or >2 arguments +#define _TWO_ARGS(_,n,m,l,k,j,i,h,g,f,e,d,c,b,a,Z,Y,X,W,V,U,T,S,R,Q,P,O,N,M,L,K,J,I,H,G,F,E,D,C,B,A,OUT,...) OUT +#define TWO_ARGS(V...) _TWO_ARGS(0,V,3,3,3,3,3,3,3,3,3,3,3,3,3,3,3,3,3,3,3,3,3,3,3,3,3,3,3,3,3,3,3,3,3,3,3,3,3,3,2,1,0) #ifdef __cplusplus @@ -305,19 +411,29 @@ extern "C++" { // C++11 solution that is standards compliant. Return type is deduced automatically - template static inline constexpr auto _MIN(const L lhs, const R rhs) -> decltype(lhs + rhs) { + template static constexpr auto _MIN(const L lhs, const R rhs) -> decltype(lhs + rhs) { return lhs < rhs ? lhs : rhs; } - template static inline constexpr auto _MAX(const L lhs, const R rhs) -> decltype(lhs + rhs) { + template static constexpr auto _MAX(const L lhs, const R rhs) -> decltype(lhs + rhs) { return lhs > rhs ? lhs : rhs; } - template static inline constexpr const T _MIN(T V, Ts... Vs) { return _MIN(V, _MIN(Vs...)); } - template static inline constexpr const T _MAX(T V, Ts... Vs) { return _MAX(V, _MAX(Vs...)); } + template static constexpr const T _MIN(T V, Ts... Vs) { return _MIN(V, _MIN(Vs...)); } + template static constexpr const T _MAX(T V, Ts... Vs) { return _MAX(V, _MAX(Vs...)); } } #endif + // Allow manipulating enumeration value like flags without ugly cast everywhere + #define ENUM_FLAGS(T) \ + FORCE_INLINE constexpr T operator&(T x, T y) { return static_cast(static_cast(x) & static_cast(y)); } \ + FORCE_INLINE constexpr T operator|(T x, T y) { return static_cast(static_cast(x) | static_cast(y)); } \ + FORCE_INLINE constexpr T operator^(T x, T y) { return static_cast(static_cast(x) ^ static_cast(y)); } \ + FORCE_INLINE constexpr T operator~(T x) { return static_cast(~static_cast(x)); } \ + FORCE_INLINE T & operator&=(T &x, T y) { return x &= y; } \ + FORCE_INLINE T & operator|=(T &x, T y) { return x |= y; } \ + FORCE_INLINE T & operator^=(T &x, T y) { return x ^= y; } + // C++11 solution that is standard compliant. is not available on all platform namespace Private { template struct enable_if { }; @@ -329,7 +445,7 @@ template struct first_type_of { typedef T type; }; template struct first_type_of { typedef T type; }; } - // C++11 solution using SFINAE to detect the existance of a member in a class at compile time. + // C++11 solution using SFINAE to detect the existence of a member in a class at compile time. // It creates a HasMember structure containing 'value' set to true if the member exists #define HAS_MEMBER_IMPL(Member) \ namespace Private { \ @@ -357,51 +473,59 @@ return *str ? findStringEnd(str + 1) : str; } - // Check whether a string contains a slash - constexpr bool containsSlash(const char *str) { - return *str == '/' ? true : (*str ? containsSlash(str + 1) : false); + // Check whether a string contains a specific character + constexpr bool contains(const char *str, const char ch) { + return *str == ch ? true : (*str ? contains(str + 1, ch) : false); } - // Find the last position of the slash - constexpr const char* findLastSlashPos(const char* str) { - return *str == '/' ? (str + 1) : findLastSlashPos(str - 1); + // Find the last position of the specific character (should be called with findStringEnd) + constexpr const char* findLastPos(const char *str, const char ch) { + return *str == ch ? (str + 1) : findLastPos(str - 1, ch); } // Compile-time evaluation of the last part of a file path // Typically used to shorten the path to file in compiled strings // CompileTimeString::baseName(__FILE__) returns "macros.h" and not /path/to/Marlin/src/core/macros.h - constexpr const char* baseName(const char* str) { - return containsSlash(str) ? findLastSlashPos(findStringEnd(str)) : str; + constexpr const char* baseName(const char *str) { + return contains(str, '/') ? findLastPos(findStringEnd(str), '/') : str; + } + + // Find the first occurrence of a character in a string (or return the last position in the string) + constexpr const char* findFirst(const char *str, const char ch) { + return *str == ch || *str == 0 ? (str + 1) : findFirst(str + 1, ch); + } + // Compute the string length at compile time + constexpr unsigned stringLen(const char *str) { + return *str == 0 ? 0 : 1 + stringLen(str + 1); } } #define ONLY_FILENAME CompileTimeString::baseName(__FILE__) + /** Get the templated type name. This does not depends on RTTI, but on the preprocessor, so it should be quite safe to use even on old compilers. + WARNING: DO NOT RENAME THIS FUNCTION (or change the text inside the function to match what the preprocessor will generate) + The name is chosen very short since the binary will store "const char* gtn(T*) [with T = YourTypeHere]" so avoid long function name here */ + template + inline const char* gtn(T*) { + // It works on GCC by instantiating __PRETTY_FUNCTION__ and parsing the result. So the syntax here is very limited to GCC output + constexpr unsigned verboseChatLen = sizeof("const char* gtn(T*) [with T = ") - 1; + static char templateType[sizeof(__PRETTY_FUNCTION__) - verboseChatLen] = {}; + __builtin_memcpy(templateType, __PRETTY_FUNCTION__ + verboseChatLen, sizeof(__PRETTY_FUNCTION__) - verboseChatLen - 2); + return templateType; + } #else - #define MIN_2(a,b) ((a)<(b)?(a):(b)) - #define MIN_3(a,V...) MIN_2(a,MIN_2(V)) - #define MIN_4(a,V...) MIN_2(a,MIN_3(V)) - #define MIN_5(a,V...) MIN_2(a,MIN_4(V)) - #define MIN_6(a,V...) MIN_2(a,MIN_5(V)) - #define MIN_7(a,V...) MIN_2(a,MIN_6(V)) - #define MIN_8(a,V...) MIN_2(a,MIN_7(V)) - #define MIN_9(a,V...) MIN_2(a,MIN_8(V)) - #define MIN_10(a,V...) MIN_2(a,MIN_9(V)) #define __MIN_N(N,V...) MIN_##N(V) #define _MIN_N(N,V...) __MIN_N(N,V) - #define _MIN(V...) _MIN_N(NUM_ARGS(V), V) + #define _MIN_N_REF() _MIN_N + #define _MIN(V...) EVAL(_MIN_N(TWO_ARGS(V),V)) + #define MIN_2(a,b) ((a)<(b)?(a):(b)) + #define MIN_3(a,V...) MIN_2(a,DEFER2(_MIN_N_REF)()(TWO_ARGS(V),V)) - #define MAX_2(a,b) ((a)>(b)?(a):(b)) - #define MAX_3(a,V...) MAX_2(a,MAX_2(V)) - #define MAX_4(a,V...) MAX_2(a,MAX_3(V)) - #define MAX_5(a,V...) MAX_2(a,MAX_4(V)) - #define MAX_6(a,V...) MAX_2(a,MAX_5(V)) - #define MAX_7(a,V...) MAX_2(a,MAX_6(V)) - #define MAX_8(a,V...) MAX_2(a,MAX_7(V)) - #define MAX_9(a,V...) MAX_2(a,MAX_8(V)) - #define MAX_10(a,V...) MAX_2(a,MAX_9(V)) #define __MAX_N(N,V...) MAX_##N(V) #define _MAX_N(N,V...) __MAX_N(N,V) - #define _MAX(V...) _MAX_N(NUM_ARGS(V), V) + #define _MAX_N_REF() _MAX_N + #define _MAX(V...) EVAL(_MAX_N(TWO_ARGS(V),V)) + #define MAX_2(a,b) ((a)>(b)?(a):(b)) + #define MAX_3(a,V...) MAX_2(a,DEFER2(_MAX_N_REF)()(TWO_ARGS(V),V)) #endif @@ -422,6 +546,11 @@ #define INC_13 14 #define INC_14 15 #define INC_15 16 +#define INC_16 17 +#define INC_17 18 +#define INC_18 19 +#define INC_19 20 +#define INC_20 21 #define INCREMENT_(n) INC_##n #define INCREMENT(n) INCREMENT_(n) @@ -436,6 +565,9 @@ #define ADD8(N) ADD4(ADD4(N)) #define ADD9(N) ADD4(ADD5(N)) #define ADD10(N) ADD5(ADD5(N)) +#define SUM(A,B) _CAT(ADD,A)(B) +#define DOUBLE_(n) ADD##n(n) +#define DOUBLE(n) DOUBLE_(n) // Macros for subtracting #define DEC_0 0 @@ -544,6 +676,7 @@ // Repeat a macro passing S...N-1. #define REPEAT_S(S,N,OP) EVAL(_REPEAT(S,SUB##S(N),OP)) #define REPEAT(N,OP) REPEAT_S(0,N,OP) +#define REPEAT_1(N,OP) REPEAT_S(1,INCREMENT(N),OP) // Repeat a macro passing 0...N-1 plus additional arguments. #define REPEAT2_S(S,N,OP,V...) EVAL(_REPEAT2(S,SUB##S(N),OP,V)) diff --git a/Marlin/src/core/multi_language.h b/Marlin/src/core/multi_language.h index 5a26edf8d453..2106f946ac73 100644 --- a/Marlin/src/core/multi_language.h +++ b/Marlin/src/core/multi_language.h @@ -1,28 +1,35 @@ -/******************** - * multi_language.h * - ********************/ - -/**************************************************************************** - * Written By Marcio Teixeira 2019 - Aleph Objects, Inc. * - * * - * This program is free software: you can redistribute it and/or modify * - * it under the terms of the GNU General Public License as published by * - * the Free Software Foundation, either version 3 of the License, or * - * (at your option) any later version. * - * * - * This program is distributed in the hope that it will be useful, * - * but WITHOUT ANY WARRANTY; without even the implied warranty of * - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * - * GNU General Public License for more details. * - * * - * To view a copy of the GNU General Public License, go to the following * - * location: . * - ****************************************************************************/ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + * + */ #pragma once +/******************************************************* + * multi_language.h * + * By Marcio Teixeira 2019 for Aleph Objects * + *******************************************************/ + #include "../inc/MarlinConfigPre.h" typedef const char Language_Str[]; +#define LSTR PROGMEM Language_Str #ifdef LCD_LANGUAGE_5 #define NUM_LANGUAGES 5 @@ -36,21 +43,17 @@ typedef const char Language_Str[]; #define NUM_LANGUAGES 1 #endif -// Setting the unused languages equal to each other allows -// the compiler to optimize away the conditionals - +// Set unused languages equal to each other so the +// compiler can optimize away the conditionals. #ifndef LCD_LANGUAGE_2 #define LCD_LANGUAGE_2 LCD_LANGUAGE #endif - #ifndef LCD_LANGUAGE_3 #define LCD_LANGUAGE_3 LCD_LANGUAGE_2 #endif - #ifndef LCD_LANGUAGE_4 #define LCD_LANGUAGE_4 LCD_LANGUAGE_3 #endif - #ifndef LCD_LANGUAGE_5 #define LCD_LANGUAGE_5 LCD_LANGUAGE_4 #endif @@ -61,11 +64,11 @@ typedef const char Language_Str[]; #if NUM_LANGUAGES > 1 #define HAS_MULTI_LANGUAGE 1 #define GET_TEXT(MSG) ( \ - ui.language == 0 ? GET_LANG(LCD_LANGUAGE )::MSG : \ - ui.language == 1 ? GET_LANG(LCD_LANGUAGE_2)::MSG : \ - ui.language == 2 ? GET_LANG(LCD_LANGUAGE_3)::MSG : \ + ui.language == 4 ? GET_LANG(LCD_LANGUAGE_5)::MSG : \ ui.language == 3 ? GET_LANG(LCD_LANGUAGE_4)::MSG : \ - GET_LANG(LCD_LANGUAGE_5)::MSG ) + ui.language == 2 ? GET_LANG(LCD_LANGUAGE_3)::MSG : \ + ui.language == 1 ? GET_LANG(LCD_LANGUAGE_2)::MSG : \ + GET_LANG(LCD_LANGUAGE )::MSG ) #define MAX_LANG_CHARSIZE _MAX(GET_LANG(LCD_LANGUAGE )::CHARSIZE, \ GET_LANG(LCD_LANGUAGE_2)::CHARSIZE, \ GET_LANG(LCD_LANGUAGE_3)::CHARSIZE, \ @@ -75,7 +78,7 @@ typedef const char Language_Str[]; #define GET_TEXT(MSG) GET_LANG(LCD_LANGUAGE)::MSG #define MAX_LANG_CHARSIZE LANG_CHARSIZE #endif -#define GET_TEXT_F(MSG) (const __FlashStringHelper*)GET_TEXT(MSG) +#define GET_TEXT_F(MSG) FPSTR(GET_TEXT(MSG)) #define GET_LANGUAGE_NAME(INDEX) GET_LANG(LCD_LANGUAGE_##INDEX)::LANGUAGE #define LANG_CHARSIZE GET_TEXT(CHARSIZE) diff --git a/Marlin/src/core/serial.cpp b/Marlin/src/core/serial.cpp index 01f850ba566b..2b1ae1f1fe00 100644 --- a/Marlin/src/core/serial.cpp +++ b/Marlin/src/core/serial.cpp @@ -36,54 +36,56 @@ PGMSTR(X_LBL, "X:"); PGMSTR(Y_LBL, "Y:"); PGMSTR(Z_LBL, "Z:"); PGMST PGMSTR(SP_A_STR, " A"); PGMSTR(SP_B_STR, " B"); PGMSTR(SP_C_STR, " C"); PGMSTR(SP_X_STR, " X"); PGMSTR(SP_Y_STR, " Y"); PGMSTR(SP_Z_STR, " Z"); PGMSTR(SP_E_STR, " E"); PGMSTR(SP_X_LBL, " X:"); PGMSTR(SP_Y_LBL, " Y:"); PGMSTR(SP_Z_LBL, " Z:"); PGMSTR(SP_E_LBL, " E:"); +PGMSTR(I_STR, STR_I); PGMSTR(J_STR, STR_J); PGMSTR(K_STR, STR_K); +PGMSTR(I_LBL, STR_I ":"); PGMSTR(J_LBL, STR_J ":"); PGMSTR(K_LBL, STR_K ":"); +PGMSTR(SP_I_STR, " " STR_I); PGMSTR(SP_J_STR, " " STR_J); PGMSTR(SP_K_STR, " " STR_K); +PGMSTR(SP_I_LBL, " " STR_I ":"); PGMSTR(SP_J_LBL, " " STR_J ":"); PGMSTR(SP_K_LBL, " " STR_K ":"); +// Hook Meatpack if it's enabled on the first leaf +#if ENABLED(MEATPACK_ON_SERIAL_PORT_1) + SerialLeafT1 mpSerial1(false, _SERIAL_LEAF_1); +#endif +#if ENABLED(MEATPACK_ON_SERIAL_PORT_2) + SerialLeafT2 mpSerial2(false, _SERIAL_LEAF_2); +#endif +#if ENABLED(MEATPACK_ON_SERIAL_PORT_3) + SerialLeafT3 mpSerial3(false, _SERIAL_LEAF_3); +#endif + +// Step 2: For multiserial, handle the second serial port as well #if HAS_MULTI_SERIAL - #ifdef SERIAL_CATCHALL - SerialOutputT multiSerial(MYSERIAL, SERIAL_CATCHALL); - #else - #if HAS_ETHERNET - // Runtime checking of the condition variable - ConditionalSerial serialOut1(ethernet.have_telnet_client, MYSERIAL1, false); // Takes reference here - #else - // Don't pay for runtime checking a true variable, instead use the output directly - #define serialOut1 MYSERIAL1 - #endif - SerialOutputT multiSerial(MYSERIAL0, serialOut1); + #if HAS_ETHERNET + // We need a definition here + SerialLeafT2 msSerial2(ethernet.have_telnet_client, MYSERIAL2, false); #endif -#endif -#if ENABLED(MEATPACK) - MeatpackSerial mpSerial(false, _SERIAL_IMPL); + #define __S_LEAF(N) ,SERIAL_LEAF_##N + #define _S_LEAF(N) __S_LEAF(N) + + SerialOutputT multiSerial( SERIAL_LEAF_1 REPEAT_S(2, INCREMENT(NUM_SERIAL), _S_LEAF) ); + + #undef __S_LEAF + #undef _S_LEAF + #endif -void serialprintPGM(PGM_P str) { +void serial_print_P(PGM_P str) { while (const char c = pgm_read_byte(str++)) SERIAL_CHAR(c); } -void serial_echo_start() { static PGMSTR(echomagic, "echo:"); serialprintPGM(echomagic); } -void serial_error_start() { static PGMSTR(errormagic, "Error:"); serialprintPGM(errormagic); } - -void serial_echopair_PGM(PGM_P const s_P, serial_char_t v) { serialprintPGM(s_P); SERIAL_CHAR(v.c); } -void serial_echopair_PGM(PGM_P const s_P, const char *v) { serialprintPGM(s_P); SERIAL_ECHO(v); } -void serial_echopair_PGM(PGM_P const s_P, char v) { serialprintPGM(s_P); SERIAL_ECHO(v); } -void serial_echopair_PGM(PGM_P const s_P, int v) { serialprintPGM(s_P); SERIAL_ECHO(v); } -void serial_echopair_PGM(PGM_P const s_P, long v) { serialprintPGM(s_P); SERIAL_ECHO(v); } -void serial_echopair_PGM(PGM_P const s_P, float v) { serialprintPGM(s_P); SERIAL_DECIMAL(v); } -void serial_echopair_PGM(PGM_P const s_P, double v) { serialprintPGM(s_P); SERIAL_DECIMAL(v); } -void serial_echopair_PGM(PGM_P const s_P, unsigned char v) { serialprintPGM(s_P); SERIAL_ECHO(v); } -void serial_echopair_PGM(PGM_P const s_P, unsigned int v) { serialprintPGM(s_P); SERIAL_ECHO(v); } -void serial_echopair_PGM(PGM_P const s_P, unsigned long v) { serialprintPGM(s_P); SERIAL_ECHO(v); } +void serial_echo_start() { static PGMSTR(echomagic, "echo:"); serial_print_P(echomagic); } +void serial_error_start() { static PGMSTR(errormagic, "Error:"); serial_print_P(errormagic); } void serial_spaces(uint8_t count) { count *= (PROPORTIONAL_FONT_RATIO); while (count--) SERIAL_CHAR(' '); } -void serial_ternary(const bool onoff, PGM_P const pre, PGM_P const on, PGM_P const off, PGM_P const post/*=nullptr*/) { - if (pre) serialprintPGM(pre); - serialprintPGM(onoff ? on : off); - if (post) serialprintPGM(post); +void serial_ternary(const bool onoff, FSTR_P const pre, FSTR_P const on, FSTR_P const off, FSTR_P const post/*=nullptr*/) { + if (pre) serial_print(pre); + serial_print(onoff ? on : off); + if (post) serial_print(post); } -void serialprint_onoff(const bool onoff) { serialprintPGM(onoff ? PSTR(STR_ON) : PSTR(STR_OFF)); } +void serialprint_onoff(const bool onoff) { serial_print(onoff ? F(STR_ON) : F(STR_OFF)); } void serialprintln_onoff(const bool onoff) { serialprint_onoff(onoff); SERIAL_EOL(); } -void serialprint_truefalse(const bool tf) { serialprintPGM(tf ? PSTR("true") : PSTR("false")); } +void serialprint_truefalse(const bool tf) { serial_print(tf ? F("true") : F("false")); } void print_bin(uint16_t val) { for (uint8_t i = 16; i--;) { @@ -92,8 +94,10 @@ void print_bin(uint16_t val) { } } -void print_xyz(const float &x, const float &y, const float &z, PGM_P const prefix/*=nullptr*/, PGM_P const suffix/*=nullptr*/) { - if (prefix) serialprintPGM(prefix); - SERIAL_ECHOPAIR_P(SP_X_STR, x, SP_Y_STR, y, SP_Z_STR, z); - if (suffix) serialprintPGM(suffix); else SERIAL_EOL(); +void print_pos(LINEAR_AXIS_ARGS(const_float_t), FSTR_P const prefix/*=nullptr*/, FSTR_P const suffix/*=nullptr*/) { + if (prefix) serial_print(prefix); + SERIAL_ECHOPGM_P( + LIST_N(DOUBLE(LINEAR_AXES), SP_X_STR, x, SP_Y_STR, y, SP_Z_STR, z, SP_I_STR, i, SP_J_STR, j, SP_K_STR, k) + ); + if (suffix) serial_print(suffix); else SERIAL_EOL(); } diff --git a/Marlin/src/core/serial.h b/Marlin/src/core/serial.h index ec955a8deab9..aee4d4d43db8 100644 --- a/Marlin/src/core/serial.h +++ b/Marlin/src/core/serial.h @@ -24,17 +24,22 @@ #include "../inc/MarlinConfig.h" #include "serial_hook.h" -#if ENABLED(MEATPACK) +#if HAS_MEATPACK #include "../feature/meatpack.h" #endif // Commonly-used strings in serial output -extern const char NUL_STR[], SP_P_STR[], SP_T_STR[], +extern const char NUL_STR[], + SP_X_STR[], SP_Y_STR[], SP_Z_STR[], + SP_A_STR[], SP_B_STR[], SP_C_STR[], SP_E_STR[], + SP_X_LBL[], SP_Y_LBL[], SP_Z_LBL[], SP_E_LBL[], + SP_I_STR[], SP_J_STR[], SP_K_STR[], + SP_I_LBL[], SP_J_LBL[], SP_K_LBL[], + SP_P_STR[], SP_T_STR[], X_STR[], Y_STR[], Z_STR[], E_STR[], + I_STR[], J_STR[], K_STR[], X_LBL[], Y_LBL[], Z_LBL[], E_LBL[], - SP_A_STR[], SP_B_STR[], SP_C_STR[], - SP_X_STR[], SP_Y_STR[], SP_Z_STR[], SP_E_STR[], - SP_X_LBL[], SP_Y_LBL[], SP_Z_LBL[], SP_E_LBL[]; + I_LBL[], J_LBL[], K_LBL[]; // // Debugging flags for use by M111 @@ -62,37 +67,82 @@ extern uint8_t marlin_debug_flags; // // Serial redirection // -#define SERIAL_ALL 0x7F +// Step 1: Find out what the first serial leaf is +#if HAS_MULTI_SERIAL && defined(SERIAL_CATCHALL) + #define _SERIAL_LEAF_1 MYSERIAL +#else + #define _SERIAL_LEAF_1 MYSERIAL1 +#endif + +// Hook Meatpack if it's enabled on the first leaf +#if ENABLED(MEATPACK_ON_SERIAL_PORT_1) + typedef MeatpackSerial SerialLeafT1; + extern SerialLeafT1 mpSerial1; + #define SERIAL_LEAF_1 mpSerial1 +#else + #define SERIAL_LEAF_1 _SERIAL_LEAF_1 +#endif + +// Step 2: For multiserial wrap all serial ports in a single +// interface with the ability to output to multiple serial ports. #if HAS_MULTI_SERIAL - #define _PORT_REDIRECT(n,p) REMEMBER(n,multiSerial.portMask,p) - #define _PORT_RESTORE(n,p) RESTORE(n) - #define SERIAL_ASSERT(P) if(multiSerial.portMask!=(P)){ debugger(); } + #define _PORT_REDIRECT(n,p) REMEMBER(n,multiSerial.portMask,p) + #define _PORT_RESTORE(n) RESTORE(n) + #define SERIAL_ASSERT(P) if (multiSerial.portMask!=(P)) { debugger(); } + // If we have a catchall, use that directly #ifdef SERIAL_CATCHALL - typedef MultiSerial SerialOutputT; + #define _SERIAL_LEAF_2 SERIAL_CATCHALL + #elif HAS_ETHERNET + typedef ConditionalSerial SerialLeafT2; // We need to create an instance here + extern SerialLeafT2 msSerial2; + #define _SERIAL_LEAF_2 msSerial2 #else - typedef MultiSerial, decltype(MYSERIAL1)), 0> SerialOutputT; + #define _SERIAL_LEAF_2 MYSERIAL2 // Don't create a useless instance here, directly use the existing instance + #endif + + // Nothing complicated here + #define _SERIAL_LEAF_3 MYSERIAL3 + + // Hook Meatpack if it's enabled on the second leaf + #if ENABLED(MEATPACK_ON_SERIAL_PORT_2) + typedef MeatpackSerial SerialLeafT2; + extern SerialLeafT2 mpSerial2; + #define SERIAL_LEAF_2 mpSerial2 + #else + #define SERIAL_LEAF_2 _SERIAL_LEAF_2 + #endif + + // Hook Meatpack if it's enabled on the third leaf + #if ENABLED(MEATPACK_ON_SERIAL_PORT_3) + typedef MeatpackSerial SerialLeafT3; + extern SerialLeafT3 mpSerial3; + #define SERIAL_LEAF_3 mpSerial3 + #else + #define SERIAL_LEAF_3 _SERIAL_LEAF_3 #endif - extern SerialOutputT multiSerial; - #define _SERIAL_IMPL multiSerial -#else - #define _PORT_REDIRECT(n,p) NOOP - #define _PORT_RESTORE(n) NOOP - #define SERIAL_ASSERT(P) NOOP - #define _SERIAL_IMPL MYSERIAL0 -#endif -#if ENABLED(MEATPACK) - extern MeatpackSerial mpSerial; - #define SERIAL_IMPL mpSerial + #define __S_MULTI(N) decltype(SERIAL_LEAF_##N), + #define _S_MULTI(N) __S_MULTI(N) + + typedef MultiSerial< REPEAT_1(NUM_SERIAL, _S_MULTI) 0> SerialOutputT; + + #undef __S_MULTI + #undef _S_MULTI + + extern SerialOutputT multiSerial; + #define SERIAL_IMPL multiSerial #else - #define SERIAL_IMPL _SERIAL_IMPL + #define _PORT_REDIRECT(n,p) NOOP + #define _PORT_RESTORE(n) NOOP + #define SERIAL_ASSERT(P) NOOP + #define SERIAL_IMPL SERIAL_LEAF_1 #endif #define SERIAL_OUT(WHAT, V...) (void)SERIAL_IMPL.WHAT(V) -#define PORT_REDIRECT(p) _PORT_REDIRECT(1,p) -#define PORT_RESTORE() _PORT_RESTORE(1) -#define SERIAL_PORTMASK(P) _BV(P) +#define PORT_REDIRECT(p) _PORT_REDIRECT(1,p) +#define PORT_RESTORE() _PORT_RESTORE(1) +#define SERIAL_PORTMASK(P) SerialMask::from(P) // // SERIAL_CHAR - Print one or more individual chars @@ -115,160 +165,94 @@ void SERIAL_ECHO(T x) { SERIAL_IMPL.print(x); } typedef struct SerialChar { char c; SerialChar(char n) : c(n) { } } serial_char_t; inline void SERIAL_ECHO(serial_char_t x) { SERIAL_IMPL.write(x.c); } #define AS_CHAR(C) serial_char_t(C) - -// SERIAL_ECHO_F prints a floating point value with optional precision -inline void SERIAL_ECHO_F(EnsureDouble x, int digit = 2) { SERIAL_IMPL.print(x, digit); } +#define AS_DIGIT(C) AS_CHAR('0' + (C)) template void SERIAL_ECHOLN(T x) { SERIAL_IMPL.println(x); } -// SERIAL_PRINT works like SERIAL_ECHO but allow to specify the encoding base of the number printed +// SERIAL_PRINT works like SERIAL_ECHO but also takes the numeric base template void SERIAL_PRINT(T x, U y) { SERIAL_IMPL.print(x, y); } -template -void SERIAL_PRINTLN(T x, U y) { SERIAL_IMPL.println(x, y); } +template +void SERIAL_PRINTLN(T x, PrintBase y) { SERIAL_IMPL.println(x, y); } // Flush the serial port inline void SERIAL_FLUSH() { SERIAL_IMPL.flush(); } inline void SERIAL_FLUSHTX() { SERIAL_IMPL.flushTX(); } -// Print a single PROGMEM string to serial -void serialprintPGM(PGM_P str); - -// SERIAL_ECHOPAIR / SERIAL_ECHOPAIR_P is used to output a key value pair. The key must be a string and the value can be anything -// Print up to 12 pairs of values. Odd elements auto-wrapped in PSTR(). -#define __SEP_N(N,V...) _SEP_##N(V) -#define _SEP_N(N,V...) __SEP_N(N,V) -#define _SEP_1(PRE) SERIAL_ECHOPGM(PRE) -#define _SEP_2(PRE,V) serial_echopair_PGM(PSTR(PRE),V) -#define _SEP_3(a,b,c) do{ _SEP_2(a,b); SERIAL_ECHOPGM(c); }while(0) -#define _SEP_4(a,b,V...) do{ _SEP_2(a,b); _SEP_2(V); }while(0) -#define _SEP_5(a,b,V...) do{ _SEP_2(a,b); _SEP_3(V); }while(0) -#define _SEP_6(a,b,V...) do{ _SEP_2(a,b); _SEP_4(V); }while(0) -#define _SEP_7(a,b,V...) do{ _SEP_2(a,b); _SEP_5(V); }while(0) -#define _SEP_8(a,b,V...) do{ _SEP_2(a,b); _SEP_6(V); }while(0) -#define _SEP_9(a,b,V...) do{ _SEP_2(a,b); _SEP_7(V); }while(0) -#define _SEP_10(a,b,V...) do{ _SEP_2(a,b); _SEP_8(V); }while(0) -#define _SEP_11(a,b,V...) do{ _SEP_2(a,b); _SEP_9(V); }while(0) -#define _SEP_12(a,b,V...) do{ _SEP_2(a,b); _SEP_10(V); }while(0) -#define _SEP_13(a,b,V...) do{ _SEP_2(a,b); _SEP_11(V); }while(0) -#define _SEP_14(a,b,V...) do{ _SEP_2(a,b); _SEP_12(V); }while(0) -#define _SEP_15(a,b,V...) do{ _SEP_2(a,b); _SEP_13(V); }while(0) -#define _SEP_16(a,b,V...) do{ _SEP_2(a,b); _SEP_14(V); }while(0) -#define _SEP_17(a,b,V...) do{ _SEP_2(a,b); _SEP_15(V); }while(0) -#define _SEP_18(a,b,V...) do{ _SEP_2(a,b); _SEP_16(V); }while(0) -#define _SEP_19(a,b,V...) do{ _SEP_2(a,b); _SEP_17(V); }while(0) -#define _SEP_20(a,b,V...) do{ _SEP_2(a,b); _SEP_18(V); }while(0) -#define _SEP_21(a,b,V...) do{ _SEP_2(a,b); _SEP_19(V); }while(0) -#define _SEP_22(a,b,V...) do{ _SEP_2(a,b); _SEP_20(V); }while(0) -#define _SEP_23(a,b,V...) do{ _SEP_2(a,b); _SEP_21(V); }while(0) -#define _SEP_24(a,b,V...) do{ _SEP_2(a,b); _SEP_22(V); }while(0) - -#define SERIAL_ECHOPAIR(V...) _SEP_N(NUM_ARGS(V),V) - -// Print up to 12 pairs of values. Odd elements must be PSTR pointers. -#define __SEP_N_P(N,V...) _SEP_##N##_P(V) -#define _SEP_N_P(N,V...) __SEP_N_P(N,V) -#define _SEP_1_P(PRE) serialprintPGM(PRE) -#define _SEP_2_P(PRE,V) serial_echopair_PGM(PRE,V) -#define _SEP_3_P(a,b,c) do{ _SEP_2_P(a,b); serialprintPGM(c); }while(0) -#define _SEP_4_P(a,b,V...) do{ _SEP_2_P(a,b); _SEP_2_P(V); }while(0) -#define _SEP_5_P(a,b,V...) do{ _SEP_2_P(a,b); _SEP_3_P(V); }while(0) -#define _SEP_6_P(a,b,V...) do{ _SEP_2_P(a,b); _SEP_4_P(V); }while(0) -#define _SEP_7_P(a,b,V...) do{ _SEP_2_P(a,b); _SEP_5_P(V); }while(0) -#define _SEP_8_P(a,b,V...) do{ _SEP_2_P(a,b); _SEP_6_P(V); }while(0) -#define _SEP_9_P(a,b,V...) do{ _SEP_2_P(a,b); _SEP_7_P(V); }while(0) -#define _SEP_10_P(a,b,V...) do{ _SEP_2_P(a,b); _SEP_8_P(V); }while(0) -#define _SEP_11_P(a,b,V...) do{ _SEP_2_P(a,b); _SEP_9_P(V); }while(0) -#define _SEP_12_P(a,b,V...) do{ _SEP_2_P(a,b); _SEP_10_P(V); }while(0) -#define _SEP_13_P(a,b,V...) do{ _SEP_2_P(a,b); _SEP_11_P(V); }while(0) -#define _SEP_14_P(a,b,V...) do{ _SEP_2_P(a,b); _SEP_12_P(V); }while(0) -#define _SEP_15_P(a,b,V...) do{ _SEP_2_P(a,b); _SEP_13_P(V); }while(0) -#define _SEP_16_P(a,b,V...) do{ _SEP_2_P(a,b); _SEP_14_P(V); }while(0) -#define _SEP_17_P(a,b,V...) do{ _SEP_2_P(a,b); _SEP_15_P(V); }while(0) -#define _SEP_18_P(a,b,V...) do{ _SEP_2_P(a,b); _SEP_16_P(V); }while(0) -#define _SEP_19_P(a,b,V...) do{ _SEP_2_P(a,b); _SEP_17_P(V); }while(0) -#define _SEP_20_P(a,b,V...) do{ _SEP_2_P(a,b); _SEP_18_P(V); }while(0) -#define _SEP_21_P(a,b,V...) do{ _SEP_2_P(a,b); _SEP_19_P(V); }while(0) -#define _SEP_22_P(a,b,V...) do{ _SEP_2_P(a,b); _SEP_20_P(V); }while(0) -#define _SEP_23_P(a,b,V...) do{ _SEP_2_P(a,b); _SEP_21_P(V); }while(0) -#define _SEP_24_P(a,b,V...) do{ _SEP_2_P(a,b); _SEP_22_P(V); }while(0) - -// SERIAL_ECHOPAIR_P is used to output a key value pair. Unlike SERIAL_ECHOPAIR, the key must be a PGM string already and the value can be anything -#define SERIAL_ECHOPAIR_P(V...) _SEP_N_P(NUM_ARGS(V),V) - -// Print up to 12 pairs of values followed by newline -#define __SELP_N(N,V...) _SELP_##N(V) -#define _SELP_N(N,V...) __SELP_N(N,V) -#define _SELP_1(PRE) SERIAL_ECHOLNPGM(PRE) -#define _SELP_2(PRE,V) do{ serial_echopair_PGM(PSTR(PRE),V); SERIAL_EOL(); }while(0) -#define _SELP_3(a,b,c) do{ _SEP_2(a,b); SERIAL_ECHOLNPGM(c); }while(0) -#define _SELP_4(a,b,V...) do{ _SEP_2(a,b); _SELP_2(V); }while(0) -#define _SELP_5(a,b,V...) do{ _SEP_2(a,b); _SELP_3(V); }while(0) -#define _SELP_6(a,b,V...) do{ _SEP_2(a,b); _SELP_4(V); }while(0) -#define _SELP_7(a,b,V...) do{ _SEP_2(a,b); _SELP_5(V); }while(0) -#define _SELP_8(a,b,V...) do{ _SEP_2(a,b); _SELP_6(V); }while(0) -#define _SELP_9(a,b,V...) do{ _SEP_2(a,b); _SELP_7(V); }while(0) -#define _SELP_10(a,b,V...) do{ _SEP_2(a,b); _SELP_8(V); }while(0) -#define _SELP_11(a,b,V...) do{ _SEP_2(a,b); _SELP_9(V); }while(0) -#define _SELP_12(a,b,V...) do{ _SEP_2(a,b); _SELP_10(V); }while(0) -#define _SELP_13(a,b,V...) do{ _SEP_2(a,b); _SELP_11(V); }while(0) -#define _SELP_14(a,b,V...) do{ _SEP_2(a,b); _SELP_12(V); }while(0) -#define _SELP_15(a,b,V...) do{ _SEP_2(a,b); _SELP_13(V); }while(0) -#define _SELP_16(a,b,V...) do{ _SEP_2(a,b); _SELP_14(V); }while(0) -#define _SELP_17(a,b,V...) do{ _SEP_2(a,b); _SELP_15(V); }while(0) -#define _SELP_18(a,b,V...) do{ _SEP_2(a,b); _SELP_16(V); }while(0) -#define _SELP_19(a,b,V...) do{ _SEP_2(a,b); _SELP_17(V); }while(0) -#define _SELP_20(a,b,V...) do{ _SEP_2(a,b); _SELP_18(V); }while(0) -#define _SELP_21(a,b,V...) do{ _SEP_2(a,b); _SELP_19(V); }while(0) -#define _SELP_22(a,b,V...) do{ _SEP_2(a,b); _SELP_20(V); }while(0) -#define _SELP_23(a,b,V...) do{ _SEP_2(a,b); _SELP_21(V); }while(0) -#define _SELP_24(a,b,V...) do{ _SEP_2(a,b); _SELP_22(V); }while(0) -#define _SELP_25(a,b,V...) do{ _SEP_2(a,b); _SELP_23(V); }while(0) -#define _SELP_26(a,b,V...) do{ _SEP_2(a,b); _SELP_24(V); }while(0) -#define _SELP_27(a,b,V...) do{ _SEP_2(a,b); _SELP_25(V); }while(0) -#define _SELP_28(a,b,V...) do{ _SEP_2(a,b); _SELP_26(V); }while(0) -#define _SELP_29(a,b,V...) do{ _SEP_2(a,b); _SELP_27(V); }while(0) -#define _SELP_30(a,b,V...) do{ _SEP_2(a,b); _SELP_28(V); }while(0) // Eat two args, pass the rest up - -#define SERIAL_ECHOLNPAIR(V...) _SELP_N(NUM_ARGS(V),V) - -// Print up to 12 pairs of values followed by newline -#define __SELP_N_P(N,V...) _SELP_##N##_P(V) -#define _SELP_N_P(N,V...) __SELP_N_P(N,V) -#define _SELP_1_P(PRE) serialprintPGM(PRE) -#define _SELP_2_P(PRE,V) do{ serial_echopair_PGM(PRE,V); SERIAL_EOL(); }while(0) -#define _SELP_3_P(a,b,c) do{ _SEP_2_P(a,b); serialprintPGM(c); }while(0) -#define _SELP_4_P(a,b,V...) do{ _SEP_2_P(a,b); _SELP_2_P(V); }while(0) -#define _SELP_5_P(a,b,V...) do{ _SEP_2_P(a,b); _SELP_3_P(V); }while(0) -#define _SELP_6_P(a,b,V...) do{ _SEP_2_P(a,b); _SELP_4_P(V); }while(0) -#define _SELP_7_P(a,b,V...) do{ _SEP_2_P(a,b); _SELP_5_P(V); }while(0) -#define _SELP_8_P(a,b,V...) do{ _SEP_2_P(a,b); _SELP_6_P(V); }while(0) -#define _SELP_9_P(a,b,V...) do{ _SEP_2_P(a,b); _SELP_7_P(V); }while(0) -#define _SELP_10_P(a,b,V...) do{ _SEP_2_P(a,b); _SELP_8_P(V); }while(0) -#define _SELP_11_P(a,b,V...) do{ _SEP_2_P(a,b); _SELP_9_P(V); }while(0) -#define _SELP_12_P(a,b,V...) do{ _SEP_2_P(a,b); _SELP_10_P(V); }while(0) -#define _SELP_13_P(a,b,V...) do{ _SEP_2_P(a,b); _SELP_11_P(V); }while(0) -#define _SELP_14_P(a,b,V...) do{ _SEP_2_P(a,b); _SELP_12_P(V); }while(0) -#define _SELP_15_P(a,b,V...) do{ _SEP_2_P(a,b); _SELP_13_P(V); }while(0) -#define _SELP_16_P(a,b,V...) do{ _SEP_2_P(a,b); _SELP_14_P(V); }while(0) -#define _SELP_17_P(a,b,V...) do{ _SEP_2_P(a,b); _SELP_15_P(V); }while(0) -#define _SELP_18_P(a,b,V...) do{ _SEP_2_P(a,b); _SELP_16_P(V); }while(0) -#define _SELP_19_P(a,b,V...) do{ _SEP_2_P(a,b); _SELP_17_P(V); }while(0) -#define _SELP_20_P(a,b,V...) do{ _SEP_2_P(a,b); _SELP_18_P(V); }while(0) -#define _SELP_21_P(a,b,V...) do{ _SEP_2_P(a,b); _SELP_19_P(V); }while(0) -#define _SELP_22_P(a,b,V...) do{ _SEP_2_P(a,b); _SELP_20_P(V); }while(0) -#define _SELP_23_P(a,b,V...) do{ _SEP_2_P(a,b); _SELP_21_P(V); }while(0) -#define _SELP_24_P(a,b,V...) do{ _SEP_2_P(a,b); _SELP_22_P(V); }while(0) -#define _SELP_25_P(a,b,V...) do{ _SEP_2_P(a,b); _SELP_23_P(V); }while(0) -#define _SELP_26_P(a,b,V...) do{ _SEP_2_P(a,b); _SELP_24_P(V); }while(0) -#define _SELP_27_P(a,b,V...) do{ _SEP_2_P(a,b); _SELP_25_P(V); }while(0) -#define _SELP_28_P(a,b,V...) do{ _SEP_2_P(a,b); _SELP_26_P(V); }while(0) -#define _SELP_29_P(a,b,V...) do{ _SEP_2_P(a,b); _SELP_27_P(V); }while(0) -#define _SELP_30_P(a,b,V...) do{ _SEP_2_P(a,b); _SELP_28_P(V); }while(0) // Eat two args, pass the rest up - -#define SERIAL_ECHOLNPAIR_P(V...) _SELP_N_P(NUM_ARGS(V),V) +// Serial echo and error prefixes +#define SERIAL_ECHO_START() serial_echo_start() +#define SERIAL_ERROR_START() serial_error_start() + +// Serial end-of-line +#define SERIAL_EOL() SERIAL_CHAR('\n') + +// Print a single PROGMEM, PGM_P, or PSTR() string. +void serial_print_P(PGM_P str); +inline void serial_println_P(PGM_P str) { serial_print_P(str); SERIAL_EOL(); } + +// Print a single FSTR_P, F(), or FPSTR() string. +inline void serial_print(FSTR_P const fstr) { serial_print_P(FTOP(fstr)); } +inline void serial_println(FSTR_P const fstr) { serial_println_P(FTOP(fstr)); } + +// +// SERIAL_ECHOPGM... macros are used to output string-value pairs. +// + +// Print up to 20 pairs of values. Odd elements must be literal strings. +#define __SEP_N(N,V...) _SEP_##N(V) +#define _SEP_N(N,V...) __SEP_N(N,V) +#define _SEP_N_REF() _SEP_N +#define _SEP_1(s) serial_print(F(s)); +#define _SEP_2(s,v) serial_echopair(F(s),v); +#define _SEP_3(s,v,V...) _SEP_2(s,v); DEFER2(_SEP_N_REF)()(TWO_ARGS(V),V); +#define SERIAL_ECHOPGM(V...) do{ EVAL(_SEP_N(TWO_ARGS(V),V)); }while(0) + +// Print up to 20 pairs of values followed by newline. Odd elements must be literal strings. +#define __SELP_N(N,V...) _SELP_##N(V) +#define _SELP_N(N,V...) __SELP_N(N,V) +#define _SELP_N_REF() _SELP_N +#define _SELP_1(s) serial_print(F(s "\n")); +#define _SELP_2(s,v) serial_echolnpair(F(s),v); +#define _SELP_3(s,v,V...) _SEP_2(s,v); DEFER2(_SELP_N_REF)()(TWO_ARGS(V),V); +#define SERIAL_ECHOLNPGM(V...) do{ EVAL(_SELP_N(TWO_ARGS(V),V)); }while(0) + +// Print up to 20 pairs of values. Odd elements must be PSTR pointers. +#define __SEP_N_P(N,V...) _SEP_##N##_P(V) +#define _SEP_N_P(N,V...) __SEP_N_P(N,V) +#define _SEP_N_P_REF() _SEP_N_P +#define _SEP_1_P(p) serial_print_P(p); +#define _SEP_2_P(p,v) serial_echopair_P(p,v); +#define _SEP_3_P(p,v,V...) _SEP_2_P(p,v); DEFER2(_SEP_N_P_REF)()(TWO_ARGS(V),V); +#define SERIAL_ECHOPGM_P(V...) do{ EVAL(_SEP_N_P(TWO_ARGS(V),V)); }while(0) + +// Print up to 20 pairs of values followed by newline. Odd elements must be PSTR pointers. +#define __SELP_N_P(N,V...) _SELP_##N##_P(V) +#define _SELP_N_P(N,V...) __SELP_N_P(N,V) +#define _SELP_N_P_REF() _SELP_N_P +#define _SELP_1_P(p) serial_println_P(p) +#define _SELP_2_P(p,v) serial_echolnpair_P(p,v) +#define _SELP_3_P(p,v,V...) { _SEP_2_P(p,v); DEFER2(_SELP_N_P_REF)()(TWO_ARGS(V),V); } +#define SERIAL_ECHOLNPGM_P(V...) do{ EVAL(_SELP_N_P(TWO_ARGS(V),V)); }while(0) + +// Print up to 20 pairs of values. Odd elements must be FSTR_P, F(), or FPSTR(). +#define __SEP_N_F(N,V...) _SEP_##N##_F(V) +#define _SEP_N_F(N,V...) __SEP_N_F(N,V) +#define _SEP_N_F_REF() _SEP_N_F +#define _SEP_1_F(p) serial_print(p); +#define _SEP_2_F(p,v) serial_echopair(p,v); +#define _SEP_3_F(p,v,V...) _SEP_2_F(p,v); DEFER2(_SEP_N_F_REF)()(TWO_ARGS(V),V); +#define SERIAL_ECHOF(V...) do{ EVAL(_SEP_N_F(TWO_ARGS(V),V)); }while(0) + +// Print up to 20 pairs of values followed by newline. Odd elements must be FSTR_P, F(), or FPSTR(). +#define __SELP_N_F(N,V...) _SELP_##N##_F(V) +#define _SELP_N_F(N,V...) __SELP_N_F(N,V) +#define _SELP_N_F_REF() _SELP_N_F +#define _SELP_1_F(p) serial_println(p) +#define _SELP_2_F(p,v) serial_echolnpair(p,v) +#define _SELP_3_F(p,v,V...) { _SEP_2_F(p,v); DEFER2(_SELP_N_F_REF)()(TWO_ARGS(V),V); } +#define SERIAL_ECHOLNF(V...) do{ EVAL(_SELP_N_F(TWO_ARGS(V),V)); }while(0) #ifdef AllowDifferentTypeInList @@ -279,53 +263,49 @@ void serialprintPGM(PGM_P str); template void SERIAL_ECHOLIST_IMPL(T && t, Args && ... args) { SERIAL_IMPL.print(t); - serialprintPGM(PSTR(", ")); + serial_print(F(", ")); SERIAL_ECHOLIST_IMPL(args...); } template - void SERIAL_ECHOLIST(PGM_P const str, Args && ... args) { - SERIAL_IMPL.print(str); + void SERIAL_ECHOLIST(FSTR_P const str, Args && ... args) { + SERIAL_IMPL.print(FTOP(str)); SERIAL_ECHOLIST_IMPL(args...); } #else // Optimization if the listed type are all the same (seems to be the case in the codebase so use that instead) template - void SERIAL_ECHOLIST(PGM_P const str, Args && ... args) { - serialprintPGM(str); + void SERIAL_ECHOLIST(FSTR_P const fstr, Args && ... args) { + serial_print(fstr); typename Private::first_type_of::type values[] = { args... }; constexpr size_t argsSize = sizeof...(args); for (size_t i = 0; i < argsSize; i++) { - if (i) serialprintPGM(PSTR(", ")); + if (i) serial_print(F(", ")); SERIAL_IMPL.print(values[i]); } } #endif -#define SERIAL_ECHOPGM_P(P) (serialprintPGM(P)) -#define SERIAL_ECHOLNPGM_P(P) do{ serialprintPGM(P); SERIAL_EOL(); }while(0) - -#define SERIAL_ECHOPGM(S) (serialprintPGM(PSTR(S))) -#define SERIAL_ECHOLNPGM(S) (serialprintPGM(PSTR(S "\n"))) +// SERIAL_ECHO_F prints a floating point value with optional precision +inline void SERIAL_ECHO_F(EnsureDouble x, int digit=2) { SERIAL_IMPL.print(x, digit); } -#define SERIAL_ECHOPAIR_F_P(P,V...) do{ serialprintPGM(P); SERIAL_ECHO_F(V); }while(0) -#define SERIAL_ECHOLNPAIR_F_P(V...) do{ SERIAL_ECHOPAIR_F_P(V); SERIAL_EOL(); }while(0) +#define SERIAL_ECHOPAIR_F_P(P,V...) do{ serial_print_P(P); SERIAL_ECHO_F(V); }while(0) +#define SERIAL_ECHOLNPAIR_F_P(P,V...) do{ SERIAL_ECHOPAIR_F_P(P,V); SERIAL_EOL(); }while(0) -#define SERIAL_ECHOPAIR_F(S,V...) SERIAL_ECHOPAIR_F_P(PSTR(S),V) -#define SERIAL_ECHOLNPAIR_F(V...) do{ SERIAL_ECHOPAIR_F(V); SERIAL_EOL(); }while(0) +#define SERIAL_ECHOPAIR_F_F(S,V...) do{ serial_print(S); SERIAL_ECHO_F(V); }while(0) +#define SERIAL_ECHOLNPAIR_F_F(S,V...) do{ SERIAL_ECHOPAIR_F_F(S,V); SERIAL_EOL(); }while(0) -#define SERIAL_ECHO_START() serial_echo_start() -#define SERIAL_ERROR_START() serial_error_start() -#define SERIAL_EOL() SERIAL_CHAR('\n') +#define SERIAL_ECHOPAIR_F(S,V...) SERIAL_ECHOPAIR_F_F(F(S),V) +#define SERIAL_ECHOLNPAIR_F(V...) do{ SERIAL_ECHOPAIR_F(V); SERIAL_EOL(); }while(0) -#define SERIAL_ECHO_MSG(V...) do{ SERIAL_ECHO_START(); SERIAL_ECHOLNPAIR(V); }while(0) -#define SERIAL_ERROR_MSG(V...) do{ SERIAL_ERROR_START(); SERIAL_ECHOLNPAIR(V); }while(0) +#define SERIAL_ECHO_MSG(V...) do{ SERIAL_ECHO_START(); SERIAL_ECHOLNPGM(V); }while(0) +#define SERIAL_ERROR_MSG(V...) do{ SERIAL_ERROR_START(); SERIAL_ECHOLNPGM(V); }while(0) -#define SERIAL_ECHO_SP(C) serial_spaces(C) +#define SERIAL_ECHO_SP(C) serial_spaces(C) -#define SERIAL_ECHO_TERNARY(TF, PRE, ON, OFF, POST) serial_ternary(TF, PSTR(PRE), PSTR(ON), PSTR(OFF), PSTR(POST)) +#define SERIAL_ECHO_TERNARY(TF, PRE, ON, OFF, POST) serial_ternary(TF, F(PRE), F(ON), F(OFF), F(POST)) #if SERIAL_FLOAT_PRECISION #define SERIAL_DECIMAL(V) SERIAL_PRINT(V, SERIAL_FLOAT_PRECISION) @@ -336,33 +316,42 @@ void serialprintPGM(PGM_P str); // // Functions for serial printing from PROGMEM. (Saves loads of SRAM.) // -void serial_echopair_PGM(PGM_P const s_P, serial_char_t v); -void serial_echopair_PGM(PGM_P const s_P, const char *v); -void serial_echopair_PGM(PGM_P const s_P, char v); -void serial_echopair_PGM(PGM_P const s_P, int v); -void serial_echopair_PGM(PGM_P const s_P, long v); -void serial_echopair_PGM(PGM_P const s_P, float v); -void serial_echopair_PGM(PGM_P const s_P, double v); -void serial_echopair_PGM(PGM_P const s_P, unsigned char v); -void serial_echopair_PGM(PGM_P const s_P, unsigned int v); -void serial_echopair_PGM(PGM_P const s_P, unsigned long v); -inline void serial_echopair_PGM(PGM_P const s_P, bool v) { serial_echopair_PGM(s_P, (int)v); } -inline void serial_echopair_PGM(PGM_P const s_P, void *v) { serial_echopair_PGM(s_P, (uintptr_t)v); } +inline void serial_echopair_P(PGM_P const pstr, serial_char_t v) { serial_print_P(pstr); SERIAL_CHAR(v.c); } +inline void serial_echopair_P(PGM_P const pstr, float v) { serial_print_P(pstr); SERIAL_DECIMAL(v); } +inline void serial_echopair_P(PGM_P const pstr, double v) { serial_print_P(pstr); SERIAL_DECIMAL(v); } +//inline void serial_echopair_P(PGM_P const pstr, const char *v) { serial_print_P(pstr); SERIAL_ECHO(v); } +inline void serial_echopair_P(PGM_P const pstr, FSTR_P v) { serial_print_P(pstr); SERIAL_ECHOF(v); } + +// Default implementation for types without a specialization. Handles integers. +template +inline void serial_echopair_P(PGM_P const pstr, T v) { serial_print_P(pstr); SERIAL_ECHO(v); } + +// Add a newline. +template +inline void serial_echolnpair_P(PGM_P const pstr, T v) { serial_echopair_P(pstr, v); SERIAL_EOL(); } + +// Catch-all for __FlashStringHelper * +template +inline void serial_echopair(FSTR_P const fstr, T v) { serial_echopair_P(FTOP(fstr), v); } + +// Add a newline to the serial output +template +inline void serial_echolnpair(FSTR_P const fstr, T v) { serial_echolnpair_P(FTOP(fstr), v); } void serial_echo_start(); void serial_error_start(); -void serial_ternary(const bool onoff, PGM_P const pre, PGM_P const on, PGM_P const off, PGM_P const post=nullptr); +void serial_ternary(const bool onoff, FSTR_P const pre, FSTR_P const on, FSTR_P const off, FSTR_P const post=nullptr); void serialprint_onoff(const bool onoff); void serialprintln_onoff(const bool onoff); void serialprint_truefalse(const bool tf); void serial_spaces(uint8_t count); void print_bin(const uint16_t val); -void print_xyz(const float &x, const float &y, const float &z, PGM_P const prefix=nullptr, PGM_P const suffix=nullptr); +void print_pos(LINEAR_AXIS_ARGS(const_float_t), FSTR_P const prefix=nullptr, FSTR_P const suffix=nullptr); -inline void print_xyz(const xyz_pos_t &xyz, PGM_P const prefix=nullptr, PGM_P const suffix=nullptr) { - print_xyz(xyz.x, xyz.y, xyz.z, prefix, suffix); +inline void print_pos(const xyz_pos_t &xyz, FSTR_P const prefix=nullptr, FSTR_P const suffix=nullptr) { + print_pos(LINEAR_AXIS_ELEM(xyz), prefix, suffix); } -#define SERIAL_POS(SUFFIX,VAR) do { print_xyz(VAR, PSTR(" " STRINGIFY(VAR) "="), PSTR(" : " SUFFIX "\n")); }while(0) -#define SERIAL_XYZ(PREFIX,V...) do { print_xyz(V, PSTR(PREFIX), nullptr); }while(0) +#define SERIAL_POS(SUFFIX,VAR) do { print_pos(VAR, F(" " STRINGIFY(VAR) "="), F(" : " SUFFIX "\n")); }while(0) +#define SERIAL_XYZ(PREFIX,V...) do { print_pos(V, F(PREFIX)); }while(0) diff --git a/Marlin/src/core/serial_base.h b/Marlin/src/core/serial_base.h index 418bb557e7cb..a5abd67d8727 100644 --- a/Marlin/src/core/serial_base.h +++ b/Marlin/src/core/serial_base.h @@ -22,15 +22,28 @@ #pragma once #include "../inc/MarlinConfigPre.h" -#include "macros.h" #if ENABLED(EMERGENCY_PARSER) #include "../feature/e_parser.h" #endif -// flushTX is not implemented in all HAL, so use SFINAE to call the method where it is. -CALL_IF_EXISTS_IMPL(void, flushTX); -CALL_IF_EXISTS_IMPL(bool, connected, true); +// Used in multiple places +// You can build it but not manipulate it. +// There are only few places where it's required to access the underlying member: GCodeQueue, SerialMask and MultiSerial +struct serial_index_t { + // A signed index, where -1 is a special case meaning no action (neither output or input) + int8_t index; + + // Check if the index is within the range [a ... b] + constexpr inline bool within(const int8_t a, const int8_t b) const { return WITHIN(index, a, b); } + constexpr inline bool valid() const { return WITHIN(index, 0, 7); } // At most, 8 bits + + // Construction is either from an index + constexpr serial_index_t(const int8_t index) : index(index) {} + + // Default to "no index" + constexpr serial_index_t() : index(-1) {} +}; // In order to catch usage errors in code, we make the base to encode number explicit // If given a number (and not this enum), the compiler will reject the overload, falling back to the (double, digit) version @@ -42,19 +55,34 @@ enum class PrintBase { Bin = 2 }; -// A simple forward struct that prevent the compiler to select print(double, int) as a default overload for any type different than -// double or float. For double or float, a conversion exists so the call will be transparent +// A simple feature list enumeration +enum class SerialFeature { + None = 0x00, + MeatPack = 0x01, //!< Enabled when Meatpack is present + BinaryFileTransfer = 0x02, //!< Enabled for BinaryFile transfer support (in the future) + Virtual = 0x04, //!< Enabled for virtual serial port (like Telnet / Websocket / ...) + Hookable = 0x08, //!< Enabled if the serial class supports a setHook method +}; +ENUM_FLAGS(SerialFeature); + +// flushTX is not implemented in all HAL, so use SFINAE to call the method where it is. +CALL_IF_EXISTS_IMPL(void, flushTX); +CALL_IF_EXISTS_IMPL(bool, connected, true); +CALL_IF_EXISTS_IMPL(SerialFeature, features, SerialFeature::None); + +// A simple forward struct to prevent the compiler from selecting print(double, int) as a default overload +// for any type other than double/float. For double/float, a conversion exists so the call will be invisible. struct EnsureDouble { double a; - FORCE_INLINE operator double() { return a; } - // If the compiler breaks on ambiguity here, it's likely because you're calling print(X, base) with X not a double or a float, and a - // base that's not one of PrintBase's value. This exact code is made to detect such error, you NEED to set a base explicitely like this: + operator double() { return a; } + // If the compiler breaks on ambiguity here, it's likely because print(X, base) is called with X not a double/float, and + // a base that's not a PrintBase value. This code is made to detect the error. You MUST set a base explicitly like this: // SERIAL_PRINT(v, PrintBase::Hex) - FORCE_INLINE EnsureDouble(double a) : a(a) {} - FORCE_INLINE EnsureDouble(float a) : a(a) {} + EnsureDouble(double a) : a(a) {} + EnsureDouble(float a) : a(a) {} }; -// Using Curiously Recurring Template Pattern here to avoid virtual table cost when compiling. +// Using Curiously-Recurring Template Pattern here to avoid virtual table cost when compiling. // Since the real serial class is known at compile time, this results in the compiler writing // a completely efficient code. template @@ -68,93 +96,130 @@ struct SerialBase { SerialBase(const bool) {} #endif + #define SerialChild static_cast(this) + // Static dispatch methods below: // The most important method here is where it all ends to: - size_t write(uint8_t c) { return static_cast(this)->write(c); } + void write(uint8_t c) { SerialChild->write(c); } + // Called when the parser finished processing an instruction, usually build to nothing - void msgDone() { static_cast(this)->msgDone(); } - // Called upon initialization - void begin(const long baudRate) { static_cast(this)->begin(baudRate); } - // Called upon destruction - void end() { static_cast(this)->end(); } + void msgDone() const { SerialChild->msgDone(); } + + // Called on initialization + void begin(const long baudRate) { SerialChild->begin(baudRate); } + + // Called on destruction + void end() { SerialChild->end(); } + /** Check for available data from the port @param index The port index, usually 0 */ - int available(uint8_t index = 0) { return static_cast(this)->available(index); } + int available(serial_index_t index=0) const { return SerialChild->available(index); } + /** Read a value from the port @param index The port index, usually 0 */ - int read(uint8_t index = 0) { return static_cast(this)->read(index); } + int read(serial_index_t index=0) { return SerialChild->read(index); } + + /** Combine the features of this serial instance and return it + @param index The port index, usually 0 */ + SerialFeature features(serial_index_t index=0) const { return static_cast(this)->features(index); } + + // Check if the serial port has a feature + bool has_feature(serial_index_t index, SerialFeature flag) const { return (features(index) & flag) != SerialFeature::None; } + // Check if the serial port is connected (usually bypassed) - bool connected() { return static_cast(this)->connected(); } + bool connected() const { return SerialChild->connected(); } + // Redirect flush - void flush() { static_cast(this)->flush(); } + void flush() { SerialChild->flush(); } + // Not all implementation have a flushTX, so let's call them only if the child has the implementation - void flushTX() { CALL_IF_EXISTS(void, static_cast(this), flushTX); } + void flushTX() { CALL_IF_EXISTS(void, SerialChild, flushTX); } // Glue code here - FORCE_INLINE void write(const char* str) { while (*str) write(*str++); } - FORCE_INLINE void write(const uint8_t* buffer, size_t size) { while (size--) write(*buffer++); } - FORCE_INLINE void print(const char* str) { write(str); } + void write(const char *str) { while (*str) write(*str++); } + void write(const uint8_t *buffer, size_t size) { while (size--) write(*buffer++); } + void print(char *str) { write(str); } + void print(const char *str) { write(str); } // No default argument to avoid ambiguity - NO_INLINE void print(char c, PrintBase base) { printNumber((signed long)c, (uint8_t)base); } - NO_INLINE void print(unsigned char c, PrintBase base) { printNumber((unsigned long)c, (uint8_t)base); } - NO_INLINE void print(int c, PrintBase base) { printNumber((signed long)c, (uint8_t)base); } - NO_INLINE void print(unsigned int c, PrintBase base) { printNumber((unsigned long)c, (uint8_t)base); } - void print(unsigned long c, PrintBase base) { printNumber((unsigned long)c, (uint8_t)base); } - void print(long c, PrintBase base) { printNumber((signed long)c, (uint8_t)base); } - void print(EnsureDouble c, int digits) { printFloat(c, digits); } + + // Define print for every fundamental integer type, to ensure that all redirect properly + // to the correct underlying implementation. + + // Prints are performed with a single size, to avoid needing multiple print functions. + // The fixed integer size used for prints will be the larger of long or a pointer. + #if __LONG_WIDTH__ >= __INTPTR_WIDTH__ + typedef long int_fixed_print_t; + typedef unsigned long uint_fixed_print_t; + #else + typedef intptr_t int_fixed_print_t; + typedef uintptr_t uint_fixed_print_t; + + FORCE_INLINE void print(intptr_t c, PrintBase base) { printNumber_signed(c, base); } + FORCE_INLINE void print(uintptr_t c, PrintBase base) { printNumber_unsigned(c, base); } + #endif + + FORCE_INLINE void print(char c, PrintBase base) { printNumber_signed(c, base); } + FORCE_INLINE void print(short c, PrintBase base) { printNumber_signed(c, base); } + FORCE_INLINE void print(int c, PrintBase base) { printNumber_signed(c, base); } + FORCE_INLINE void print(long c, PrintBase base) { printNumber_signed(c, base); } + FORCE_INLINE void print(unsigned char c, PrintBase base) { printNumber_unsigned(c, base); } + FORCE_INLINE void print(unsigned short c, PrintBase base) { printNumber_unsigned(c, base); } + FORCE_INLINE void print(unsigned int c, PrintBase base) { printNumber_unsigned(c, base); } + FORCE_INLINE void print(unsigned long c, PrintBase base) { printNumber_unsigned(c, base); } + + + void print(EnsureDouble c, int digits) { printFloat(c, digits); } // Forward the call to the former's method - FORCE_INLINE void print(char c) { print(c, PrintBase::Dec); } - FORCE_INLINE void print(unsigned char c) { print(c, PrintBase::Dec); } - FORCE_INLINE void print(int c) { print(c, PrintBase::Dec); } - FORCE_INLINE void print(unsigned int c) { print(c, PrintBase::Dec); } - FORCE_INLINE void print(unsigned long c) { print(c, PrintBase::Dec); } - FORCE_INLINE void print(long c) { print(c, PrintBase::Dec); } - FORCE_INLINE void print(double c) { print(c, 2); } - - FORCE_INLINE void println(const char s[]) { print(s); println(); } - FORCE_INLINE void println(char c, PrintBase base) { print(c, base); println(); } - FORCE_INLINE void println(unsigned char c, PrintBase base) { print(c, base); println(); } - FORCE_INLINE void println(int c, PrintBase base) { print(c, base); println(); } - FORCE_INLINE void println(unsigned int c, PrintBase base) { print(c, base); println(); } - FORCE_INLINE void println(long c, PrintBase base) { print(c, base); println(); } - FORCE_INLINE void println(unsigned long c, PrintBase base) { print(c, base); println(); } - FORCE_INLINE void println(double c, int digits) { print(c, digits); println(); } - FORCE_INLINE void println() { write('\r'); write('\n'); } + + // Default implementation for anything without a specialization + // This handles integers since they are the most common + template + void print(T c) { print(c, PrintBase::Dec); } + + void print(float c) { print(c, 2); } + void print(double c) { print(c, 2); } + + void println(char *s) { print(s); println(); } + void println(const char *s) { print(s); println(); } + void println(float c, int digits) { print(c, digits); println(); } + void println(double c, int digits) { print(c, digits); println(); } + void println() { write('\r'); write('\n'); } + + // Default implementations for types without a specialization. Handles integers. + template + void println(T c, PrintBase base) { print(c, base); println(); } + + template + void println(T c) { println(c, PrintBase::Dec); } // Forward the call to the former's method - FORCE_INLINE void println(char c) { println(c, PrintBase::Dec); } - FORCE_INLINE void println(unsigned char c) { println(c, PrintBase::Dec); } - FORCE_INLINE void println(int c) { println(c, PrintBase::Dec); } - FORCE_INLINE void println(unsigned int c) { println(c, PrintBase::Dec); } - FORCE_INLINE void println(unsigned long c) { println(c, PrintBase::Dec); } - FORCE_INLINE void println(long c) { println(c, PrintBase::Dec); } - FORCE_INLINE void println(double c) { println(c, 2); } + void println(float c) { println(c, 2); } + void println(double c) { println(c, 2); } // Print a number with the given base - NO_INLINE void printNumber(unsigned long n, const uint8_t base) { - if (!base) return; // Hopefully, this should raise visible bug immediately - + NO_INLINE void printNumber_unsigned(uint_fixed_print_t n, PrintBase base) { if (n) { unsigned char buf[8 * sizeof(long)]; // Enough space for base 2 int8_t i = 0; while (n) { - buf[i++] = n % base; - n /= base; + buf[i++] = n % (uint_fixed_print_t)base; + n /= (uint_fixed_print_t)base; } while (i--) write((char)(buf[i] + (buf[i] < 10 ? '0' : 'A' - 10))); } else write('0'); } - void printNumber(signed long n, const uint8_t base) { - if (base == 10 && n < 0) { + + NO_INLINE void printNumber_signed(int_fixed_print_t n, PrintBase base) { + if (base == PrintBase::Dec && n < 0) { n = -n; // This works because all platforms Marlin's builds on are using 2-complement encoding for negative number // On such CPU, changing the sign of a number is done by inverting the bits and adding one, so if n = 0x80000000 = -2147483648 then // -n = 0x7FFFFFFF + 1 => 0x80000000 = 2147483648 (if interpreted as unsigned) or -2147483648 if interpreted as signed. // On non 2-complement CPU, there would be no possible representation for 2147483648. write('-'); } - printNumber((unsigned long)n , base); + printNumber_unsigned((uint_fixed_print_t)n , base); } // Print a decimal number @@ -173,7 +238,7 @@ struct SerialBase { // Extract the integer part of the number and print it unsigned long int_part = (unsigned long)number; double remainder = number - (double)int_part; - printNumber(int_part, 10); + printNumber_unsigned(int_part, PrintBase::Dec); // Print the decimal point, but only if there are digits beyond if (digits) { @@ -182,7 +247,7 @@ struct SerialBase { while (digits--) { remainder *= 10.0; unsigned long toPrint = (unsigned long)remainder; - printNumber(toPrint, 10); + printNumber_unsigned(toPrint, PrintBase::Dec); remainder -= toPrint; } } diff --git a/Marlin/src/core/serial_hook.h b/Marlin/src/core/serial_hook.h index afd43892c720..9b9fa8fa3828 100644 --- a/Marlin/src/core/serial_hook.h +++ b/Marlin/src/core/serial_hook.h @@ -21,11 +21,32 @@ */ #pragma once -#include "macros.h" #include "serial_base.h" -// Used in multiple places -typedef int8_t serial_index_t; +// A mask containing a bitmap of the serial port to act upon +// This is written to ensure a serial index is never used as a serial mask +class SerialMask { + uint8_t mask; + + // This constructor is private to ensure you can't convert an index to a mask + // The compiler will stop here if you are mixing index and mask in your code. + // If you need to, you'll have to use the explicit static "from" method here + SerialMask(const serial_index_t); + +public: + inline constexpr bool enabled(const SerialMask PortMask) const { return mask & PortMask.mask; } + inline constexpr SerialMask combine(const SerialMask other) const { return SerialMask(mask | other.mask); } + inline constexpr SerialMask operator<< (const int offset) const { return SerialMask(mask << offset); } + static SerialMask from(const serial_index_t index) { + if (index.valid()) return SerialMask(_BV(index.index)); + return SerialMask(0); // A invalid index mean no output + } + + constexpr SerialMask(const uint8_t mask) : mask(mask) {} + constexpr SerialMask(const SerialMask & other) : mask(other.mask) {} // Can't use = default here since not all framework support this + + static constexpr uint8_t All = 0xFF; +}; // The most basic serial class: it dispatch to the base serial class with no hook whatsoever. This will compile to nothing but the base serial class template @@ -39,12 +60,14 @@ struct BaseSerial : public SerialBase< BaseSerial >, public SerialT { void msgDone() {} // We don't care about indices here, since if one can call us, it's the right index anyway - int available(uint8_t) { return (int)SerialT::available(); } - int read(uint8_t) { return (int)SerialT::read(); } - bool connected() { return CALL_IF_EXISTS(bool, static_cast(this), connected);; } - void flushTX() { CALL_IF_EXISTS(void, static_cast(this), flushTX); } + int available(serial_index_t) { return (int)SerialT::available(); } + int read(serial_index_t) { return (int)SerialT::read(); } + bool connected() { return CALL_IF_EXISTS(bool, static_cast(this), connected);; } + void flushTX() { CALL_IF_EXISTS(void, static_cast(this), flushTX); } + + SerialFeature features(serial_index_t index) const { return CALL_IF_EXISTS(SerialFeature, static_cast(this), features, index); } - // We have 2 implementation of the same method in both base class, let's say which one we want + // Two implementations of the same method exist in both base classes so indicate the right one using SerialT::available; using SerialT::read; using SerialT::begin; @@ -77,16 +100,16 @@ struct ConditionalSerial : public SerialBase< ConditionalSerial > { bool connected() { return CALL_IF_EXISTS(bool, &out, connected); } void flushTX() { CALL_IF_EXISTS(void, &out, flushTX); } - int available(uint8_t ) { return (int)out.available(); } - int read(uint8_t ) { return (int)out.read(); } - int available() { return (int)out.available(); } - int read() { return (int)out.read(); } - + int available(serial_index_t) { return (int)out.available(); } + int read(serial_index_t) { return (int)out.read(); } + int available() { return (int)out.available(); } + int read() { return (int)out.read(); } + SerialFeature features(serial_index_t index) const { return CALL_IF_EXISTS(SerialFeature, &out, features, index); } ConditionalSerial(bool & conditionVariable, SerialT & out, const bool e) : BaseClassT(e), condition(conditionVariable), out(out) {} }; -// A simple foward class that taking a reference to an existing serial instance (likely created in their respective framework) +// A simple forward class that taking a reference to an existing serial instance (likely created in their respective framework) template struct ForwardSerial : public SerialBase< ForwardSerial > { typedef SerialBase< ForwardSerial > BaseClassT; @@ -102,15 +125,16 @@ struct ForwardSerial : public SerialBase< ForwardSerial > { bool connected() { return Private::HasMember_connected::value ? CALL_IF_EXISTS(bool, &out, connected) : (bool)out; } void flushTX() { CALL_IF_EXISTS(void, &out, flushTX); } - int available(uint8_t) { return (int)out.available(); } - int read(uint8_t) { return (int)out.read(); } + int available(serial_index_t) { return (int)out.available(); } + int read(serial_index_t) { return (int)out.read(); } int available() { return (int)out.available(); } int read() { return (int)out.read(); } + SerialFeature features(serial_index_t index) const { return CALL_IF_EXISTS(SerialFeature, &out, features, index); } ForwardSerial(const bool e, SerialT & out) : BaseClassT(e), out(out) {} }; -// A class that's can be hooked and unhooked at runtime, useful to capturing the output of the serial interface +// A class that can be hooked and unhooked at runtime, useful to capture the output of the serial interface template struct RuntimeSerial : public SerialBase< RuntimeSerial >, public SerialT { typedef SerialBase< RuntimeSerial > BaseClassT; @@ -130,8 +154,8 @@ struct RuntimeSerial : public SerialBase< RuntimeSerial >, public Seria if (eofHook) eofHook(userPointer); } - int available(uint8_t) { return (int)SerialT::available(); } - int read(uint8_t) { return (int)SerialT::read(); } + int available(serial_index_t) { return (int)SerialT::available(); } + int read(serial_index_t) { return (int)SerialT::read(); } using SerialT::available; using SerialT::read; using SerialT::flush; @@ -143,9 +167,15 @@ struct RuntimeSerial : public SerialBase< RuntimeSerial >, public Seria // Underlying implementation might use Arduino's bool operator bool connected() { - return Private::HasMember_connected::value ? CALL_IF_EXISTS(bool, static_cast(this), connected) : static_cast(this)->operator bool(); + return Private::HasMember_connected::value + ? CALL_IF_EXISTS(bool, static_cast(this), connected) + : static_cast(this)->operator bool(); } - void flushTX() { CALL_IF_EXISTS(void, static_cast(this), flushTX); } + + void flushTX() { CALL_IF_EXISTS(void, static_cast(this), flushTX); } + + // Append Hookable for this class + SerialFeature features(serial_index_t index) const { return SerialFeature::Hookable | CALL_IF_EXISTS(SerialFeature, static_cast(this), features, index); } void setHook(WriteHook writeHook = 0, EndOfMessageHook eofHook = 0, void * userPointer = 0) { // Order is important here as serial code can be called inside interrupts @@ -165,58 +195,71 @@ struct RuntimeSerial : public SerialBase< RuntimeSerial >, public Seria RuntimeSerial(const bool e, Args... args) : BaseClassT(e), SerialT(args...), writeHook(0), eofHook(0), userPointer(0) {} }; -// A class that's duplicating its output conditionally to 2 serial interface -template -struct MultiSerial : public SerialBase< MultiSerial > { - typedef SerialBase< MultiSerial > BaseClassT; +#define _S_CLASS(N) class Serial##N##T, +#define _S_NAME(N) Serial##N##T, - uint8_t portMask; - Serial0T & serial0; - Serial1T & serial1; +template < REPEAT(NUM_SERIAL, _S_CLASS) const uint8_t offset=0, const uint8_t step=1 > +struct MultiSerial : public SerialBase< MultiSerial< REPEAT(NUM_SERIAL, _S_NAME) offset, step > > { + typedef SerialBase< MultiSerial< REPEAT(NUM_SERIAL, _S_NAME) offset, step > > BaseClassT; - enum Masks { - UsageMask = ((1 << step) - 1), // A bit mask containing as many bits as step - FirstOutputMask = (UsageMask << offset), - SecondOutputMask = (UsageMask << (offset + step)), - AllMask = FirstOutputMask | SecondOutputMask, - }; + #undef _S_CLASS + #undef _S_NAME - NO_INLINE size_t write(uint8_t c) { - size_t ret = 0; - if (portMask & FirstOutputMask) ret = serial0.write(c); - if (portMask & SecondOutputMask) ret = serial1.write(c) | ret; - return ret; + SerialMask portMask; + + #define _S_DECLARE(N) Serial##N##T & serial##N; + REPEAT(NUM_SERIAL, _S_DECLARE); + #undef _S_DECLARE + + static constexpr uint8_t Usage = _BV(step) - 1; // A bit mask containing 'step' bits + + #define _OUT_PORT(N) (Usage << (offset + (step * N))), + static constexpr uint8_t output[] = { REPEAT(NUM_SERIAL, _OUT_PORT) }; + #undef _OUT_PORT + + #define _OUT_MASK(N) | output[N] + static constexpr uint8_t ALL = 0 REPEAT(NUM_SERIAL, _OUT_MASK); + #undef _OUT_MASK + + NO_INLINE void write(uint8_t c) { + #define _S_WRITE(N) if (portMask.enabled(output[N])) serial##N.write(c); + REPEAT(NUM_SERIAL, _S_WRITE); + #undef _S_WRITE } NO_INLINE void msgDone() { - if (portMask & FirstOutputMask) serial0.msgDone(); - if (portMask & SecondOutputMask) serial1.msgDone(); + #define _S_DONE(N) if (portMask.enabled(output[N])) serial##N.msgDone(); + REPEAT(NUM_SERIAL, _S_DONE); + #undef _S_DONE } - int available(uint8_t index) { - if (index >= 0 + offset && index < step + offset) - return serial0.available(index); - else if (index >= step + offset && index < 2 * step + offset) - return serial1.available(index); + int available(serial_index_t index) { + uint8_t pos = offset; + #define _S_AVAILABLE(N) if (index.within(pos, pos + step - 1)) return serial##N.available(index); else pos += step; + REPEAT(NUM_SERIAL, _S_AVAILABLE); + #undef _S_AVAILABLE return false; } - int read(uint8_t index) { - if (index >= 0 + offset && index < step + offset) - return serial0.read(index); - else if (index >= step + offset && index < 2 * step + offset) - return serial1.read(index); + int read(serial_index_t index) { + uint8_t pos = offset; + #define _S_READ(N) if (index.within(pos, pos + step - 1)) return serial##N.read(index); else pos += step; + REPEAT(NUM_SERIAL, _S_READ); + #undef _S_READ return -1; } void begin(const long br) { - if (portMask & FirstOutputMask) serial0.begin(br); - if (portMask & SecondOutputMask) serial1.begin(br); + #define _S_BEGIN(N) if (portMask.enabled(output[N])) serial##N.begin(br); + REPEAT(NUM_SERIAL, _S_BEGIN); + #undef _S_BEGIN } void end() { - if (portMask & FirstOutputMask) serial0.end(); - if (portMask & SecondOutputMask) serial1.end(); + #define _S_END(N) if (portMask.enabled(output[N])) serial##N.end(); + REPEAT(NUM_SERIAL, _S_END); + #undef _S_END } bool connected() { bool ret = true; - if (portMask & FirstOutputMask) ret = CALL_IF_EXISTS(bool, &serial0, connected); - if (portMask & SecondOutputMask) ret = ret && CALL_IF_EXISTS(bool, &serial1, connected); + #define _S_CONNECTED(N) if (portMask.enabled(output[N]) && !CALL_IF_EXISTS(bool, &serial##N, connected)) ret = false; + REPEAT(NUM_SERIAL, _S_CONNECTED); + #undef _S_CONNECTED return ret; } @@ -224,23 +267,40 @@ struct MultiSerial : public SerialBase< MultiSerial= 3 + #define Serial3Class ConditionalSerial + #endif #endif diff --git a/Marlin/src/core/types.h b/Marlin/src/core/types.h index 20519e1888b5..b95b59565920 100644 --- a/Marlin/src/core/types.h +++ b/Marlin/src/core/types.h @@ -26,8 +26,45 @@ #include "../inc/MarlinConfigPre.h" -class __FlashStringHelper; -typedef const __FlashStringHelper *progmem_str; +// +// Conditional type assignment magic. For example... +// +// typename IF<(MYOPT==12), int, float>::type myvar; +// +template +struct IF { typedef R type; }; +template +struct IF { typedef L type; }; + +#define LINEAR_AXIS_GANG(V...) GANG_N(LINEAR_AXES, V) +#define LINEAR_AXIS_CODE(V...) CODE_N(LINEAR_AXES, V) +#define LINEAR_AXIS_LIST(V...) LIST_N(LINEAR_AXES, V) +#define LINEAR_AXIS_ARRAY(V...) { LINEAR_AXIS_LIST(V) } +#define LINEAR_AXIS_ARGS(T...) LINEAR_AXIS_LIST(T x, T y, T z, T i, T j, T k) +#define LINEAR_AXIS_ELEM(O) LINEAR_AXIS_LIST(O.x, O.y, O.z, O.i, O.j, O.k) +#define LINEAR_AXIS_DEFS(T,V) LINEAR_AXIS_LIST(T x=V, T y=V, T z=V, T i=V, T j=V, T k=V) + +#define LOGICAL_AXIS_GANG(E,V...) LINEAR_AXIS_GANG(V) GANG_ITEM_E(E) +#define LOGICAL_AXIS_CODE(E,V...) LINEAR_AXIS_CODE(V) CODE_ITEM_E(E) +#define LOGICAL_AXIS_LIST(E,V...) LINEAR_AXIS_LIST(V) LIST_ITEM_E(E) +#define LOGICAL_AXIS_ARRAY(E,V...) { LOGICAL_AXIS_LIST(E,V) } +#define LOGICAL_AXIS_ARGS(T...) LOGICAL_AXIS_LIST(T e, T x, T y, T z, T i, T j, T k) +#define LOGICAL_AXIS_ELEM(O) LOGICAL_AXIS_LIST(O.e, O.x, O.y, O.z, O.i, O.j, O.k) +#define LOGICAL_AXIS_DECL(T,V) LOGICAL_AXIS_LIST(T e=V, T x=V, T y=V, T z=V, T i=V, T j=V, T k=V) + +#define LOGICAL_AXES_STRING LOGICAL_AXIS_GANG("E", "X", "Y", "Z", STR_I, STR_J, STR_K) + +#if HAS_EXTRUDERS + #define LIST_ITEM_E(N) , N + #define CODE_ITEM_E(N) ; N + #define GANG_ITEM_E(N) N +#else + #define LIST_ITEM_E(N) + #define CODE_ITEM_E(N) + #define GANG_ITEM_E(N) +#endif + +#define AXIS_COLLISION(L) (AXIS4_NAME == L || AXIS5_NAME == L || AXIS6_NAME == L) // // Enumerated axis indices @@ -37,51 +74,84 @@ typedef const __FlashStringHelper *progmem_str; // - X_HEAD, Y_HEAD, and Z_HEAD should be used for Steppers on Core kinematics // enum AxisEnum : uint8_t { - X_AXIS = 0, A_AXIS = 0, - Y_AXIS = 1, B_AXIS = 1, - Z_AXIS = 2, C_AXIS = 2, - E_AXIS = 3, - X_HEAD = 4, Y_HEAD = 5, Z_HEAD = 6, - E0_AXIS = 3, - E1_AXIS, E2_AXIS, E3_AXIS, E4_AXIS, E5_AXIS, E6_AXIS, E7_AXIS, - ALL_AXES = 0xFE, NO_AXIS = 0xFF + + // Linear axes may be controlled directly or indirectly + LINEAR_AXIS_LIST(X_AXIS, Y_AXIS, Z_AXIS, I_AXIS, J_AXIS, K_AXIS) + + // Extruder axes may be considered distinctly + #define _EN_ITEM(N) , E##N##_AXIS + REPEAT(EXTRUDERS, _EN_ITEM) + #undef _EN_ITEM + + // Core also keeps toolhead directions + #if ANY(IS_CORE, MARKFORGED_XY, MARKFORGED_YX) + , X_HEAD, Y_HEAD, Z_HEAD + #endif + + // Distinct axes, including all E and Core + , NUM_AXIS_ENUMS + + // Most of the time we refer only to the single E_AXIS + #if HAS_EXTRUDERS + , E_AXIS = E0_AXIS + #endif + + // A, B, and C are for DELTA, SCARA, etc. + , A_AXIS = X_AXIS + #if HAS_Y_AXIS + , B_AXIS = Y_AXIS + #endif + #if HAS_Z_AXIS + , C_AXIS = Z_AXIS + #endif + + // To refer to all or none + , ALL_AXES_ENUM = 0xFE, NO_AXIS_ENUM = 0xFF }; +typedef IF<(NUM_AXIS_ENUMS > 8), uint16_t, uint8_t>::type axis_bits_t; + // -// Loop over XYZE axes +// Loop over axes // -#define LOOP_XYZ(VAR) LOOP_S_LE_N(VAR, X_AXIS, Z_AXIS) -#define LOOP_XYZE(VAR) LOOP_S_LE_N(VAR, X_AXIS, E_AXIS) -#define LOOP_XYZE_N(VAR) LOOP_S_L_N(VAR, X_AXIS, XYZE_N) #define LOOP_ABC(VAR) LOOP_S_LE_N(VAR, A_AXIS, C_AXIS) -#define LOOP_ABCE(VAR) LOOP_S_LE_N(VAR, A_AXIS, E_AXIS) -#define LOOP_ABCE_N(VAR) LOOP_S_L_N(VAR, A_AXIS, XYZE_N) +#define LOOP_LINEAR_AXES(VAR) LOOP_S_L_N(VAR, X_AXIS, LINEAR_AXES) +#define LOOP_LOGICAL_AXES(VAR) LOOP_S_L_N(VAR, X_AXIS, LOGICAL_AXES) +#define LOOP_DISTINCT_AXES(VAR) LOOP_S_L_N(VAR, X_AXIS, DISTINCT_AXES) // -// Conditional type assignment magic. For example... +// feedRate_t is just a humble float // -// typename IF<(MYOPT==12), int, float>::type myvar; +typedef float feedRate_t; + // -template -struct IF { typedef R type; }; -template -struct IF { typedef L type; }; +// celsius_t is the native unit of temperature. Signed to handle a disconnected thermistor value (-14). +// For more resolition (e.g., for a chocolate printer) this may later be changed to Celsius x 100 +// +typedef int16_t celsius_t; +typedef float celsius_float_t; // -// feedRate_t is just a humble float +// On AVR pointers are only 2 bytes so use 'const float &' for 'const float' // -typedef float feedRate_t; +#ifdef __AVR__ + typedef const float & const_float_t; +#else + typedef const float const_float_t; +#endif +typedef const_float_t const_feedRate_t; +typedef const_float_t const_celsius_float_t; // Conversion macros -#define MMM_TO_MMS(MM_M) feedRate_t(float(MM_M) / 60.0f) -#define MMS_TO_MMM(MM_S) (float(MM_S) * 60.0f) +#define MMM_TO_MMS(MM_M) feedRate_t(static_cast(MM_M) / 60.0f) +#define MMS_TO_MMM(MM_S) (static_cast(MM_S) * 60.0f) // // Coordinates structures for XY, XYZ, XYZE... // // Helpers -#define _RECIP(N) ((N) ? 1.0f / float(N) : 0.0f) +#define _RECIP(N) ((N) ? 1.0f / static_cast(N) : 0.0f) #define _ABS(N) ((N) < 0 ? -(N) : (N)) #define _LS(N) (N = (T)(uint32_t(N) << v)) #define _RS(N) (N = (T)(uint32_t(N) >> v)) @@ -169,7 +239,7 @@ void toNative(xyz_pos_t &raw); void toNative(xyze_pos_t &raw); // -// XY coordinates, counters, etc. +// Paired XY coordinates, counters, flags, etc. // template struct XYval { @@ -178,18 +248,34 @@ struct XYval { struct { T a, b; }; T pos[2]; }; + + // Set all to 0 + FI void reset() { x = y = 0; } + + // Setters taking struct types and arrays FI void set(const T px) { x = px; } - FI void set(const T px, const T py) { x = px; y = py; } - FI void set(const T (&arr)[XY]) { x = arr[0]; y = arr[1]; } - FI void set(const T (&arr)[XYZ]) { x = arr[0]; y = arr[1]; } - FI void set(const T (&arr)[XYZE]) { x = arr[0]; y = arr[1]; } - #if XYZE_N > XYZE - FI void set(const T (&arr)[XYZE_N]) { x = arr[0]; y = arr[1]; } + #if HAS_Y_AXIS + FI void set(const T px, const T py) { x = px; y = py; } + FI void set(const T (&arr)[XY]) { x = arr[0]; y = arr[1]; } #endif - FI void reset() { x = y = 0; } + #if LINEAR_AXES > XY + FI void set(const T (&arr)[LINEAR_AXES]) { x = arr[0]; y = arr[1]; } + #endif + #if LOGICAL_AXES > LINEAR_AXES + FI void set(const T (&arr)[LOGICAL_AXES]) { x = arr[0]; y = arr[1]; } + #if DISTINCT_AXES > LOGICAL_AXES + FI void set(const T (&arr)[DISTINCT_AXES]) { x = arr[0]; y = arr[1]; } + #endif + #endif + + // Length reduced to one dimension FI T magnitude() const { return (T)sqrtf(x*x + y*y); } + // Pointer to the data as a simple array FI operator T* () { return pos; } + // If any element is true then it's true FI operator bool() { return x || y; } + + // Explicit copy and copies with conversion FI XYval copy() const { return *this; } FI XYval ABS() const { return { T(_ABS(x)), T(_ABS(y)) }; } FI XYval asInt() { return { int16_t(x), int16_t(y) }; } @@ -198,20 +284,30 @@ struct XYval { FI XYval asLong() const { return { int32_t(x), int32_t(y) }; } FI XYval ROUNDL() { return { int32_t(LROUND(x)), int32_t(LROUND(y)) }; } FI XYval ROUNDL() const { return { int32_t(LROUND(x)), int32_t(LROUND(y)) }; } - FI XYval asFloat() { return { float(x), float(y) }; } - FI XYval asFloat() const { return { float(x), float(y) }; } + FI XYval asFloat() { return { static_cast(x), static_cast(y) }; } + FI XYval asFloat() const { return { static_cast(x), static_cast(y) }; } FI XYval reciprocal() const { return { _RECIP(x), _RECIP(y) }; } + + // Marlin workspace shifting is done with G92 and M206 FI XYval asLogical() const { XYval o = asFloat(); toLogical(o); return o; } FI XYval asNative() const { XYval o = asFloat(); toNative(o); return o; } + + // Cast to a type with more fields by making a new object FI operator XYZval() { return { x, y }; } FI operator XYZval() const { return { x, y }; } FI operator XYZEval() { return { x, y }; } FI operator XYZEval() const { return { x, y }; } - FI T& operator[](const int i) { return pos[i]; } - FI const T& operator[](const int i) const { return pos[i]; } + + // Accessor via an AxisEnum (or any integer) [index] + FI T& operator[](const int n) { return pos[n]; } + FI const T& operator[](const int n) const { return pos[n]; } + + // Assignment operator overrides do the expected thing FI XYval& operator= (const T v) { set(v, v ); return *this; } FI XYval& operator= (const XYZval &rs) { set(rs.x, rs.y); return *this; } FI XYval& operator= (const XYZEval &rs) { set(rs.x, rs.y); return *this; } + + // Override other operators to get intuitive behaviors FI XYval operator+ (const XYval &rs) const { XYval ls = *this; ls.x += rs.x; ls.y += rs.y; return ls; } FI XYval operator+ (const XYval &rs) { XYval ls = *this; ls.x += rs.x; ls.y += rs.y; return ls; } FI XYval operator- (const XYval &rs) const { XYval ls = *this; ls.x -= rs.x; ls.y -= rs.y; return ls; } @@ -248,6 +344,10 @@ struct XYval { FI XYval operator>>(const int &v) { XYval ls = *this; _RS(ls.x); _RS(ls.y); return ls; } FI XYval operator<<(const int &v) const { XYval ls = *this; _LS(ls.x); _LS(ls.y); return ls; } FI XYval operator<<(const int &v) { XYval ls = *this; _LS(ls.x); _LS(ls.y); return ls; } + FI const XYval operator-() const { XYval o = *this; o.x = -x; o.y = -y; return o; } + FI XYval operator-() { XYval o = *this; o.x = -x; o.y = -y; return o; } + + // Modifier operators FI XYval& operator+=(const XYval &rs) { x += rs.x; y += rs.y; return *this; } FI XYval& operator-=(const XYval &rs) { x -= rs.x; y -= rs.y; return *this; } FI XYval& operator*=(const XYval &rs) { x *= rs.x; y *= rs.y; return *this; } @@ -261,6 +361,8 @@ struct XYval { FI XYval& operator*=(const int &v) { x *= v; y *= v; return *this; } FI XYval& operator>>=(const int &v) { _RS(x); _RS(y); return *this; } FI XYval& operator<<=(const int &v) { _LS(x); _LS(y); return *this; } + + // Exact comparisons. For floats a "NEAR" operation may be better. FI bool operator==(const XYval &rs) { return x == rs.x && y == rs.y; } FI bool operator==(const XYZval &rs) { return x == rs.x && y == rs.y; } FI bool operator==(const XYZEval &rs) { return x == rs.x && y == rs.y; } @@ -273,224 +375,293 @@ struct XYval { FI bool operator!=(const XYval &rs) const { return !operator==(rs); } FI bool operator!=(const XYZval &rs) const { return !operator==(rs); } FI bool operator!=(const XYZEval &rs) const { return !operator==(rs); } - FI XYval operator-() { XYval o = *this; o.x = -x; o.y = -y; return o; } - FI const XYval operator-() const { XYval o = *this; o.x = -x; o.y = -y; return o; } }; // -// XYZ coordinates, counters, etc. +// Linear Axes coordinates, counters, flags, etc. // template struct XYZval { union { - struct { T x, y, z; }; - struct { T a, b, c; }; - T pos[3]; + struct { T LINEAR_AXIS_ARGS(); }; + struct { T LINEAR_AXIS_LIST(a, b, c, u, v, w); }; + T pos[LINEAR_AXES]; }; + + // Set all to 0 + FI void reset() { LINEAR_AXIS_GANG(x =, y =, z =, i =, j =, k =) 0; } + + // Setters taking struct types and arrays FI void set(const T px) { x = px; } FI void set(const T px, const T py) { x = px; y = py; } - FI void set(const T px, const T py, const T pz) { x = px; y = py; z = pz; } - FI void set(const XYval pxy, const T pz) { x = pxy.x; y = pxy.y; z = pz; } + FI void set(const XYval pxy) { x = pxy.x; y = pxy.y; } + FI void set(const XYval pxy, const T pz) { LINEAR_AXIS_CODE(x = pxy.x, y = pxy.y, z = pz, NOOP, NOOP, NOOP); } FI void set(const T (&arr)[XY]) { x = arr[0]; y = arr[1]; } - FI void set(const T (&arr)[XYZ]) { x = arr[0]; y = arr[1]; z = arr[2]; } - FI void set(const T (&arr)[XYZE]) { x = arr[0]; y = arr[1]; z = arr[2]; } - #if XYZE_N > XYZE - FI void set(const T (&arr)[XYZE_N]) { x = arr[0]; y = arr[1]; z = arr[2]; } + #if HAS_Z_AXIS + FI void set(const T (&arr)[LINEAR_AXES]) { LINEAR_AXIS_CODE(x = arr[0], y = arr[1], z = arr[2], i = arr[3], j = arr[4], k = arr[5]); } + FI void set(LINEAR_AXIS_ARGS(const T)) { LINEAR_AXIS_CODE(a = x, b = y, c = z, u = i, v = j, w = k ); } + #endif + #if LOGICAL_AXES > LINEAR_AXES + FI void set(const T (&arr)[LOGICAL_AXES]) { LINEAR_AXIS_CODE(x = arr[0], y = arr[1], z = arr[2], i = arr[3], j = arr[4], k = arr[5]); } + FI void set(LOGICAL_AXIS_ARGS(const T)) { LINEAR_AXIS_CODE(a = x, b = y, c = z, u = i, v = j, w = k ); } + #if DISTINCT_AXES > LOGICAL_AXES + FI void set(const T (&arr)[DISTINCT_AXES]) { LINEAR_AXIS_CODE(x = arr[0], y = arr[1], z = arr[2], i = arr[3], j = arr[4], k = arr[5]); } + #endif + #endif + #if HAS_I_AXIS + FI void set(const T px, const T py, const T pz) { x = px; y = py; z = pz; } + #endif + #if HAS_J_AXIS + FI void set(const T px, const T py, const T pz, const T pi) { x = px; y = py; z = pz; i = pi; } + #endif + #if HAS_K_AXIS + FI void set(const T px, const T py, const T pz, const T pi, const T pj) { x = px; y = py; z = pz; i = pi; j = pj; } #endif - FI void reset() { x = y = z = 0; } - FI T magnitude() const { return (T)sqrtf(x*x + y*y + z*z); } + + // Length reduced to one dimension + FI T magnitude() const { return (T)sqrtf(LINEAR_AXIS_GANG(x*x, + y*y, + z*z, + i*i, + j*j, + k*k)); } + // Pointer to the data as a simple array FI operator T* () { return pos; } - FI operator bool() { return z || x || y; } + // If any element is true then it's true + FI operator bool() { return LINEAR_AXIS_GANG(x, || y, || z, || i, || j, || k); } + + // Explicit copy and copies with conversion FI XYZval copy() const { XYZval o = *this; return o; } - FI XYZval ABS() const { return { T(_ABS(x)), T(_ABS(y)), T(_ABS(z)) }; } - FI XYZval asInt() { return { int16_t(x), int16_t(y), int16_t(z) }; } - FI XYZval asInt() const { return { int16_t(x), int16_t(y), int16_t(z) }; } - FI XYZval asLong() { return { int32_t(x), int32_t(y), int32_t(z) }; } - FI XYZval asLong() const { return { int32_t(x), int32_t(y), int32_t(z) }; } - FI XYZval ROUNDL() { return { int32_t(LROUND(x)), int32_t(LROUND(y)), int32_t(LROUND(z)) }; } - FI XYZval ROUNDL() const { return { int32_t(LROUND(x)), int32_t(LROUND(y)), int32_t(LROUND(z)) }; } - FI XYZval asFloat() { return { float(x), float(y), float(z) }; } - FI XYZval asFloat() const { return { float(x), float(y), float(z) }; } - FI XYZval reciprocal() const { return { _RECIP(x), _RECIP(y), _RECIP(z) }; } + FI XYZval ABS() const { return LINEAR_AXIS_ARRAY(T(_ABS(x)), T(_ABS(y)), T(_ABS(z)), T(_ABS(i)), T(_ABS(j)), T(_ABS(k))); } + FI XYZval asInt() { return LINEAR_AXIS_ARRAY(int16_t(x), int16_t(y), int16_t(z), int16_t(i), int16_t(j), int16_t(k)); } + FI XYZval asInt() const { return LINEAR_AXIS_ARRAY(int16_t(x), int16_t(y), int16_t(z), int16_t(i), int16_t(j), int16_t(k)); } + FI XYZval asLong() { return LINEAR_AXIS_ARRAY(int32_t(x), int32_t(y), int32_t(z), int32_t(i), int32_t(j), int32_t(k)); } + FI XYZval asLong() const { return LINEAR_AXIS_ARRAY(int32_t(x), int32_t(y), int32_t(z), int32_t(i), int32_t(j), int32_t(k)); } + FI XYZval ROUNDL() { return LINEAR_AXIS_ARRAY(int32_t(LROUND(x)), int32_t(LROUND(y)), int32_t(LROUND(z)), int32_t(LROUND(i)), int32_t(LROUND(j)), int32_t(LROUND(k))); } + FI XYZval ROUNDL() const { return LINEAR_AXIS_ARRAY(int32_t(LROUND(x)), int32_t(LROUND(y)), int32_t(LROUND(z)), int32_t(LROUND(i)), int32_t(LROUND(j)), int32_t(LROUND(k))); } + FI XYZval asFloat() { return LINEAR_AXIS_ARRAY(static_cast(x), static_cast(y), static_cast(z), static_cast(i), static_cast(j), static_cast(k)); } + FI XYZval asFloat() const { return LINEAR_AXIS_ARRAY(static_cast(x), static_cast(y), static_cast(z), static_cast(i), static_cast(j), static_cast(k)); } + FI XYZval reciprocal() const { return LINEAR_AXIS_ARRAY(_RECIP(x), _RECIP(y), _RECIP(z), _RECIP(i), _RECIP(j), _RECIP(k)); } + + // Marlin workspace shifting is done with G92 and M206 FI XYZval asLogical() const { XYZval o = asFloat(); toLogical(o); return o; } FI XYZval asNative() const { XYZval o = asFloat(); toNative(o); return o; } + + // In-place cast to types having fewer fields FI operator XYval&() { return *(XYval*)this; } FI operator const XYval&() const { return *(const XYval*)this; } - FI operator XYZEval() const { return { x, y, z }; } - FI T& operator[](const int i) { return pos[i]; } - FI const T& operator[](const int i) const { return pos[i]; } - FI XYZval& operator= (const T v) { set(v, v, v ); return *this; } + + // Cast to a type with more fields by making a new object + FI operator XYZEval() const { return LINEAR_AXIS_ARRAY(x, y, z, i, j, k); } + + // Accessor via an AxisEnum (or any integer) [index] + FI T& operator[](const int n) { return pos[n]; } + FI const T& operator[](const int n) const { return pos[n]; } + + // Assignment operator overrides do the expected thing + FI XYZval& operator= (const T v) { set(ARRAY_N_1(LINEAR_AXES, v)); return *this; } FI XYZval& operator= (const XYval &rs) { set(rs.x, rs.y ); return *this; } - FI XYZval& operator= (const XYZEval &rs) { set(rs.x, rs.y, rs.z); return *this; } - FI XYZval operator+ (const XYval &rs) const { XYZval ls = *this; ls.x += rs.x; ls.y += rs.y; return ls; } - FI XYZval operator+ (const XYval &rs) { XYZval ls = *this; ls.x += rs.x; ls.y += rs.y; return ls; } - FI XYZval operator- (const XYval &rs) const { XYZval ls = *this; ls.x -= rs.x; ls.y -= rs.y; return ls; } - FI XYZval operator- (const XYval &rs) { XYZval ls = *this; ls.x -= rs.x; ls.y -= rs.y; return ls; } - FI XYZval operator* (const XYval &rs) const { XYZval ls = *this; ls.x *= rs.x; ls.y *= rs.y; return ls; } - FI XYZval operator* (const XYval &rs) { XYZval ls = *this; ls.x *= rs.x; ls.y *= rs.y; return ls; } - FI XYZval operator/ (const XYval &rs) const { XYZval ls = *this; ls.x /= rs.x; ls.y /= rs.y; return ls; } - FI XYZval operator/ (const XYval &rs) { XYZval ls = *this; ls.x /= rs.x; ls.y /= rs.y; return ls; } - FI XYZval operator+ (const XYZval &rs) const { XYZval ls = *this; ls.x += rs.x; ls.y += rs.y; ls.z += rs.z; return ls; } - FI XYZval operator+ (const XYZval &rs) { XYZval ls = *this; ls.x += rs.x; ls.y += rs.y; ls.z += rs.z; return ls; } - FI XYZval operator- (const XYZval &rs) const { XYZval ls = *this; ls.x -= rs.x; ls.y -= rs.y; ls.z -= rs.z; return ls; } - FI XYZval operator- (const XYZval &rs) { XYZval ls = *this; ls.x -= rs.x; ls.y -= rs.y; ls.z -= rs.z; return ls; } - FI XYZval operator* (const XYZval &rs) const { XYZval ls = *this; ls.x *= rs.x; ls.y *= rs.y; ls.z *= rs.z; return ls; } - FI XYZval operator* (const XYZval &rs) { XYZval ls = *this; ls.x *= rs.x; ls.y *= rs.y; ls.z *= rs.z; return ls; } - FI XYZval operator/ (const XYZval &rs) const { XYZval ls = *this; ls.x /= rs.x; ls.y /= rs.y; ls.z /= rs.z; return ls; } - FI XYZval operator/ (const XYZval &rs) { XYZval ls = *this; ls.x /= rs.x; ls.y /= rs.y; ls.z /= rs.z; return ls; } - FI XYZval operator+ (const XYZEval &rs) const { XYZval ls = *this; ls.x += rs.x; ls.y += rs.y; ls.z += rs.z; return ls; } - FI XYZval operator+ (const XYZEval &rs) { XYZval ls = *this; ls.x += rs.x; ls.y += rs.y; ls.z += rs.z; return ls; } - FI XYZval operator- (const XYZEval &rs) const { XYZval ls = *this; ls.x -= rs.x; ls.y -= rs.y; ls.z -= rs.z; return ls; } - FI XYZval operator- (const XYZEval &rs) { XYZval ls = *this; ls.x -= rs.x; ls.y -= rs.y; ls.z -= rs.z; return ls; } - FI XYZval operator* (const XYZEval &rs) const { XYZval ls = *this; ls.x *= rs.x; ls.y *= rs.y; ls.z *= rs.z; return ls; } - FI XYZval operator* (const XYZEval &rs) { XYZval ls = *this; ls.x *= rs.x; ls.y *= rs.y; ls.z *= rs.z; return ls; } - FI XYZval operator/ (const XYZEval &rs) const { XYZval ls = *this; ls.x /= rs.x; ls.y /= rs.y; ls.z /= rs.z; return ls; } - FI XYZval operator/ (const XYZEval &rs) { XYZval ls = *this; ls.x /= rs.x; ls.y /= rs.y; ls.z /= rs.z; return ls; } - FI XYZval operator* (const float &v) const { XYZval ls = *this; ls.x *= v; ls.y *= v; ls.z *= v; return ls; } - FI XYZval operator* (const float &v) { XYZval ls = *this; ls.x *= v; ls.y *= v; ls.z *= v; return ls; } - FI XYZval operator* (const int &v) const { XYZval ls = *this; ls.x *= v; ls.y *= v; ls.z *= v; return ls; } - FI XYZval operator* (const int &v) { XYZval ls = *this; ls.x *= v; ls.y *= v; ls.z *= v; return ls; } - FI XYZval operator/ (const float &v) const { XYZval ls = *this; ls.x /= v; ls.y /= v; ls.z /= v; return ls; } - FI XYZval operator/ (const float &v) { XYZval ls = *this; ls.x /= v; ls.y /= v; ls.z /= v; return ls; } - FI XYZval operator/ (const int &v) const { XYZval ls = *this; ls.x /= v; ls.y /= v; ls.z /= v; return ls; } - FI XYZval operator/ (const int &v) { XYZval ls = *this; ls.x /= v; ls.y /= v; ls.z /= v; return ls; } - FI XYZval operator>>(const int &v) const { XYZval ls = *this; _RS(ls.x); _RS(ls.y); _RS(ls.z); return ls; } - FI XYZval operator>>(const int &v) { XYZval ls = *this; _RS(ls.x); _RS(ls.y); _RS(ls.z); return ls; } - FI XYZval operator<<(const int &v) const { XYZval ls = *this; _LS(ls.x); _LS(ls.y); _LS(ls.z); return ls; } - FI XYZval operator<<(const int &v) { XYZval ls = *this; _LS(ls.x); _LS(ls.y); _LS(ls.z); return ls; } - FI XYZval& operator+=(const XYval &rs) { x += rs.x; y += rs.y; return *this; } - FI XYZval& operator-=(const XYval &rs) { x -= rs.x; y -= rs.y; return *this; } - FI XYZval& operator*=(const XYval &rs) { x *= rs.x; y *= rs.y; return *this; } - FI XYZval& operator/=(const XYval &rs) { x /= rs.x; y /= rs.y; return *this; } - FI XYZval& operator+=(const XYZval &rs) { x += rs.x; y += rs.y; z += rs.z; return *this; } - FI XYZval& operator-=(const XYZval &rs) { x -= rs.x; y -= rs.y; z -= rs.z; return *this; } - FI XYZval& operator*=(const XYZval &rs) { x *= rs.x; y *= rs.y; z *= rs.z; return *this; } - FI XYZval& operator/=(const XYZval &rs) { x /= rs.x; y /= rs.y; z /= rs.z; return *this; } - FI XYZval& operator+=(const XYZEval &rs) { x += rs.x; y += rs.y; z += rs.z; return *this; } - FI XYZval& operator-=(const XYZEval &rs) { x -= rs.x; y -= rs.y; z -= rs.z; return *this; } - FI XYZval& operator*=(const XYZEval &rs) { x *= rs.x; y *= rs.y; z *= rs.z; return *this; } - FI XYZval& operator/=(const XYZEval &rs) { x /= rs.x; y /= rs.y; z /= rs.z; return *this; } - FI XYZval& operator*=(const float &v) { x *= v; y *= v; z *= v; return *this; } - FI XYZval& operator*=(const int &v) { x *= v; y *= v; z *= v; return *this; } - FI XYZval& operator>>=(const int &v) { _RS(x); _RS(y); _RS(z); return *this; } - FI XYZval& operator<<=(const int &v) { _LS(x); _LS(y); _LS(z); return *this; } - FI bool operator==(const XYZEval &rs) { return x == rs.x && y == rs.y && z == rs.z; } + FI XYZval& operator= (const XYZEval &rs) { set(LINEAR_AXIS_ELEM(rs)); return *this; } + + // Override other operators to get intuitive behaviors + FI XYZval operator+ (const XYval &rs) const { XYZval ls = *this; LINEAR_AXIS_CODE(ls.x += rs.x, ls.y += rs.y, NOOP , NOOP , NOOP , NOOP ); return ls; } + FI XYZval operator+ (const XYval &rs) { XYZval ls = *this; LINEAR_AXIS_CODE(ls.x += rs.x, ls.y += rs.y, NOOP , NOOP , NOOP , NOOP ); return ls; } + FI XYZval operator- (const XYval &rs) const { XYZval ls = *this; LINEAR_AXIS_CODE(ls.x -= rs.x, ls.y -= rs.y, NOOP , NOOP , NOOP , NOOP ); return ls; } + FI XYZval operator- (const XYval &rs) { XYZval ls = *this; LINEAR_AXIS_CODE(ls.x -= rs.x, ls.y -= rs.y, NOOP , NOOP , NOOP , NOOP ); return ls; } + FI XYZval operator* (const XYval &rs) const { XYZval ls = *this; LINEAR_AXIS_CODE(ls.x *= rs.x, ls.y *= rs.y, NOOP , NOOP , NOOP , NOOP ); return ls; } + FI XYZval operator* (const XYval &rs) { XYZval ls = *this; LINEAR_AXIS_CODE(ls.x *= rs.x, ls.y *= rs.y, NOOP , NOOP , NOOP , NOOP ); return ls; } + FI XYZval operator/ (const XYval &rs) const { XYZval ls = *this; LINEAR_AXIS_CODE(ls.x /= rs.x, ls.y /= rs.y, NOOP , NOOP , NOOP , NOOP ); return ls; } + FI XYZval operator/ (const XYval &rs) { XYZval ls = *this; LINEAR_AXIS_CODE(ls.x /= rs.x, ls.y /= rs.y, NOOP , NOOP , NOOP , NOOP ); return ls; } + FI XYZval operator+ (const XYZval &rs) const { XYZval ls = *this; LINEAR_AXIS_CODE(ls.x += rs.x, ls.y += rs.y, ls.z += rs.z, ls.i += rs.i, ls.j += rs.j, ls.k += rs.k); return ls; } + FI XYZval operator+ (const XYZval &rs) { XYZval ls = *this; LINEAR_AXIS_CODE(ls.x += rs.x, ls.y += rs.y, ls.z += rs.z, ls.i += rs.i, ls.j += rs.j, ls.k += rs.k); return ls; } + FI XYZval operator- (const XYZval &rs) const { XYZval ls = *this; LINEAR_AXIS_CODE(ls.x -= rs.x, ls.y -= rs.y, ls.z -= rs.z, ls.i -= rs.i, ls.j -= rs.j, ls.k -= rs.k); return ls; } + FI XYZval operator- (const XYZval &rs) { XYZval ls = *this; LINEAR_AXIS_CODE(ls.x -= rs.x, ls.y -= rs.y, ls.z -= rs.z, ls.i -= rs.i, ls.j -= rs.j, ls.k -= rs.k); return ls; } + FI XYZval operator* (const XYZval &rs) const { XYZval ls = *this; LINEAR_AXIS_CODE(ls.x *= rs.x, ls.y *= rs.y, ls.z *= rs.z, ls.i *= rs.i, ls.j *= rs.j, ls.k *= rs.k); return ls; } + FI XYZval operator* (const XYZval &rs) { XYZval ls = *this; LINEAR_AXIS_CODE(ls.x *= rs.x, ls.y *= rs.y, ls.z *= rs.z, ls.i *= rs.i, ls.j *= rs.j, ls.k *= rs.k); return ls; } + FI XYZval operator/ (const XYZval &rs) const { XYZval ls = *this; LINEAR_AXIS_CODE(ls.x /= rs.x, ls.y /= rs.y, ls.z /= rs.z, ls.i /= rs.i, ls.j /= rs.j, ls.k /= rs.k); return ls; } + FI XYZval operator/ (const XYZval &rs) { XYZval ls = *this; LINEAR_AXIS_CODE(ls.x /= rs.x, ls.y /= rs.y, ls.z /= rs.z, ls.i /= rs.i, ls.j /= rs.j, ls.k /= rs.k); return ls; } + FI XYZval operator+ (const XYZEval &rs) const { XYZval ls = *this; LINEAR_AXIS_CODE(ls.x += rs.x, ls.y += rs.y, ls.z += rs.z, ls.i += rs.i, ls.j += rs.j, ls.k += rs.k); return ls; } + FI XYZval operator+ (const XYZEval &rs) { XYZval ls = *this; LINEAR_AXIS_CODE(ls.x += rs.x, ls.y += rs.y, ls.z += rs.z, ls.i += rs.i, ls.j += rs.j, ls.k += rs.k); return ls; } + FI XYZval operator- (const XYZEval &rs) const { XYZval ls = *this; LINEAR_AXIS_CODE(ls.x -= rs.x, ls.y -= rs.y, ls.z -= rs.z, ls.i -= rs.i, ls.j -= rs.j, ls.k -= rs.k); return ls; } + FI XYZval operator- (const XYZEval &rs) { XYZval ls = *this; LINEAR_AXIS_CODE(ls.x -= rs.x, ls.y -= rs.y, ls.z -= rs.z, ls.i -= rs.i, ls.j -= rs.j, ls.k -= rs.k); return ls; } + FI XYZval operator* (const XYZEval &rs) const { XYZval ls = *this; LINEAR_AXIS_CODE(ls.x *= rs.x, ls.y *= rs.y, ls.z *= rs.z, ls.i *= rs.i, ls.j *= rs.j, ls.k *= rs.k); return ls; } + FI XYZval operator* (const XYZEval &rs) { XYZval ls = *this; LINEAR_AXIS_CODE(ls.x *= rs.x, ls.y *= rs.y, ls.z *= rs.z, ls.i *= rs.i, ls.j *= rs.j, ls.k *= rs.k); return ls; } + FI XYZval operator/ (const XYZEval &rs) const { XYZval ls = *this; LINEAR_AXIS_CODE(ls.x /= rs.x, ls.y /= rs.y, ls.z /= rs.z, ls.i /= rs.i, ls.j /= rs.j, ls.k /= rs.k); return ls; } + FI XYZval operator/ (const XYZEval &rs) { XYZval ls = *this; LINEAR_AXIS_CODE(ls.x /= rs.x, ls.y /= rs.y, ls.z /= rs.z, ls.i /= rs.i, ls.j /= rs.j, ls.k /= rs.k); return ls; } + FI XYZval operator* (const float &v) const { XYZval ls = *this; LINEAR_AXIS_CODE(ls.x *= v, ls.y *= v, ls.z *= v, ls.i *= v, ls.j *= v, ls.k *= v ); return ls; } + FI XYZval operator* (const float &v) { XYZval ls = *this; LINEAR_AXIS_CODE(ls.x *= v, ls.y *= v, ls.z *= v, ls.i *= v, ls.j *= v, ls.k *= v ); return ls; } + FI XYZval operator* (const int &v) const { XYZval ls = *this; LINEAR_AXIS_CODE(ls.x *= v, ls.y *= v, ls.z *= v, ls.i *= v, ls.j *= v, ls.k *= v ); return ls; } + FI XYZval operator* (const int &v) { XYZval ls = *this; LINEAR_AXIS_CODE(ls.x *= v, ls.y *= v, ls.z *= v, ls.i *= v, ls.j *= v, ls.k *= v ); return ls; } + FI XYZval operator/ (const float &v) const { XYZval ls = *this; LINEAR_AXIS_CODE(ls.x /= v, ls.y /= v, ls.z /= v, ls.i /= v, ls.j /= v, ls.k /= v ); return ls; } + FI XYZval operator/ (const float &v) { XYZval ls = *this; LINEAR_AXIS_CODE(ls.x /= v, ls.y /= v, ls.z /= v, ls.i /= v, ls.j /= v, ls.k /= v ); return ls; } + FI XYZval operator/ (const int &v) const { XYZval ls = *this; LINEAR_AXIS_CODE(ls.x /= v, ls.y /= v, ls.z /= v, ls.i /= v, ls.j /= v, ls.k /= v ); return ls; } + FI XYZval operator/ (const int &v) { XYZval ls = *this; LINEAR_AXIS_CODE(ls.x /= v, ls.y /= v, ls.z /= v, ls.i /= v, ls.j /= v, ls.k /= v ); return ls; } + FI XYZval operator>>(const int &v) const { XYZval ls = *this; LINEAR_AXIS_CODE(_RS(ls.x), _RS(ls.y), _RS(ls.z), _RS(ls.i), _RS(ls.j), _RS(ls.k) ); return ls; } + FI XYZval operator>>(const int &v) { XYZval ls = *this; LINEAR_AXIS_CODE(_RS(ls.x), _RS(ls.y), _RS(ls.z), _RS(ls.i), _RS(ls.j), _RS(ls.k) ); return ls; } + FI XYZval operator<<(const int &v) const { XYZval ls = *this; LINEAR_AXIS_CODE(_LS(ls.x), _LS(ls.y), _LS(ls.z), _LS(ls.i), _LS(ls.j), _LS(ls.k) ); return ls; } + FI XYZval operator<<(const int &v) { XYZval ls = *this; LINEAR_AXIS_CODE(_LS(ls.x), _LS(ls.y), _LS(ls.z), _LS(ls.i), _LS(ls.j), _LS(ls.k) ); return ls; } + FI const XYZval operator-() const { XYZval o = *this; LINEAR_AXIS_CODE(o.x = -x, o.y = -y, o.z = -z, o.i = -i, o.j = -j, o.k = -k); return o; } + FI XYZval operator-() { XYZval o = *this; LINEAR_AXIS_CODE(o.x = -x, o.y = -y, o.z = -z, o.i = -i, o.j = -j, o.k = -k); return o; } + + // Modifier operators + FI XYZval& operator+=(const XYval &rs) { LINEAR_AXIS_CODE(x += rs.x, y += rs.y, NOOP, NOOP, NOOP, NOOP ); return *this; } + FI XYZval& operator-=(const XYval &rs) { LINEAR_AXIS_CODE(x -= rs.x, y -= rs.y, NOOP, NOOP, NOOP, NOOP ); return *this; } + FI XYZval& operator*=(const XYval &rs) { LINEAR_AXIS_CODE(x *= rs.x, y *= rs.y, NOOP, NOOP, NOOP, NOOP ); return *this; } + FI XYZval& operator/=(const XYval &rs) { LINEAR_AXIS_CODE(x /= rs.x, y /= rs.y, NOOP, NOOP, NOOP, NOOP ); return *this; } + FI XYZval& operator+=(const XYZval &rs) { LINEAR_AXIS_CODE(x += rs.x, y += rs.y, z += rs.z, i += rs.i, j += rs.j, k += rs.k); return *this; } + FI XYZval& operator-=(const XYZval &rs) { LINEAR_AXIS_CODE(x -= rs.x, y -= rs.y, z -= rs.z, i -= rs.i, j -= rs.j, k -= rs.k); return *this; } + FI XYZval& operator*=(const XYZval &rs) { LINEAR_AXIS_CODE(x *= rs.x, y *= rs.y, z *= rs.z, i *= rs.i, j *= rs.j, k *= rs.k); return *this; } + FI XYZval& operator/=(const XYZval &rs) { LINEAR_AXIS_CODE(x /= rs.x, y /= rs.y, z /= rs.z, i /= rs.i, j /= rs.j, k /= rs.k); return *this; } + FI XYZval& operator+=(const XYZEval &rs) { LINEAR_AXIS_CODE(x += rs.x, y += rs.y, z += rs.z, i += rs.i, j += rs.j, k += rs.k); return *this; } + FI XYZval& operator-=(const XYZEval &rs) { LINEAR_AXIS_CODE(x -= rs.x, y -= rs.y, z -= rs.z, i -= rs.i, j -= rs.j, k -= rs.k); return *this; } + FI XYZval& operator*=(const XYZEval &rs) { LINEAR_AXIS_CODE(x *= rs.x, y *= rs.y, z *= rs.z, i *= rs.i, j *= rs.j, k *= rs.k); return *this; } + FI XYZval& operator/=(const XYZEval &rs) { LINEAR_AXIS_CODE(x /= rs.x, y /= rs.y, z /= rs.z, i /= rs.i, j /= rs.j, k /= rs.k); return *this; } + FI XYZval& operator*=(const float &v) { LINEAR_AXIS_CODE(x *= v, y *= v, z *= v, i *= v, j *= v, k *= v); return *this; } + FI XYZval& operator*=(const int &v) { LINEAR_AXIS_CODE(x *= v, y *= v, z *= v, i *= v, j *= v, k *= v); return *this; } + FI XYZval& operator>>=(const int &v) { LINEAR_AXIS_CODE(_RS(x), _RS(y), _RS(z), _RS(i), _RS(j), _RS(k)); return *this; } + FI XYZval& operator<<=(const int &v) { LINEAR_AXIS_CODE(_LS(x), _LS(y), _LS(z), _LS(i), _LS(j), _LS(k)); return *this; } + + // Exact comparisons. For floats a "NEAR" operation may be better. + FI bool operator==(const XYZEval &rs) { return true LINEAR_AXIS_GANG(&& x == rs.x, && y == rs.y, && z == rs.z, && i == rs.i, && j == rs.j, && k == rs.k); } + FI bool operator==(const XYZEval &rs) const { return true LINEAR_AXIS_GANG(&& x == rs.x, && y == rs.y, && z == rs.z, && i == rs.i, && j == rs.j, && k == rs.k); } FI bool operator!=(const XYZEval &rs) { return !operator==(rs); } - FI bool operator==(const XYZEval &rs) const { return x == rs.x && y == rs.y && z == rs.z; } FI bool operator!=(const XYZEval &rs) const { return !operator==(rs); } - FI XYZval operator-() { XYZval o = *this; o.x = -x; o.y = -y; o.z = -z; return o; } - FI const XYZval operator-() const { XYZval o = *this; o.x = -x; o.y = -y; o.z = -z; return o; } }; // -// XYZE coordinates, counters, etc. +// Logical Axes coordinates, counters, etc. // template struct XYZEval { union { - struct{ T x, y, z, e; }; - struct{ T a, b, c; }; - T pos[4]; + struct { T LOGICAL_AXIS_ARGS(); }; + struct { T LOGICAL_AXIS_LIST(_e, a, b, c, u, v, w); }; + T pos[LOGICAL_AXES]; }; - FI void reset() { x = y = z = e = 0; } - FI T magnitude() const { return (T)sqrtf(x*x + y*y + z*z + e*e); } - FI operator T* () { return pos; } - FI operator bool() { return e || z || x || y; } - FI void set(const T px) { x = px; } - FI void set(const T px, const T py) { x = px; y = py; } - FI void set(const T px, const T py, const T pz) { x = px; y = py; z = pz; } - FI void set(const T px, const T py, const T pz, const T pe) { x = px; y = py; z = pz; e = pe; } - FI void set(const XYval pxy) { x = pxy.x; y = pxy.y; } - FI void set(const XYval pxy, const T pz) { x = pxy.x; y = pxy.y; z = pz; } - FI void set(const XYZval pxyz) { x = pxyz.x; y = pxyz.y; z = pxyz.z; } - FI void set(const XYval pxy, const T pz, const T pe) { x = pxy.x; y = pxy.y; z = pz; e = pe; } - FI void set(const XYval pxy, const XYval pze) { x = pxy.x; y = pxy.y; z = pze.z; e = pze.e; } - FI void set(const XYZval pxyz, const T pe) { x = pxyz.x; y = pxyz.y; z = pxyz.z; e = pe; } - FI void set(const T (&arr)[XY]) { x = arr[0]; y = arr[1]; } - FI void set(const T (&arr)[XYZ]) { x = arr[0]; y = arr[1]; z = arr[2]; } - FI void set(const T (&arr)[XYZE]) { x = arr[0]; y = arr[1]; z = arr[2]; e = arr[3]; } - #if XYZE_N > XYZE - FI void set(const T (&arr)[XYZE_N]) { x = arr[0]; y = arr[1]; z = arr[2]; e = arr[3]; } + // Reset all to 0 + FI void reset() { LOGICAL_AXIS_GANG(e =, x =, y =, z =, i =, j =, k =) 0; } + + // Setters for some number of linear axes, not all + FI void set(const T px) { x = px; } + FI void set(const T px, const T py) { x = px; y = py; } + #if HAS_I_AXIS + FI void set(const T px, const T py, const T pz) { x = px; y = py; z = pz; } + #endif + #if HAS_J_AXIS + FI void set(const T px, const T py, const T pz, const T pi) { x = px; y = py; z = pz; i = pi; } + #endif + #if HAS_K_AXIS + FI void set(const T px, const T py, const T pz, const T pi, const T pj) { x = px; y = py; z = pz; i = pi; j = pj; } #endif - FI XYZEval copy() const { return *this; } - FI XYZEval ABS() const { return { T(_ABS(x)), T(_ABS(y)), T(_ABS(z)), T(_ABS(e)) }; } - FI XYZEval asInt() { return { int16_t(x), int16_t(y), int16_t(z), int16_t(e) }; } - FI XYZEval asInt() const { return { int16_t(x), int16_t(y), int16_t(z), int16_t(e) }; } - FI XYZEval asLong() { return { int32_t(x), int32_t(y), int32_t(z), int32_t(e) }; } - FI XYZEval asLong() const { return { int32_t(x), int32_t(y), int32_t(z), int32_t(e) }; } - FI XYZEval ROUNDL() { return { int32_t(LROUND(x)), int32_t(LROUND(y)), int32_t(LROUND(z)), int32_t(LROUND(e)) }; } - FI XYZEval ROUNDL() const { return { int32_t(LROUND(x)), int32_t(LROUND(y)), int32_t(LROUND(z)), int32_t(LROUND(e)) }; } - FI XYZEval asFloat() { return { float(x), float(y), float(z), float(e) }; } - FI XYZEval asFloat() const { return { float(x), float(y), float(z), float(e) }; } - FI XYZEval reciprocal() const { return { _RECIP(x), _RECIP(y), _RECIP(z), _RECIP(e) }; } - FI XYZEval asLogical() const { XYZEval o = asFloat(); toLogical(o); return o; } - FI XYZEval asNative() const { XYZEval o = asFloat(); toNative(o); return o; } - FI operator XYval&() { return *(XYval*)this; } - FI operator const XYval&() const { return *(const XYval*)this; } - FI operator XYZval&() { return *(XYZval*)this; } - FI operator const XYZval&() const { return *(const XYZval*)this; } - FI T& operator[](const int i) { return pos[i]; } - FI const T& operator[](const int i) const { return pos[i]; } - FI XYZEval& operator= (const T v) { set(v, v, v, v); return *this; } - FI XYZEval& operator= (const XYval &rs) { set(rs.x, rs.y); return *this; } - FI XYZEval& operator= (const XYZval &rs) { set(rs.x, rs.y, rs.z); return *this; } - FI XYZEval operator+ (const XYval &rs) const { XYZEval ls = *this; ls.x += rs.x; ls.y += rs.y; return ls; } - FI XYZEval operator+ (const XYval &rs) { XYZEval ls = *this; ls.x += rs.x; ls.y += rs.y; return ls; } - FI XYZEval operator- (const XYval &rs) const { XYZEval ls = *this; ls.x -= rs.x; ls.y -= rs.y; return ls; } - FI XYZEval operator- (const XYval &rs) { XYZEval ls = *this; ls.x -= rs.x; ls.y -= rs.y; return ls; } - FI XYZEval operator* (const XYval &rs) const { XYZEval ls = *this; ls.x *= rs.x; ls.y *= rs.y; return ls; } - FI XYZEval operator* (const XYval &rs) { XYZEval ls = *this; ls.x *= rs.x; ls.y *= rs.y; return ls; } - FI XYZEval operator/ (const XYval &rs) const { XYZEval ls = *this; ls.x /= rs.x; ls.y /= rs.y; return ls; } - FI XYZEval operator/ (const XYval &rs) { XYZEval ls = *this; ls.x /= rs.x; ls.y /= rs.y; return ls; } - FI XYZEval operator+ (const XYZval &rs) const { XYZEval ls = *this; ls.x += rs.x; ls.y += rs.y; ls.z += rs.z; return ls; } - FI XYZEval operator+ (const XYZval &rs) { XYZEval ls = *this; ls.x += rs.x; ls.y += rs.y; ls.z += rs.z; return ls; } - FI XYZEval operator- (const XYZval &rs) const { XYZEval ls = *this; ls.x -= rs.x; ls.y -= rs.y; ls.z -= rs.z; return ls; } - FI XYZEval operator- (const XYZval &rs) { XYZEval ls = *this; ls.x -= rs.x; ls.y -= rs.y; ls.z -= rs.z; return ls; } - FI XYZEval operator* (const XYZval &rs) const { XYZEval ls = *this; ls.x *= rs.x; ls.y *= rs.y; ls.z *= rs.z; return ls; } - FI XYZEval operator* (const XYZval &rs) { XYZEval ls = *this; ls.x *= rs.x; ls.y *= rs.y; ls.z *= rs.z; return ls; } - FI XYZEval operator/ (const XYZval &rs) const { XYZEval ls = *this; ls.x /= rs.x; ls.y /= rs.y; ls.z /= rs.z; return ls; } - FI XYZEval operator/ (const XYZval &rs) { XYZEval ls = *this; ls.x /= rs.x; ls.y /= rs.y; ls.z /= rs.z; return ls; } - FI XYZEval operator+ (const XYZEval &rs) const { XYZEval ls = *this; ls.x += rs.x; ls.y += rs.y; ls.z += rs.z; ls.e += rs.e; return ls; } - FI XYZEval operator+ (const XYZEval &rs) { XYZEval ls = *this; ls.x += rs.x; ls.y += rs.y; ls.z += rs.z; ls.e += rs.e; return ls; } - FI XYZEval operator- (const XYZEval &rs) const { XYZEval ls = *this; ls.x -= rs.x; ls.y -= rs.y; ls.z -= rs.z; ls.e -= rs.e; return ls; } - FI XYZEval operator- (const XYZEval &rs) { XYZEval ls = *this; ls.x -= rs.x; ls.y -= rs.y; ls.z -= rs.z; ls.e -= rs.e; return ls; } - FI XYZEval operator* (const XYZEval &rs) const { XYZEval ls = *this; ls.x *= rs.x; ls.y *= rs.y; ls.z *= rs.z; ls.e *= rs.e; return ls; } - FI XYZEval operator* (const XYZEval &rs) { XYZEval ls = *this; ls.x *= rs.x; ls.y *= rs.y; ls.z *= rs.z; ls.e *= rs.e; return ls; } - FI XYZEval operator/ (const XYZEval &rs) const { XYZEval ls = *this; ls.x /= rs.x; ls.y /= rs.y; ls.z /= rs.z; ls.e /= rs.e; return ls; } - FI XYZEval operator/ (const XYZEval &rs) { XYZEval ls = *this; ls.x /= rs.x; ls.y /= rs.y; ls.z /= rs.z; ls.e /= rs.e; return ls; } - FI XYZEval operator* (const float &v) const { XYZEval ls = *this; ls.x *= v; ls.y *= v; ls.z *= v; ls.e *= v; return ls; } - FI XYZEval operator* (const float &v) { XYZEval ls = *this; ls.x *= v; ls.y *= v; ls.z *= v; ls.e *= v; return ls; } - FI XYZEval operator* (const int &v) const { XYZEval ls = *this; ls.x *= v; ls.y *= v; ls.z *= v; ls.e *= v; return ls; } - FI XYZEval operator* (const int &v) { XYZEval ls = *this; ls.x *= v; ls.y *= v; ls.z *= v; ls.e *= v; return ls; } - FI XYZEval operator/ (const float &v) const { XYZEval ls = *this; ls.x /= v; ls.y /= v; ls.z /= v; ls.e /= v; return ls; } - FI XYZEval operator/ (const float &v) { XYZEval ls = *this; ls.x /= v; ls.y /= v; ls.z /= v; ls.e /= v; return ls; } - FI XYZEval operator/ (const int &v) const { XYZEval ls = *this; ls.x /= v; ls.y /= v; ls.z /= v; ls.e /= v; return ls; } - FI XYZEval operator/ (const int &v) { XYZEval ls = *this; ls.x /= v; ls.y /= v; ls.z /= v; ls.e /= v; return ls; } - FI XYZEval operator>>(const int &v) const { XYZEval ls = *this; _RS(ls.x); _RS(ls.y); _RS(ls.z); _RS(ls.e); return ls; } - FI XYZEval operator>>(const int &v) { XYZEval ls = *this; _RS(ls.x); _RS(ls.y); _RS(ls.z); _RS(ls.e); return ls; } - FI XYZEval operator<<(const int &v) const { XYZEval ls = *this; _LS(ls.x); _LS(ls.y); _LS(ls.z); _LS(ls.e); return ls; } - FI XYZEval operator<<(const int &v) { XYZEval ls = *this; _LS(ls.x); _LS(ls.y); _LS(ls.z); _LS(ls.e); return ls; } - FI XYZEval& operator+=(const XYval &rs) { x += rs.x; y += rs.y; return *this; } - FI XYZEval& operator-=(const XYval &rs) { x -= rs.x; y -= rs.y; return *this; } - FI XYZEval& operator*=(const XYval &rs) { x *= rs.x; y *= rs.y; return *this; } - FI XYZEval& operator/=(const XYval &rs) { x /= rs.x; y /= rs.y; return *this; } - FI XYZEval& operator+=(const XYZval &rs) { x += rs.x; y += rs.y; z += rs.z; return *this; } - FI XYZEval& operator-=(const XYZval &rs) { x -= rs.x; y -= rs.y; z -= rs.z; return *this; } - FI XYZEval& operator*=(const XYZval &rs) { x *= rs.x; y *= rs.y; z *= rs.z; return *this; } - FI XYZEval& operator/=(const XYZval &rs) { x /= rs.x; y /= rs.y; z /= rs.z; return *this; } - FI XYZEval& operator+=(const XYZEval &rs) { x += rs.x; y += rs.y; z += rs.z; e += rs.e; return *this; } - FI XYZEval& operator-=(const XYZEval &rs) { x -= rs.x; y -= rs.y; z -= rs.z; e -= rs.e; return *this; } - FI XYZEval& operator*=(const XYZEval &rs) { x *= rs.x; y *= rs.y; z *= rs.z; e *= rs.e; return *this; } - FI XYZEval& operator/=(const XYZEval &rs) { x /= rs.x; y /= rs.y; z /= rs.z; e /= rs.e; return *this; } - FI XYZEval& operator*=(const T &v) { x *= v; y *= v; z *= v; e *= v; return *this; } - FI XYZEval& operator>>=(const int &v) { _RS(x); _RS(y); _RS(z); _RS(e); return *this; } - FI XYZEval& operator<<=(const int &v) { _LS(x); _LS(y); _LS(z); _LS(e); return *this; } - FI bool operator==(const XYZval &rs) { return x == rs.x && y == rs.y && z == rs.z; } - FI bool operator!=(const XYZval &rs) { return !operator==(rs); } - FI bool operator==(const XYZval &rs) const { return x == rs.x && y == rs.y && z == rs.z; } - FI bool operator!=(const XYZval &rs) const { return !operator==(rs); } - FI XYZEval operator-() { return { -x, -y, -z, -e }; } - FI const XYZEval operator-() const { return { -x, -y, -z, -e }; } + // Setters taking struct types and arrays + FI void set(const XYval pxy) { x = pxy.x; y = pxy.y; } + FI void set(const XYZval pxyz) { set(LINEAR_AXIS_ELEM(pxyz)); } + #if HAS_Z_AXIS + FI void set(LINEAR_AXIS_ARGS(const T)) { LINEAR_AXIS_CODE(a = x, b = y, c = z, u = i, v = j, w = k); } + #endif + FI void set(const XYval pxy, const T pz) { set(pxy); TERN_(HAS_Z_AXIS, z = pz); } + #if LOGICAL_AXES > LINEAR_AXES + FI void set(const XYval pxy, const T pz, const T pe) { set(pxy, pz); e = pe; } + FI void set(const XYZval pxyz, const T pe) { set(pxyz); e = pe; } + FI void set(LOGICAL_AXIS_ARGS(const T)) { LOGICAL_AXIS_CODE(_e = e, a = x, b = y, c = z, u = i, v = j, w = k); } + #endif + + // Length reduced to one dimension + FI T magnitude() const { return (T)sqrtf(LOGICAL_AXIS_GANG(+ e*e, + x*x, + y*y, + z*z, + i*i, + j*j, + k*k)); } + // Pointer to the data as a simple array + FI operator T* () { return pos; } + // If any element is true then it's true + FI operator bool() { return 0 LOGICAL_AXIS_GANG(|| e, || x, || y, || z, || i, || j, || k); } + + // Explicit copy and copies with conversion + FI XYZEval copy() const { XYZEval o = *this; return o; } + FI XYZEval ABS() const { return LOGICAL_AXIS_ARRAY(T(_ABS(e)), T(_ABS(x)), T(_ABS(y)), T(_ABS(z)), T(_ABS(i)), T(_ABS(j)), T(_ABS(k))); } + FI XYZEval asInt() { return LOGICAL_AXIS_ARRAY(int16_t(e), int16_t(x), int16_t(y), int16_t(z), int16_t(i), int16_t(j), int16_t(k)); } + FI XYZEval asInt() const { return LOGICAL_AXIS_ARRAY(int16_t(e), int16_t(x), int16_t(y), int16_t(z), int16_t(i), int16_t(j), int16_t(k)); } + FI XYZEval asLong() { return LOGICAL_AXIS_ARRAY(int32_t(e), int32_t(x), int32_t(y), int32_t(z), int32_t(i), int32_t(j), int32_t(k)); } + FI XYZEval asLong() const { return LOGICAL_AXIS_ARRAY(int32_t(e), int32_t(x), int32_t(y), int32_t(z), int32_t(i), int32_t(j), int32_t(k)); } + FI XYZEval ROUNDL() { return LOGICAL_AXIS_ARRAY(int32_t(LROUND(e)), int32_t(LROUND(x)), int32_t(LROUND(y)), int32_t(LROUND(z)), int32_t(LROUND(i)), int32_t(LROUND(j)), int32_t(LROUND(k))); } + FI XYZEval ROUNDL() const { return LOGICAL_AXIS_ARRAY(int32_t(LROUND(e)), int32_t(LROUND(x)), int32_t(LROUND(y)), int32_t(LROUND(z)), int32_t(LROUND(i)), int32_t(LROUND(j)), int32_t(LROUND(k))); } + FI XYZEval asFloat() { return LOGICAL_AXIS_ARRAY(static_cast(e), static_cast(x), static_cast(y), static_cast(z), static_cast(i), static_cast(j), static_cast(k)); } + FI XYZEval asFloat() const { return LOGICAL_AXIS_ARRAY(static_cast(e), static_cast(x), static_cast(y), static_cast(z), static_cast(i), static_cast(j), static_cast(k)); } + FI XYZEval reciprocal() const { return LOGICAL_AXIS_ARRAY(_RECIP(e), _RECIP(x), _RECIP(y), _RECIP(z), _RECIP(i), _RECIP(j), _RECIP(k)); } + + // Marlin workspace shifting is done with G92 and M206 + FI XYZEval asLogical() const { XYZEval o = asFloat(); toLogical(o); return o; } + FI XYZEval asNative() const { XYZEval o = asFloat(); toNative(o); return o; } + + // In-place cast to types having fewer fields + FI operator XYval&() { return *(XYval*)this; } + FI operator const XYval&() const { return *(const XYval*)this; } + FI operator XYZval&() { return *(XYZval*)this; } + FI operator const XYZval&() const { return *(const XYZval*)this; } + + // Accessor via an AxisEnum (or any integer) [index] + FI T& operator[](const int n) { return pos[n]; } + FI const T& operator[](const int n) const { return pos[n]; } + + // Assignment operator overrides do the expected thing + FI XYZEval& operator= (const T v) { set(LIST_N_1(LINEAR_AXES, v)); return *this; } + FI XYZEval& operator= (const XYval &rs) { set(rs.x, rs.y); return *this; } + FI XYZEval& operator= (const XYZval &rs) { set(LINEAR_AXIS_ELEM(rs)); return *this; } + + // Override other operators to get intuitive behaviors + FI XYZEval operator+ (const XYval &rs) const { XYZEval ls = *this; ls.x += rs.x; ls.y += rs.y; return ls; } + FI XYZEval operator+ (const XYval &rs) { XYZEval ls = *this; ls.x += rs.x; ls.y += rs.y; return ls; } + FI XYZEval operator- (const XYval &rs) const { XYZEval ls = *this; ls.x -= rs.x; ls.y -= rs.y; return ls; } + FI XYZEval operator- (const XYval &rs) { XYZEval ls = *this; ls.x -= rs.x; ls.y -= rs.y; return ls; } + FI XYZEval operator* (const XYval &rs) const { XYZEval ls = *this; ls.x *= rs.x; ls.y *= rs.y; return ls; } + FI XYZEval operator* (const XYval &rs) { XYZEval ls = *this; ls.x *= rs.x; ls.y *= rs.y; return ls; } + FI XYZEval operator/ (const XYval &rs) const { XYZEval ls = *this; ls.x /= rs.x; ls.y /= rs.y; return ls; } + FI XYZEval operator/ (const XYval &rs) { XYZEval ls = *this; ls.x /= rs.x; ls.y /= rs.y; return ls; } + FI XYZEval operator+ (const XYZval &rs) const { XYZval ls = *this; LINEAR_AXIS_CODE(ls.x += rs.x, ls.y += rs.y, ls.z += rs.z, ls.i += rs.i, ls.j += rs.j, ls.k += rs.k); return ls; } + FI XYZEval operator+ (const XYZval &rs) { XYZval ls = *this; LINEAR_AXIS_CODE(ls.x += rs.x, ls.y += rs.y, ls.z += rs.z, ls.i += rs.i, ls.j += rs.j, ls.k += rs.k); return ls; } + FI XYZEval operator- (const XYZval &rs) const { XYZval ls = *this; LINEAR_AXIS_CODE(ls.x -= rs.x, ls.y -= rs.y, ls.z -= rs.z, ls.i -= rs.i, ls.j -= rs.j, ls.k -= rs.k); return ls; } + FI XYZEval operator- (const XYZval &rs) { XYZval ls = *this; LINEAR_AXIS_CODE(ls.x -= rs.x, ls.y -= rs.y, ls.z -= rs.z, ls.i -= rs.i, ls.j -= rs.j, ls.k -= rs.k); return ls; } + FI XYZEval operator* (const XYZval &rs) const { XYZval ls = *this; LINEAR_AXIS_CODE(ls.x *= rs.x, ls.y *= rs.y, ls.z *= rs.z, ls.i *= rs.i, ls.j *= rs.j, ls.k *= rs.k); return ls; } + FI XYZEval operator* (const XYZval &rs) { XYZval ls = *this; LINEAR_AXIS_CODE(ls.x *= rs.x, ls.y *= rs.y, ls.z *= rs.z, ls.i *= rs.i, ls.j *= rs.j, ls.k *= rs.k); return ls; } + FI XYZEval operator/ (const XYZval &rs) const { XYZval ls = *this; LINEAR_AXIS_CODE(ls.x /= rs.x, ls.y /= rs.y, ls.z /= rs.z, ls.i /= rs.i, ls.j /= rs.j, ls.k /= rs.k); return ls; } + FI XYZEval operator/ (const XYZval &rs) { XYZval ls = *this; LINEAR_AXIS_CODE(ls.x /= rs.x, ls.y /= rs.y, ls.z /= rs.z, ls.i /= rs.i, ls.j /= rs.j, ls.k /= rs.k); return ls; } + FI XYZEval operator+ (const XYZEval &rs) const { XYZEval ls = *this; LOGICAL_AXIS_CODE(ls.e += rs.e, ls.x += rs.x, ls.y += rs.y, ls.z += rs.z, ls.i += rs.i, ls.j += rs.j, ls.k += rs.k); return ls; } + FI XYZEval operator+ (const XYZEval &rs) { XYZEval ls = *this; LOGICAL_AXIS_CODE(ls.e += rs.e, ls.x += rs.x, ls.y += rs.y, ls.z += rs.z, ls.i += rs.i, ls.j += rs.j, ls.k += rs.k); return ls; } + FI XYZEval operator- (const XYZEval &rs) const { XYZEval ls = *this; LOGICAL_AXIS_CODE(ls.e -= rs.e, ls.x -= rs.x, ls.y -= rs.y, ls.z -= rs.z, ls.i -= rs.i, ls.j -= rs.j, ls.k -= rs.k); return ls; } + FI XYZEval operator- (const XYZEval &rs) { XYZEval ls = *this; LOGICAL_AXIS_CODE(ls.e -= rs.e, ls.x -= rs.x, ls.y -= rs.y, ls.z -= rs.z, ls.i -= rs.i, ls.j -= rs.j, ls.k -= rs.k); return ls; } + FI XYZEval operator* (const XYZEval &rs) const { XYZEval ls = *this; LOGICAL_AXIS_CODE(ls.e *= rs.e, ls.x *= rs.x, ls.y *= rs.y, ls.z *= rs.z, ls.i *= rs.i, ls.j *= rs.j, ls.k *= rs.k); return ls; } + FI XYZEval operator* (const XYZEval &rs) { XYZEval ls = *this; LOGICAL_AXIS_CODE(ls.e *= rs.e, ls.x *= rs.x, ls.y *= rs.y, ls.z *= rs.z, ls.i *= rs.i, ls.j *= rs.j, ls.k *= rs.k); return ls; } + FI XYZEval operator/ (const XYZEval &rs) const { XYZEval ls = *this; LOGICAL_AXIS_CODE(ls.e /= rs.e, ls.x /= rs.x, ls.y /= rs.y, ls.z /= rs.z, ls.i /= rs.i, ls.j /= rs.j, ls.k /= rs.k); return ls; } + FI XYZEval operator/ (const XYZEval &rs) { XYZEval ls = *this; LOGICAL_AXIS_CODE(ls.e /= rs.e, ls.x /= rs.x, ls.y /= rs.y, ls.z /= rs.z, ls.i /= rs.i, ls.j /= rs.j, ls.k /= rs.k); return ls; } + FI XYZEval operator* (const float &v) const { XYZEval ls = *this; LOGICAL_AXIS_CODE(ls.e *= v, ls.x *= v, ls.y *= v, ls.z *= v, ls.i *= v, ls.j *= v, ls.k *= v ); return ls; } + FI XYZEval operator* (const float &v) { XYZEval ls = *this; LOGICAL_AXIS_CODE(ls.e *= v, ls.x *= v, ls.y *= v, ls.z *= v, ls.i *= v, ls.j *= v, ls.k *= v ); return ls; } + FI XYZEval operator* (const int &v) const { XYZEval ls = *this; LOGICAL_AXIS_CODE(ls.e *= v, ls.x *= v, ls.y *= v, ls.z *= v, ls.i *= v, ls.j *= v, ls.k *= v ); return ls; } + FI XYZEval operator* (const int &v) { XYZEval ls = *this; LOGICAL_AXIS_CODE(ls.e *= v, ls.x *= v, ls.y *= v, ls.z *= v, ls.i *= v, ls.j *= v, ls.k *= v ); return ls; } + FI XYZEval operator/ (const float &v) const { XYZEval ls = *this; LOGICAL_AXIS_CODE(ls.e /= v, ls.x /= v, ls.y /= v, ls.z /= v, ls.i /= v, ls.j /= v, ls.k /= v ); return ls; } + FI XYZEval operator/ (const float &v) { XYZEval ls = *this; LOGICAL_AXIS_CODE(ls.e /= v, ls.x /= v, ls.y /= v, ls.z /= v, ls.i /= v, ls.j /= v, ls.k /= v ); return ls; } + FI XYZEval operator/ (const int &v) const { XYZEval ls = *this; LOGICAL_AXIS_CODE(ls.e /= v, ls.x /= v, ls.y /= v, ls.z /= v, ls.i /= v, ls.j /= v, ls.k /= v ); return ls; } + FI XYZEval operator/ (const int &v) { XYZEval ls = *this; LOGICAL_AXIS_CODE(ls.e /= v, ls.x /= v, ls.y /= v, ls.z /= v, ls.i /= v, ls.j /= v, ls.k /= v ); return ls; } + FI XYZEval operator>>(const int &v) const { XYZEval ls = *this; LOGICAL_AXIS_CODE(_RS(ls.e), _RS(ls.x), _RS(ls.y), _RS(ls.z), _RS(ls.i), _RS(ls.j), _RS(ls.k) ); return ls; } + FI XYZEval operator>>(const int &v) { XYZEval ls = *this; LOGICAL_AXIS_CODE(_RS(ls.e), _RS(ls.x), _RS(ls.y), _RS(ls.z), _RS(ls.i), _RS(ls.j), _RS(ls.k) ); return ls; } + FI XYZEval operator<<(const int &v) const { XYZEval ls = *this; LOGICAL_AXIS_CODE(_LS(ls.e), _LS(ls.x), _LS(ls.y), _LS(ls.z), _LS(ls.i), _LS(ls.j), _LS(ls.k) ); return ls; } + FI XYZEval operator<<(const int &v) { XYZEval ls = *this; LOGICAL_AXIS_CODE(_LS(ls.e), _LS(ls.x), _LS(ls.y), _LS(ls.z), _LS(ls.i), _LS(ls.j), _LS(ls.k) ); return ls; } + FI const XYZEval operator-() const { return LOGICAL_AXIS_ARRAY(-e, -x, -y, -z, -i, -j, -k); } + FI XYZEval operator-() { return LOGICAL_AXIS_ARRAY(-e, -x, -y, -z, -i, -j, -k); } + + // Modifier operators + FI XYZEval& operator+=(const XYval &rs) { x += rs.x; y += rs.y; return *this; } + FI XYZEval& operator-=(const XYval &rs) { x -= rs.x; y -= rs.y; return *this; } + FI XYZEval& operator*=(const XYval &rs) { x *= rs.x; y *= rs.y; return *this; } + FI XYZEval& operator/=(const XYval &rs) { x /= rs.x; y /= rs.y; return *this; } + FI XYZEval& operator+=(const XYZval &rs) { LINEAR_AXIS_CODE(x += rs.x, y += rs.y, z += rs.z, i += rs.i, j += rs.j, k += rs.k); return *this; } + FI XYZEval& operator-=(const XYZval &rs) { LINEAR_AXIS_CODE(x -= rs.x, y -= rs.y, z -= rs.z, i -= rs.i, j -= rs.j, k -= rs.k); return *this; } + FI XYZEval& operator*=(const XYZval &rs) { LINEAR_AXIS_CODE(x *= rs.x, y *= rs.y, z *= rs.z, i *= rs.i, j *= rs.j, k *= rs.k); return *this; } + FI XYZEval& operator/=(const XYZval &rs) { LINEAR_AXIS_CODE(x /= rs.x, y /= rs.y, z /= rs.z, i /= rs.i, j /= rs.j, k /= rs.k); return *this; } + FI XYZEval& operator+=(const XYZEval &rs) { LOGICAL_AXIS_CODE(e += rs.e, x += rs.x, y += rs.y, z += rs.z, i += rs.i, j += rs.j, k += rs.k); return *this; } + FI XYZEval& operator-=(const XYZEval &rs) { LOGICAL_AXIS_CODE(e -= rs.e, x -= rs.x, y -= rs.y, z -= rs.z, i -= rs.i, j -= rs.j, k -= rs.k); return *this; } + FI XYZEval& operator*=(const XYZEval &rs) { LOGICAL_AXIS_CODE(e *= rs.e, x *= rs.x, y *= rs.y, z *= rs.z, i *= rs.i, j *= rs.j, k *= rs.k); return *this; } + FI XYZEval& operator/=(const XYZEval &rs) { LOGICAL_AXIS_CODE(e /= rs.e, x /= rs.x, y /= rs.y, z /= rs.z, i /= rs.i, j /= rs.j, k /= rs.k); return *this; } + FI XYZEval& operator*=(const T &v) { LOGICAL_AXIS_CODE(e *= v, x *= v, y *= v, z *= v, i *= v, j *= v, k *= v); return *this; } + FI XYZEval& operator>>=(const int &v) { LOGICAL_AXIS_CODE(_RS(e), _RS(x), _RS(y), _RS(z), _RS(i), _RS(j), _RS(k)); return *this; } + FI XYZEval& operator<<=(const int &v) { LOGICAL_AXIS_CODE(_LS(e), _LS(x), _LS(y), _LS(z), _LS(i), _LS(j), _LS(k)); return *this; } + + // Exact comparisons. For floats a "NEAR" operation may be better. + FI bool operator==(const XYZval &rs) { return true LINEAR_AXIS_GANG(&& x == rs.x, && y == rs.y, && z == rs.z, && i == rs.i, && j == rs.j, && k == rs.k); } + FI bool operator==(const XYZval &rs) const { return true LINEAR_AXIS_GANG(&& x == rs.x, && y == rs.y, && z == rs.z, && i == rs.i, && j == rs.j, && k == rs.k); } + FI bool operator!=(const XYZval &rs) { return !operator==(rs); } + FI bool operator!=(const XYZval &rs) const { return !operator==(rs); } }; #undef _RECIP @@ -498,6 +669,3 @@ struct XYZEval { #undef _LS #undef _RS #undef FI - -const xyze_char_t axis_codes { 'X', 'Y', 'Z', 'E' }; -#define XYZ_CHAR(A) ((char)('X' + A)) diff --git a/Marlin/src/core/utility.cpp b/Marlin/src/core/utility.cpp index 3d7897f95a0a..19e76267447d 100644 --- a/Marlin/src/core/utility.cpp +++ b/Marlin/src/core/utility.cpp @@ -60,7 +60,8 @@ void safe_delay(millis_t ms) { TERN_(DELTA, "Delta") TERN_(IS_SCARA, "SCARA") TERN_(IS_CORE, "Core") - TERN_(MARKFORGED_XY, "MarkForged") + TERN_(MARKFORGED_XY, "MarkForgedXY") + TERN_(MARKFORGED_YX, "MarkForgedYX") TERN_(IS_CARTESIAN, "Cartesian") ); @@ -73,15 +74,16 @@ void safe_delay(millis_t ms) { TERN_(Z_PROBE_SLED, "Z_PROBE_SLED") TERN_(Z_PROBE_ALLEN_KEY, "Z_PROBE_ALLEN_KEY") TERN_(SOLENOID_PROBE, "SOLENOID_PROBE") - TERN(PROBE_SELECTED, "", "NONE") + TERN_(MAGLEV4, "MAGLEV4") + IF_DISABLED(PROBE_SELECTED, "NONE") ); #if HAS_BED_PROBE #if !HAS_PROBE_XY_OFFSET - SERIAL_ECHOPAIR("Probe Offset X0 Y0 Z", probe.offset.z, " ("); + SERIAL_ECHOPGM("Probe Offset X0 Y0 Z", probe.offset.z, " ("); #else - SERIAL_ECHOPAIR_P(PSTR("Probe Offset X"), probe.offset_xy.x, SP_Y_STR, probe.offset_xy.y, SP_Z_STR, probe.offset.z); + SERIAL_ECHOPGM_P(PSTR("Probe Offset X"), probe.offset_xy.x, SP_Y_STR, probe.offset_xy.y, SP_Z_STR, probe.offset.z); if (probe.offset_xy.x > 0) SERIAL_ECHOPGM(" (Right"); else if (probe.offset_xy.x < 0) @@ -92,9 +94,9 @@ void safe_delay(millis_t ms) { SERIAL_ECHOPGM(" (Aligned With"); if (probe.offset_xy.y > 0) - SERIAL_ECHOPGM_P(ENABLED(IS_SCARA) ? PSTR("-Distal") : PSTR("-Back")); + SERIAL_ECHOF(F(TERN(IS_SCARA, "-Distal", "-Back"))); else if (probe.offset_xy.y < 0) - SERIAL_ECHOPGM_P(ENABLED(IS_SCARA) ? PSTR("-Proximal") : PSTR("-Front")); + SERIAL_ECHOF(F(TERN(IS_SCARA, "-Proximal", "-Front"))); else if (probe.offset_xy.x != 0) SERIAL_ECHOPGM("-Center"); @@ -102,7 +104,7 @@ void safe_delay(millis_t ms) { #endif - SERIAL_ECHOPGM_P(probe.offset.z < 0 ? PSTR("Below") : probe.offset.z > 0 ? PSTR("Above") : PSTR("Same Z as")); + SERIAL_ECHOF(probe.offset.z < 0 ? F("Below") : probe.offset.z > 0 ? F("Above") : F("Same Z as")); SERIAL_ECHOLNPGM(" Nozzle)"); #endif @@ -119,13 +121,13 @@ void safe_delay(millis_t ms) { SERIAL_ECHOLNPGM(" (enabled)"); #if ENABLED(ENABLE_LEVELING_FADE_HEIGHT) if (planner.z_fade_height) - SERIAL_ECHOLNPAIR("Z Fade: ", planner.z_fade_height); + SERIAL_ECHOLNPGM("Z Fade: ", planner.z_fade_height); #endif #if ABL_PLANAR - SERIAL_ECHOPGM("ABL Adjustment X"); - LOOP_XYZ(a) { + SERIAL_ECHOPGM("ABL Adjustment"); + LOOP_LINEAR_AXES(a) { const float v = planner.get_axis_position_mm(AxisEnum(a)) - current_position[a]; - SERIAL_CHAR(' ', XYZ_CHAR(a)); + SERIAL_CHAR(' ', AXIS_CHAR(a)); if (v > 0) SERIAL_CHAR('+'); SERIAL_DECIMAL(v); } @@ -140,7 +142,7 @@ void safe_delay(millis_t ms) { SERIAL_ECHO(ftostr43sign(rz, '+')); #if ENABLED(ENABLE_LEVELING_FADE_HEIGHT) if (planner.z_fade_height) { - SERIAL_ECHOPAIR(" (", ftostr43sign(rz * planner.fade_scaling_factor_for_z(current_position.z), '+')); + SERIAL_ECHOPGM(" (", ftostr43sign(rz * planner.fade_scaling_factor_for_z(current_position.z), '+')); SERIAL_CHAR(')'); } #endif @@ -156,10 +158,10 @@ void safe_delay(millis_t ms) { SERIAL_ECHOPGM("Mesh Bed Leveling"); if (planner.leveling_active) { SERIAL_ECHOLNPGM(" (enabled)"); - SERIAL_ECHOPAIR("MBL Adjustment Z", ftostr43sign(mbl.get_z(current_position), '+')); + SERIAL_ECHOPGM("MBL Adjustment Z", ftostr43sign(mbl.get_z(current_position), '+')); #if ENABLED(ENABLE_LEVELING_FADE_HEIGHT) if (planner.z_fade_height) { - SERIAL_ECHOPAIR(" (", ftostr43sign( + SERIAL_ECHOPGM(" (", ftostr43sign( mbl.get_z(current_position, planner.fade_scaling_factor_for_z(current_position.z)), '+' )); SERIAL_CHAR(')'); diff --git a/Marlin/src/core/utility.h b/Marlin/src/core/utility.h index 645a4be80735..d248091ce575 100644 --- a/Marlin/src/core/utility.h +++ b/Marlin/src/core/utility.h @@ -25,8 +25,7 @@ #include "../core/types.h" #include "../core/millis_t.h" -// Delay that ensures heaters and watchdog are kept alive -void safe_delay(millis_t ms); +void safe_delay(millis_t ms); // Delay ensuring that temperatures are updated and the watchdog is kept alive. #if ENABLED(SERIAL_OVERRUN_PROTECTION) void serial_delay(const millis_t ms); @@ -34,7 +33,7 @@ void safe_delay(millis_t ms); inline void serial_delay(const millis_t) {} #endif -#if GRID_MAX_POINTS_X && GRID_MAX_POINTS_Y +#if (GRID_MAX_POINTS_X) && (GRID_MAX_POINTS_Y) // 16x16 bit arrays template @@ -77,3 +76,11 @@ class restorer { // Converts from an uint8_t in the range of 0-255 to an uint8_t // in the range 0-100 while avoiding rounding artifacts constexpr uint8_t ui8_to_percent(const uint8_t i) { return (int(i) * 100 + 127) / 255; } + +const xyze_char_t axis_codes LOGICAL_AXIS_ARRAY('E', 'X', 'Y', 'Z', AXIS4_NAME, AXIS5_NAME, AXIS6_NAME); + +#if LINEAR_AXES <= XYZ + #define AXIS_CHAR(A) ((char)('X' + A)) +#else + #define AXIS_CHAR(A) axis_codes[A] +#endif diff --git a/Marlin/src/feature/adc/adc_mcp3426.cpp b/Marlin/src/feature/adc/adc_mcp3426.cpp new file mode 100644 index 000000000000..aaddf46821b0 --- /dev/null +++ b/Marlin/src/feature/adc/adc_mcp3426.cpp @@ -0,0 +1,104 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2021 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + * + */ + +/** + * adc_mcp3426.cpp - library for MicroChip MCP3426 I2C A/D converter + * + * For implementation details, please take a look at the datasheet: + * https://www.microchip.com/en-us/product/MCP3426 + */ + +#include "../../inc/MarlinConfig.h" + +#if ENABLED(HAS_MCP3426_ADC) + +#include "adc_mcp3426.h" + +// Read the ADC value from MCP342X on a specific channel +int16_t MCP3426::ReadValue(uint8_t channel, uint8_t gain) { + Error = false; + + #if PINS_EXIST(I2C_SCL, I2C_SDA) && DISABLED(SOFT_I2C_EEPROM) + Wire.setSDA(pin_t(I2C_SDA_PIN)); + Wire.setSCL(pin_t(I2C_SCL_PIN)); + #endif + + Wire.begin(); // No address joins the BUS as the master + + Wire.beginTransmission(I2C_ADDRESS(MCP342X_ADC_I2C_ADDRESS)); + + // Continuous Conversion Mode, 16 bit, Channel 1, Gain x4 + // 26 = 0b00011000 + // RXXCSSGG + // R = Ready Bit + // XX = Channel (00=1, 01=2, 10=3 (MCP3428), 11=4 (MCP3428)) + // C = Conversion Mode Bit (1= Continuous Conversion Mode (Default)) + // SS = Sample rate, 10=15 samples per second @ 16 bits + // GG = Gain 00 =x1 + uint8_t controlRegister = 0b00011000; + + if (channel == 2) controlRegister |= 0b00100000; // Select channel 2 + + if (gain == 2) + controlRegister |= 0b00000001; + else if (gain == 4) + controlRegister |= 0b00000010; + else if (gain == 8) + controlRegister |= 0b00000011; + + Wire.write(controlRegister); + if (Wire.endTransmission() != 0) { + Error = true; + return 0; + } + + const uint8_t len = 3; + uint8_t buffer[len] = {}; + + do { + Wire.requestFrom(I2C_ADDRESS(MCP342X_ADC_I2C_ADDRESS), len); + if (Wire.available() != len) { + Error = true; + return 0; + } + + for (uint8_t i = 0; i < len; ++i) + buffer[i] = Wire.read(); + + // Is conversion ready, if not loop around again + } while ((buffer[2] & 0x80) != 0); + + union TwoBytesToInt16 { + uint8_t bytes[2]; + int16_t integervalue; + }; + TwoBytesToInt16 ConversionUnion; + + ConversionUnion.bytes[1] = buffer[0]; + ConversionUnion.bytes[0] = buffer[1]; + + return ConversionUnion.integervalue; +} + +MCP3426 mcp3426; + +#endif // HAS_MCP3426_ADC diff --git a/Marlin/src/feature/adc/adc_mcp3426.h b/Marlin/src/feature/adc/adc_mcp3426.h new file mode 100644 index 000000000000..35458716b975 --- /dev/null +++ b/Marlin/src/feature/adc/adc_mcp3426.h @@ -0,0 +1,41 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2021 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + * + */ +#pragma once + +/** + * Arduino library for MicroChip MCP3426 I2C A/D converter. + * https://www.microchip.com/en-us/product/MCP3426 + */ + +#include +#include + +// Address of MCP342X chip +#define MCP342X_ADC_I2C_ADDRESS 104 + +class MCP3426 { + public: + int16_t ReadValue(uint8_t channel, uint8_t gain); + bool Error; +}; + +extern MCP3426 mcp3426; diff --git a/Marlin/src/feature/ammeter.cpp b/Marlin/src/feature/ammeter.cpp new file mode 100644 index 000000000000..71b84f1121a0 --- /dev/null +++ b/Marlin/src/feature/ammeter.cpp @@ -0,0 +1,54 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2021 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + * + */ + +#include "../inc/MarlinConfig.h" + +#if ENABLED(I2C_AMMETER) + +#include "ammeter.h" + +#ifndef I2C_AMMETER_IMAX + #define I2C_AMMETER_IMAX 0.500 // Calibration range 500 Milliamps +#endif + +INA226 ina; + +Ammeter ammeter; + +float Ammeter::scale; +float Ammeter::current; + +void Ammeter::init() { + ina.begin(); + ina.configure(INA226_AVERAGES_16, INA226_BUS_CONV_TIME_1100US, INA226_SHUNT_CONV_TIME_1100US, INA226_MODE_SHUNT_BUS_CONT); + ina.calibrate(I2C_AMMETER_SHUNT_RESISTOR, I2C_AMMETER_IMAX); +} + +float Ammeter::read() { + scale = 1; + current = ina.readShuntCurrent(); + if (current <= 0.0001f) current = 0; // Clean up least-significant-bit amplification errors + if (current < 0.1f) scale = 1000; + return current * scale; +} + +#endif // I2C_AMMETER diff --git a/Marlin/src/feature/ammeter.h b/Marlin/src/feature/ammeter.h new file mode 100644 index 000000000000..86f09bb9a197 --- /dev/null +++ b/Marlin/src/feature/ammeter.h @@ -0,0 +1,39 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2021 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + * + */ +#pragma once + +#include "../inc/MarlinConfigPre.h" + +#include +#include + +class Ammeter { +private: + static float scale; + +public: + static float current; + static void init(); + static float read(); +}; + +extern Ammeter ammeter; diff --git a/Marlin/src/feature/babystep.cpp b/Marlin/src/feature/babystep.cpp index b07688146110..54ad9588f445 100644 --- a/Marlin/src/feature/babystep.cpp +++ b/Marlin/src/feature/babystep.cpp @@ -50,7 +50,7 @@ void Babystep::step_axis(const AxisEnum axis) { } } -void Babystep::add_mm(const AxisEnum axis, const float &mm) { +void Babystep::add_mm(const AxisEnum axis, const_float_t mm) { add_steps(axis, mm * planner.settings.axis_steps_per_mm[axis]); } diff --git a/Marlin/src/feature/babystep.h b/Marlin/src/feature/babystep.h index f85e5909cad4..5693afb4fc5b 100644 --- a/Marlin/src/feature/babystep.h +++ b/Marlin/src/feature/babystep.h @@ -54,16 +54,16 @@ class Babystep { #if ENABLED(BABYSTEP_DISPLAY_TOTAL) static int16_t axis_total[BS_TOTAL_IND(Z_AXIS) + 1]; // Total babysteps since G28 - static inline void reset_total(const AxisEnum axis) { + static void reset_total(const AxisEnum axis) { if (TERN1(BABYSTEP_XY, axis == Z_AXIS)) axis_total[BS_TOTAL_IND(axis)] = 0; } #endif static void add_steps(const AxisEnum axis, const int16_t distance); - static void add_mm(const AxisEnum axis, const float &mm); + static void add_mm(const AxisEnum axis, const_float_t mm); - static inline bool has_steps() { + static bool has_steps() { return steps[BS_AXIS_IND(X_AXIS)] || steps[BS_AXIS_IND(Y_AXIS)] || steps[BS_AXIS_IND(Z_AXIS)]; } @@ -71,7 +71,7 @@ class Babystep { // Called by the Temperature or Stepper ISR to // apply accumulated babysteps to the axes. // - static inline void task() { + static void task() { LOOP_LE_N(i, BS_AXIS_IND(Z_AXIS)) step_axis(BS_AXIS(i)); } diff --git a/Marlin/src/feature/backlash.cpp b/Marlin/src/feature/backlash.cpp index b848214f0c46..24c0f2ca0c15 100644 --- a/Marlin/src/feature/backlash.cpp +++ b/Marlin/src/feature/backlash.cpp @@ -60,13 +60,27 @@ Backlash backlash; * spread over multiple segments, smoothing out artifacts even more. */ -void Backlash::add_correction_steps(const int32_t &da, const int32_t &db, const int32_t &dc, const uint8_t dm, block_t * const block) { - static uint8_t last_direction_bits; - uint8_t changed_dir = last_direction_bits ^ dm; - // Ignore direction change if no steps are taken in that direction - if (da == 0) CBI(changed_dir, X_AXIS); - if (db == 0) CBI(changed_dir, Y_AXIS); - if (dc == 0) CBI(changed_dir, Z_AXIS); +void Backlash::add_correction_steps(const int32_t &da, const int32_t &db, const int32_t &dc, const axis_bits_t dm, block_t * const block) { + static axis_bits_t last_direction_bits; + axis_bits_t changed_dir = last_direction_bits ^ dm; + // Ignore direction change unless steps are taken in that direction + #if DISABLED(CORE_BACKLASH) || EITHER(MARKFORGED_XY, MARKFORGED_YX) + if (!da) CBI(changed_dir, X_AXIS); + if (!db) CBI(changed_dir, Y_AXIS); + if (!dc) CBI(changed_dir, Z_AXIS); + #elif CORE_IS_XY + if (!(da + db)) CBI(changed_dir, X_AXIS); + if (!(da - db)) CBI(changed_dir, Y_AXIS); + if (!dc) CBI(changed_dir, Z_AXIS); + #elif CORE_IS_XZ + if (!(da + dc)) CBI(changed_dir, X_AXIS); + if (!(da - dc)) CBI(changed_dir, Z_AXIS); + if (!db) CBI(changed_dir, Y_AXIS); + #elif CORE_IS_YZ + if (!(db + dc)) CBI(changed_dir, Y_AXIS); + if (!(db - dc)) CBI(changed_dir, Z_AXIS); + if (!da) CBI(changed_dir, X_AXIS); + #endif last_direction_bits ^= changed_dir; if (correction == 0) return; @@ -90,7 +104,7 @@ void Backlash::add_correction_steps(const int32_t &da, const int32_t &db, const const float f_corr = float(correction) / 255.0f; - LOOP_XYZ(axis) { + LOOP_LINEAR_AXES(axis) { if (distance_mm[axis]) { const bool reversing = TEST(dm,axis); @@ -105,18 +119,35 @@ void Backlash::add_correction_steps(const int32_t &da, const int32_t &db, const // Take up a portion of the residual_error in this segment, but only when // the current segment travels in the same direction as the correction if (reversing == (error_correction < 0)) { - if (segment_proportion == 0) - segment_proportion = _MIN(1.0f, block->millimeters / smoothing_mm); + if (segment_proportion == 0) segment_proportion = _MIN(1.0f, block->millimeters / smoothing_mm); error_correction = CEIL(segment_proportion * error_correction); } else error_correction = 0; // Don't take up any backlash in this segment, as it would subtract steps } #endif - // Making a correction reduces the residual error and adds block steps + + // This correction reduces the residual error and adds block steps if (error_correction) { block->steps[axis] += ABS(error_correction); - residual_error[axis] -= error_correction; + #if ENABLED(CORE_BACKLASH) + switch (axis) { + case CORE_AXIS_1: + //block->steps[CORE_AXIS_2] += influence_distance_mm[axis] * planner.settings.axis_steps_per_mm[CORE_AXIS_2]; + //SERIAL_ECHOLNPGM("CORE_AXIS_1 dir change. distance=", distance_mm[axis], " r.err=", residual_error[axis], + // " da=", da, " db=", db, " block->steps[axis]=", block->steps[axis], " err_corr=", error_correction); + break; + case CORE_AXIS_2: + //block->steps[CORE_AXIS_1] += influence_distance_mm[axis] * planner.settings.axis_steps_per_mm[CORE_AXIS_1];; + //SERIAL_ECHOLNPGM("CORE_AXIS_2 dir change. distance=", distance_mm[axis], " r.err=", residual_error[axis], + // " da=", da, " db=", db, " block->steps[axis]=", block->steps[axis], " err_corr=", error_correction); + break; + case NORMAL_AXIS: break; + } + residual_error[axis] = 0; // No residual_error needed for next CORE block, I think... + #else + residual_error[axis] -= error_correction; + #endif } } } diff --git a/Marlin/src/feature/backlash.h b/Marlin/src/feature/backlash.h index 49857f1f99d0..17504cc78181 100644 --- a/Marlin/src/feature/backlash.h +++ b/Marlin/src/feature/backlash.h @@ -35,8 +35,8 @@ class Backlash { static float smoothing_mm; #endif - static inline void set_correction(const float &v) { correction = _MAX(0, _MIN(1.0, v)) * all_on; } - static inline float get_correction() { return float(ui8_to_percent(correction)) / 100.0f; } + static void set_correction(const_float_t v) { correction = _MAX(0, _MIN(1.0, v)) * all_on; } + static float get_correction() { return float(ui8_to_percent(correction)) / 100.0f; } #else static constexpr uint8_t correction = (BACKLASH_CORRECTION) * 0xFF; static const xyz_float_t distance_mm; @@ -53,7 +53,7 @@ class Backlash { static void measure_with_probe(); #endif - static inline float get_measurement(const AxisEnum a) { + static float get_measurement(const AxisEnum a) { UNUSED(a); // Return the measurement averaged over all readings return TERN(MEASURE_BACKLASH_WHEN_PROBING @@ -62,16 +62,16 @@ class Backlash { ); } - static inline bool has_measurement(const AxisEnum a) { + static bool has_measurement(const AxisEnum a) { UNUSED(a); return TERN0(MEASURE_BACKLASH_WHEN_PROBING, measured_count[a] > 0); } - static inline bool has_any_measurement() { + static bool has_any_measurement() { return has_measurement(X_AXIS) || has_measurement(Y_AXIS) || has_measurement(Z_AXIS); } - void add_correction_steps(const int32_t &da, const int32_t &db, const int32_t &dc, const uint8_t dm, block_t * const block); + void add_correction_steps(const int32_t &da, const int32_t &db, const int32_t &dc, const axis_bits_t dm, block_t * const block); }; extern Backlash backlash; diff --git a/Marlin/src/feature/bedlevel/abl/abl.cpp b/Marlin/src/feature/bedlevel/abl/abl.cpp index a663ee571df5..ece748198195 100644 --- a/Marlin/src/feature/bedlevel/abl/abl.cpp +++ b/Marlin/src/feature/bedlevel/abl/abl.cpp @@ -85,9 +85,9 @@ static void extrapolate_one_point(const uint8_t x, const uint8_t y, const int8_t //#define EXTRAPOLATE_FROM_EDGE #if ENABLED(EXTRAPOLATE_FROM_EDGE) - #if GRID_MAX_POINTS_X < GRID_MAX_POINTS_Y + #if (GRID_MAX_POINTS_X) < (GRID_MAX_POINTS_Y) #define HALF_IN_X - #elif GRID_MAX_POINTS_Y < GRID_MAX_POINTS_X + #elif (GRID_MAX_POINTS_Y) < (GRID_MAX_POINTS_X) #define HALF_IN_Y #endif #endif @@ -98,23 +98,23 @@ static void extrapolate_one_point(const uint8_t x, const uint8_t y, const int8_t */ void extrapolate_unprobed_bed_level() { #ifdef HALF_IN_X - constexpr uint8_t ctrx2 = 0, xlen = GRID_MAX_POINTS_X - 1; + constexpr uint8_t ctrx2 = 0, xend = GRID_MAX_POINTS_X - 1; #else - constexpr uint8_t ctrx1 = (GRID_MAX_POINTS_X - 1) / 2, // left-of-center - ctrx2 = (GRID_MAX_POINTS_X) / 2, // right-of-center - xlen = ctrx1; + constexpr uint8_t ctrx1 = (GRID_MAX_CELLS_X) / 2, // left-of-center + ctrx2 = (GRID_MAX_POINTS_X) / 2, // right-of-center + xend = ctrx1; #endif #ifdef HALF_IN_Y - constexpr uint8_t ctry2 = 0, ylen = GRID_MAX_POINTS_Y - 1; + constexpr uint8_t ctry2 = 0, yend = GRID_MAX_POINTS_Y - 1; #else - constexpr uint8_t ctry1 = (GRID_MAX_POINTS_Y - 1) / 2, // top-of-center - ctry2 = (GRID_MAX_POINTS_Y) / 2, // bottom-of-center - ylen = ctry1; + constexpr uint8_t ctry1 = (GRID_MAX_CELLS_Y) / 2, // top-of-center + ctry2 = (GRID_MAX_POINTS_Y) / 2, // bottom-of-center + yend = ctry1; #endif - LOOP_LE_N(xo, xlen) - LOOP_LE_N(yo, ylen) { + LOOP_LE_N(xo, xend) + LOOP_LE_N(yo, yend) { uint8_t x2 = ctrx2 + xo, y2 = ctry2 + yo; #ifndef HALF_IN_X const uint8_t x1 = ctrx1 - xo; @@ -143,8 +143,8 @@ void print_bilinear_leveling_grid() { #if ENABLED(ABL_BILINEAR_SUBDIVISION) - #define ABL_GRID_POINTS_VIRT_X (GRID_MAX_POINTS_X - 1) * (BILINEAR_SUBDIVISIONS) + 1 - #define ABL_GRID_POINTS_VIRT_Y (GRID_MAX_POINTS_Y - 1) * (BILINEAR_SUBDIVISIONS) + 1 + #define ABL_GRID_POINTS_VIRT_X GRID_MAX_CELLS_X * (BILINEAR_SUBDIVISIONS) + 1 + #define ABL_GRID_POINTS_VIRT_Y GRID_MAX_CELLS_Y * (BILINEAR_SUBDIVISIONS) + 1 #define ABL_TEMP_POINTS_X (GRID_MAX_POINTS_X + 2) #define ABL_TEMP_POINTS_Y (GRID_MAX_POINTS_Y + 2) float z_values_virt[ABL_GRID_POINTS_VIRT_X][ABL_GRID_POINTS_VIRT_Y]; @@ -161,7 +161,7 @@ void print_bilinear_leveling_grid() { #define LINEAR_EXTRAPOLATION(E, I) ((E) * 2 - (I)) float bed_level_virt_coord(const uint8_t x, const uint8_t y) { uint8_t ep = 0, ip = 1; - if (x > GRID_MAX_POINTS_X + 1 || y > GRID_MAX_POINTS_Y + 1) { + if (x > (GRID_MAX_POINTS_X) + 1 || y > (GRID_MAX_POINTS_Y) + 1) { // The requested point requires extrapolating two points beyond the mesh. // These values are only requested for the edges of the mesh, which are always an actual mesh point, // and do not require interpolation. When interpolation is not needed, this "Mesh + 2" point is @@ -171,8 +171,8 @@ void print_bilinear_leveling_grid() { } if (!x || x == ABL_TEMP_POINTS_X - 1) { if (x) { - ep = GRID_MAX_POINTS_X - 1; - ip = GRID_MAX_POINTS_X - 2; + ep = (GRID_MAX_POINTS_X) - 1; + ip = GRID_MAX_CELLS_X - 1; } if (WITHIN(y, 1, ABL_TEMP_POINTS_Y - 2)) return LINEAR_EXTRAPOLATION( @@ -187,8 +187,8 @@ void print_bilinear_leveling_grid() { } if (!y || y == ABL_TEMP_POINTS_Y - 1) { if (y) { - ep = GRID_MAX_POINTS_Y - 1; - ip = GRID_MAX_POINTS_Y - 2; + ep = (GRID_MAX_POINTS_Y) - 1; + ip = GRID_MAX_CELLS_Y - 1; } if (WITHIN(x, 1, ABL_TEMP_POINTS_X - 2)) return LINEAR_EXTRAPOLATION( @@ -213,7 +213,7 @@ void print_bilinear_leveling_grid() { ) * 0.5f; } - static float bed_level_virt_2cmr(const uint8_t x, const uint8_t y, const float &tx, const float &ty) { + static float bed_level_virt_2cmr(const uint8_t x, const uint8_t y, const_float_t tx, const_float_t ty) { float row[4], column[4]; LOOP_L_N(i, 4) { LOOP_L_N(j, 4) { @@ -336,11 +336,11 @@ float bilinear_z_offset(const xy_pos_t &raw) { /* static float last_offset = 0; if (ABS(last_offset - offset) > 0.2) { - SERIAL_ECHOLNPAIR("Sudden Shift at x=", rel.x, " / ", bilinear_grid_spacing.x, " -> thisg.x=", thisg.x); - SERIAL_ECHOLNPAIR(" y=", rel.y, " / ", bilinear_grid_spacing.y, " -> thisg.y=", thisg.y); - SERIAL_ECHOLNPAIR(" ratio.x=", ratio.x, " ratio.y=", ratio.y); - SERIAL_ECHOLNPAIR(" z1=", z1, " z2=", z2, " z3=", z3, " z4=", z4); - SERIAL_ECHOLNPAIR(" L=", L, " R=", R, " offset=", offset); + SERIAL_ECHOLNPGM("Sudden Shift at x=", rel.x, " / ", bilinear_grid_spacing.x, " -> thisg.x=", thisg.x); + SERIAL_ECHOLNPGM(" y=", rel.y, " / ", bilinear_grid_spacing.y, " -> thisg.y=", thisg.y); + SERIAL_ECHOLNPGM(" ratio.x=", ratio.x, " ratio.y=", ratio.y); + SERIAL_ECHOLNPGM(" z1=", z1, " z2=", z2, " z3=", z3, " z4=", z4); + SERIAL_ECHOLNPGM(" L=", L, " R=", R, " offset=", offset); } last_offset = offset; //*/ @@ -356,7 +356,7 @@ float bilinear_z_offset(const xy_pos_t &raw) { * Prepare a bilinear-leveled linear move on Cartesian, * splitting the move where it crosses grid borders. */ - void bilinear_line_to_destination(const feedRate_t &scaled_fr_mm_s, uint16_t x_splits, uint16_t y_splits) { + void bilinear_line_to_destination(const_feedRate_t scaled_fr_mm_s, uint16_t x_splits, uint16_t y_splits) { // Get current and destination cells for this line xy_int_t c1 { CELL_INDEX(x, current_position.x), CELL_INDEX(y, current_position.y) }, c2 { CELL_INDEX(x, destination.x), CELL_INDEX(y, destination.y) }; diff --git a/Marlin/src/feature/bedlevel/abl/abl.h b/Marlin/src/feature/bedlevel/abl/abl.h index bbe2411dc346..3d54c55695e8 100644 --- a/Marlin/src/feature/bedlevel/abl/abl.h +++ b/Marlin/src/feature/bedlevel/abl/abl.h @@ -37,7 +37,7 @@ void refresh_bed_level(); #endif #if IS_CARTESIAN && DISABLED(SEGMENT_LEVELED_MOVES) - void bilinear_line_to_destination(const feedRate_t &scaled_fr_mm_s, uint16_t x_splits=0xFFFF, uint16_t y_splits=0xFFFF); + void bilinear_line_to_destination(const_feedRate_t scaled_fr_mm_s, uint16_t x_splits=0xFFFF, uint16_t y_splits=0xFFFF); #endif #define _GET_MESH_X(I) float(bilinear_start.x + (I) * bilinear_grid_spacing.x) diff --git a/Marlin/src/feature/bedlevel/abl/x_twist.cpp b/Marlin/src/feature/bedlevel/abl/x_twist.cpp new file mode 100644 index 000000000000..24fdbca95045 --- /dev/null +++ b/Marlin/src/feature/bedlevel/abl/x_twist.cpp @@ -0,0 +1,60 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2021 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + * + */ +#include "../../../inc/MarlinConfig.h" + +#if ENABLED(X_AXIS_TWIST_COMPENSATION) + +#include "../bedlevel.h" + +XATC xatc; + +float XATC::spacing, XATC::start; +xatc_array_t XATC::z_offset; + +void XATC::print_points() { + SERIAL_ECHOLNPGM(" X-Twist Correction:"); + LOOP_L_N(x, XATC_MAX_POINTS) { + SERIAL_CHAR(' '); + if (!isnan(z_offset[x])) { + if (z_offset[x] >= 0) SERIAL_CHAR('+'); + SERIAL_ECHO_F(z_offset[x], 3); + } + else { + LOOP_L_N(i, 6) + SERIAL_CHAR(i ? '=' : ' '); + } + } + SERIAL_EOL(); +} + +float lerp(const_float_t t, const_float_t a, const_float_t b) { return a + t * (b - a); } + +float XATC::compensation(const xy_pos_t &raw) { + if (NEAR_ZERO(spacing)) return 0; + float t = (raw.x - start) / spacing; + int i = FLOOR(t); + LIMIT(i, 0, XATC_MAX_POINTS - 2); + t -= i; + return lerp(t, z_offset[i], z_offset[i + 1]); +} + +#endif // X_AXIS_TWIST_COMPENSATION diff --git a/Marlin/src/feature/bedlevel/abl/x_twist.h b/Marlin/src/feature/bedlevel/abl/x_twist.h new file mode 100644 index 000000000000..9deb45a8b7c4 --- /dev/null +++ b/Marlin/src/feature/bedlevel/abl/x_twist.h @@ -0,0 +1,37 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2021 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + * + */ +#pragma once + +#include "../../../inc/MarlinConfigPre.h" + +typedef float xatc_array_t[XATC_MAX_POINTS]; + +class XATC { +public: + static float spacing, start; + static xatc_array_t z_offset; + + static float compensation(const xy_pos_t &raw); + static void print_points(); +}; + +extern XATC xatc; diff --git a/Marlin/src/feature/bedlevel/bedlevel.cpp b/Marlin/src/feature/bedlevel/bedlevel.cpp index 2ad4ffecaf76..8e03632de44d 100644 --- a/Marlin/src/feature/bedlevel/bedlevel.cpp +++ b/Marlin/src/feature/bedlevel/bedlevel.cpp @@ -98,7 +98,7 @@ TemporaryBedLevelingState::TemporaryBedLevelingState(const bool enable) : saved( #if ENABLED(ENABLE_LEVELING_FADE_HEIGHT) - void set_z_fade_height(const float zfh, const bool do_report/*=true*/) { + void set_z_fade_height(const_float_t zfh, const bool do_report/*=true*/) { if (planner.z_fade_height == zfh) return; @@ -213,27 +213,27 @@ void reset_bed_level() { void _manual_goto_xy(const xy_pos_t &pos) { + // Get the resting Z position for after the XY move #ifdef MANUAL_PROBE_START_Z - constexpr float startz = _MAX(0, MANUAL_PROBE_START_Z); - #if MANUAL_PROBE_HEIGHT > 0 - do_blocking_move_to_xy_z(pos, MANUAL_PROBE_HEIGHT); - do_blocking_move_to_z(startz); - #else - do_blocking_move_to_xy_z(pos, startz); - #endif - #elif MANUAL_PROBE_HEIGHT > 0 - const float prev_z = current_position.z; - do_blocking_move_to_xy_z(pos, MANUAL_PROBE_HEIGHT); - do_blocking_move_to_z(prev_z); + constexpr float finalz = _MAX(0, MANUAL_PROBE_START_Z); // If a MANUAL_PROBE_START_Z value is set, always respect it #else - do_blocking_move_to_xy(pos); + #warning "It's recommended to set some MANUAL_PROBE_START_Z value for manual leveling." + #endif + #if Z_CLEARANCE_BETWEEN_MANUAL_PROBES > 0 // A probe/obstacle clearance exists so there is a raise: + #ifndef MANUAL_PROBE_START_Z + const float finalz = current_position.z; // - Use the current Z for starting-Z if no MANUAL_PROBE_START_Z was provided + #endif + do_blocking_move_to_xy_z(pos, Z_CLEARANCE_BETWEEN_MANUAL_PROBES); // - Raise Z, then move to the new XY + do_blocking_move_to_z(finalz); // - Lower down to the starting Z height, ready for adjustment! + #elif defined(MANUAL_PROBE_START_Z) // A starting-Z was provided, but there's no raise: + do_blocking_move_to_xy_z(pos, finalz); // - Move in XY then down to the starting Z height, ready for adjustment! + #else // Zero raise and no starting Z height either: + do_blocking_move_to_xy(pos); // - Move over with no raise, ready for adjustment! #endif - - current_position = pos; TERN_(LCD_BED_LEVELING, ui.wait_for_move = false); } -#endif +#endif // MESH_BED_LEVELING || PROBE_MANUALLY #endif // HAS_LEVELING diff --git a/Marlin/src/feature/bedlevel/bedlevel.h b/Marlin/src/feature/bedlevel/bedlevel.h index a33f08ad0ee1..c623c99b5c52 100644 --- a/Marlin/src/feature/bedlevel/bedlevel.h +++ b/Marlin/src/feature/bedlevel/bedlevel.h @@ -24,7 +24,7 @@ #include "../../inc/MarlinConfigPre.h" #if EITHER(RESTORE_LEVELING_AFTER_G28, ENABLE_LEVELING_AFTER_G28) - #define G28_L0_ENSURES_LEVELING_OFF 1 + #define CAN_SET_LEVELING_AFTER_G28 1 #endif #if ENABLED(PROBE_MANUALLY) @@ -38,7 +38,7 @@ void set_bed_leveling_enabled(const bool enable=true); void reset_bed_level(); #if ENABLED(ENABLE_LEVELING_FADE_HEIGHT) - void set_z_fade_height(const float zfh, const bool do_report=true); + void set_z_fade_height(const_float_t zfh, const bool do_report=true); #endif #if EITHER(MESH_BED_LEVELING, PROBE_MANUALLY) @@ -63,6 +63,9 @@ class TemporaryBedLevelingState { #if ENABLED(AUTO_BED_LEVELING_BILINEAR) #include "abl/abl.h" + #if ENABLED(X_AXIS_TWIST_COMPENSATION) + #include "abl/x_twist.h" + #endif #elif ENABLED(AUTO_BED_LEVELING_UBL) #include "ubl/ubl.h" #elif ENABLED(MESH_BED_LEVELING) diff --git a/Marlin/src/feature/bedlevel/hilbert_curve.cpp b/Marlin/src/feature/bedlevel/hilbert_curve.cpp new file mode 100644 index 000000000000..7474123e3fe1 --- /dev/null +++ b/Marlin/src/feature/bedlevel/hilbert_curve.cpp @@ -0,0 +1,110 @@ +/********************* + * hilbert_curve.cpp * + *********************/ + +/**************************************************************************** + * Written By Marcio Teixeira 2021 - SynDaver Labs, Inc. * + * * + * This program is free software: you can redistribute it and/or modify * + * it under the terms of the GNU General Public License as published by * + * the Free Software Foundation, either version 3 of the License, or * + * (at your option) any later version. * + * * + * This program is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * + * GNU General Public License for more details. * + * * + * To view a copy of the GNU General Public License, go to the following * + * location: . * + ****************************************************************************/ + +#include "../../inc/MarlinConfig.h" + +#if ENABLED(UBL_HILBERT_CURVE) + +#include "bedlevel.h" +#include "hilbert_curve.h" + +constexpr int8_t to_fix(int8_t v) { return v * 2; } +constexpr int8_t to_int(int8_t v) { return v / 2; } +constexpr uint8_t log2(uint8_t n) { return (n > 1) ? 1 + log2(n >> 1) : 0; } +constexpr uint8_t order(uint8_t n) { return uint8_t(log2(n - 1)) + 1; } +constexpr uint8_t ord = order(_MAX(GRID_MAX_POINTS_X, GRID_MAX_POINTS_Y)); +constexpr uint8_t dim = _BV(ord); + +static inline bool eval_candidate(int8_t x, int8_t y, hilbert_curve::callback_ptr func, void *data) { + // The print bed likely has fewer points than the full Hilbert + // curve, so cull unnecessary points + return x < (GRID_MAX_POINTS_X) && y < (GRID_MAX_POINTS_Y) ? func(x, y, data) : false; +} + +bool hilbert_curve::hilbert(int8_t x, int8_t y, int8_t xi, int8_t xj, int8_t yi, int8_t yj, uint8_t n, hilbert_curve::callback_ptr func, void *data) { + /** + * Hilbert space-filling curve implementation + * + * x and y : coordinates of the bottom left corner + * xi and xj : i and j components of the unit x vector of the frame + * yi and yj : i and j components of the unit y vector of the frame + * + * From: http://www.fundza.com/algorithmic/space_filling/hilbert/basics/index.html + */ + if (n) + return hilbert(x, y, yi/2, yj/2, xi/2, xj/2, n-1, func, data) || + hilbert(x+xi/2, y+xj/2, xi/2, xj/2, yi/2, yj/2, n-1, func, data) || + hilbert(x+xi/2+yi/2, y+xj/2+yj/2, xi/2, xj/2, yi/2, yj/2, n-1, func, data) || + hilbert(x+xi/2+yi, y+xj/2+yj, -yi/2, -yj/2, -xi/2, -xj/2, n-1, func, data); + else + return eval_candidate(to_int(x+(xi+yi)/2), to_int(y+(xj+yj)/2), func, data); +} + +/** + * Calls func(x, y, data) for all points in the Hilbert curve. + * If that function returns true, the search is terminated. + */ +bool hilbert_curve::search(hilbert_curve::callback_ptr func, void *data) { + return hilbert(to_fix(0), to_fix(0),to_fix(dim), to_fix(0), to_fix(0), to_fix(dim), ord, func, data); +} + +/* Helper function for starting the search at a particular point */ + +typedef struct { + uint8_t x, y; + bool found_1st; + hilbert_curve::callback_ptr func; + void *data; +} search_from_t; + +static bool search_from_helper(uint8_t x, uint8_t y, void *data) { + search_from_t *d = (search_from_t *) data; + if (d->x == x && d->y == y) + d->found_1st = true; + return d->found_1st ? d->func(x, y, d->data) : false; +} + +/** + * Same as search, except start at a specific grid intersection point. + */ +bool hilbert_curve::search_from(uint8_t x, uint8_t y, hilbert_curve::callback_ptr func, void *data) { + search_from_t d; + d.x = x; + d.y = y; + d.found_1st = false; + d.func = func; + d.data = data; + // Call twice to allow search to wrap back to the beginning and picked up points prior to the start. + return search(search_from_helper, &d) || search(search_from_helper, &d); +} + +/** + * Like search_from, but takes a bed position and starts from the nearest + * point on the Hilbert curve. + */ +bool hilbert_curve::search_from_closest(const xy_pos_t &pos, hilbert_curve::callback_ptr func, void *data) { + // Find closest grid intersection + const uint8_t grid_x = LROUND(constrain(float(pos.x - (MESH_MIN_X)) / (MESH_X_DIST), 0, (GRID_MAX_POINTS_X) - 1)); + const uint8_t grid_y = LROUND(constrain(float(pos.y - (MESH_MIN_Y)) / (MESH_Y_DIST), 0, (GRID_MAX_POINTS_Y) - 1)); + return search_from(grid_x, grid_y, func, data); +} + +#endif // UBL_HILBERT_CURVE diff --git a/Marlin/src/feature/bedlevel/hilbert_curve.h b/Marlin/src/feature/bedlevel/hilbert_curve.h new file mode 100644 index 000000000000..a5dce8a22d60 --- /dev/null +++ b/Marlin/src/feature/bedlevel/hilbert_curve.h @@ -0,0 +1,32 @@ +/******************* + * hilbert_curve.h * + *******************/ + +/**************************************************************************** + * Written By Marcio Teixeira 2021 - SynDaver Labs, Inc. * + * * + * This program is free software: you can redistribute it and/or modify * + * it under the terms of the GNU General Public License as published by * + * the Free Software Foundation, either version 3 of the License, or * + * (at your option) any later version. * + * * + * This program is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * + * GNU General Public License for more details. * + * * + * To view a copy of the GNU General Public License, go to the following * + * location: . * + ****************************************************************************/ + +#pragma once + +class hilbert_curve { + public: + typedef bool (*callback_ptr)(uint8_t x, uint8_t y, void *data); + static bool search(callback_ptr func, void *data); + static bool search_from(uint8_t x, uint8_t y, callback_ptr func, void *data); + static bool search_from_closest(const xy_pos_t &pos, callback_ptr func, void *data); + private: + static bool hilbert(int8_t x, int8_t y, int8_t xi, int8_t xj, int8_t yi, int8_t yj, uint8_t n, callback_ptr func, void *data); +}; diff --git a/Marlin/src/feature/bedlevel/mbl/mesh_bed_leveling.cpp b/Marlin/src/feature/bedlevel/mbl/mesh_bed_leveling.cpp index ec5b95c108b8..51cf28f89005 100644 --- a/Marlin/src/feature/bedlevel/mbl/mesh_bed_leveling.cpp +++ b/Marlin/src/feature/bedlevel/mbl/mesh_bed_leveling.cpp @@ -61,13 +61,13 @@ * Prepare a mesh-leveled linear move in a Cartesian setup, * splitting the move where it crosses mesh borders. */ - void mesh_bed_leveling::line_to_destination(const feedRate_t &scaled_fr_mm_s, uint8_t x_splits, uint8_t y_splits) { + void mesh_bed_leveling::line_to_destination(const_feedRate_t scaled_fr_mm_s, uint8_t x_splits, uint8_t y_splits) { // Get current and destination cells for this line xy_int8_t scel = cell_indexes(current_position), ecel = cell_indexes(destination); - NOMORE(scel.x, GRID_MAX_POINTS_X - 2); - NOMORE(scel.y, GRID_MAX_POINTS_Y - 2); - NOMORE(ecel.x, GRID_MAX_POINTS_X - 2); - NOMORE(ecel.y, GRID_MAX_POINTS_Y - 2); + NOMORE(scel.x, GRID_MAX_CELLS_X - 1); + NOMORE(scel.y, GRID_MAX_CELLS_Y - 1); + NOMORE(ecel.x, GRID_MAX_CELLS_X - 1); + NOMORE(ecel.y, GRID_MAX_CELLS_Y - 1); // Start and end in the same cell? No split needed. if (scel == ecel) { diff --git a/Marlin/src/feature/bedlevel/mbl/mesh_bed_leveling.h b/Marlin/src/feature/bedlevel/mbl/mesh_bed_leveling.h index ade7a931401a..06fae16c21e9 100644 --- a/Marlin/src/feature/bedlevel/mbl/mesh_bed_leveling.h +++ b/Marlin/src/feature/bedlevel/mbl/mesh_bed_leveling.h @@ -32,8 +32,8 @@ enum MeshLevelingState : char { MeshReset // G29 S5 }; -#define MESH_X_DIST (float(MESH_MAX_X - (MESH_MIN_X)) / float(GRID_MAX_POINTS_X - 1)) -#define MESH_Y_DIST (float(MESH_MAX_Y - (MESH_MIN_Y)) / float(GRID_MAX_POINTS_Y - 1)) +#define MESH_X_DIST (float(MESH_MAX_X - (MESH_MIN_X)) / (GRID_MAX_CELLS_X)) +#define MESH_Y_DIST (float(MESH_MAX_Y - (MESH_MIN_Y)) / (GRID_MAX_CELLS_Y)) #define _GET_MESH_X(I) mbl.index_to_xpos[I] #define _GET_MESH_Y(J) mbl.index_to_ypos[J] #define Z_VALUES_ARR mbl.z_values @@ -56,56 +56,54 @@ class mesh_bed_leveling { return false; } - static void set_z(const int8_t px, const int8_t py, const float &z) { z_values[px][py] = z; } + static void set_z(const int8_t px, const int8_t py, const_float_t z) { z_values[px][py] = z; } - static inline void zigzag(const int8_t index, int8_t &px, int8_t &py) { + static void zigzag(const int8_t index, int8_t &px, int8_t &py) { px = index % (GRID_MAX_POINTS_X); py = index / (GRID_MAX_POINTS_X); - if (py & 1) px = (GRID_MAX_POINTS_X - 1) - px; // Zig zag + if (py & 1) px = (GRID_MAX_POINTS_X) - 1 - px; // Zig zag } - static void set_zigzag_z(const int8_t index, const float &z) { + static void set_zigzag_z(const int8_t index, const_float_t z) { int8_t px, py; zigzag(index, px, py); set_z(px, py, z); } - static int8_t cell_index_x(const float &x) { + static int8_t cell_index_x(const_float_t x) { int8_t cx = (x - (MESH_MIN_X)) * RECIPROCAL(MESH_X_DIST); - return constrain(cx, 0, (GRID_MAX_POINTS_X) - 2); + return constrain(cx, 0, GRID_MAX_CELLS_X - 1); } - static int8_t cell_index_y(const float &y) { + static int8_t cell_index_y(const_float_t y) { int8_t cy = (y - (MESH_MIN_Y)) * RECIPROCAL(MESH_Y_DIST); - return constrain(cy, 0, (GRID_MAX_POINTS_Y) - 2); + return constrain(cy, 0, GRID_MAX_CELLS_Y - 1); } - static inline xy_int8_t cell_indexes(const float &x, const float &y) { + static xy_int8_t cell_indexes(const_float_t x, const_float_t y) { return { cell_index_x(x), cell_index_y(y) }; } - static inline xy_int8_t cell_indexes(const xy_pos_t &xy) { return cell_indexes(xy.x, xy.y); } + static xy_int8_t cell_indexes(const xy_pos_t &xy) { return cell_indexes(xy.x, xy.y); } - static int8_t probe_index_x(const float &x) { + static int8_t probe_index_x(const_float_t x) { int8_t px = (x - (MESH_MIN_X) + 0.5f * (MESH_X_DIST)) * RECIPROCAL(MESH_X_DIST); - return WITHIN(px, 0, GRID_MAX_POINTS_X - 1) ? px : -1; + return WITHIN(px, 0, (GRID_MAX_POINTS_X) - 1) ? px : -1; } - static int8_t probe_index_y(const float &y) { + static int8_t probe_index_y(const_float_t y) { int8_t py = (y - (MESH_MIN_Y) + 0.5f * (MESH_Y_DIST)) * RECIPROCAL(MESH_Y_DIST); - return WITHIN(py, 0, GRID_MAX_POINTS_Y - 1) ? py : -1; + return WITHIN(py, 0, (GRID_MAX_POINTS_Y) - 1) ? py : -1; } - static inline xy_int8_t probe_indexes(const float &x, const float &y) { + static xy_int8_t probe_indexes(const_float_t x, const_float_t y) { return { probe_index_x(x), probe_index_y(y) }; } - static inline xy_int8_t probe_indexes(const xy_pos_t &xy) { return probe_indexes(xy.x, xy.y); } + static xy_int8_t probe_indexes(const xy_pos_t &xy) { return probe_indexes(xy.x, xy.y); } - static float calc_z0(const float &a0, const float &a1, const float &z1, const float &a2, const float &z2) { + static float calc_z0(const_float_t a0, const_float_t a1, const_float_t z1, const_float_t a2, const_float_t z2) { const float delta_z = (z2 - z1) / (a2 - a1), delta_a = a0 - a1; return z1 + delta_a * delta_z; } static float get_z(const xy_pos_t &pos - #if ENABLED(ENABLE_LEVELING_FADE_HEIGHT) - , const float &factor=1.0f - #endif + OPTARG(ENABLE_LEVELING_FADE_HEIGHT, const_float_t factor=1.0f) ) { #if DISABLED(ENABLE_LEVELING_FADE_HEIGHT) constexpr float factor = 1.0f; @@ -114,13 +112,14 @@ class mesh_bed_leveling { const float x1 = index_to_xpos[ind.x], x2 = index_to_xpos[ind.x+1], y1 = index_to_xpos[ind.y], y2 = index_to_xpos[ind.y+1], z1 = calc_z0(pos.x, x1, z_values[ind.x][ind.y ], x2, z_values[ind.x+1][ind.y ]), - z2 = calc_z0(pos.x, x1, z_values[ind.x][ind.y+1], x2, z_values[ind.x+1][ind.y+1]); + z2 = calc_z0(pos.x, x1, z_values[ind.x][ind.y+1], x2, z_values[ind.x+1][ind.y+1]), + zf = calc_z0(pos.y, y1, z1, y2, z2); - return z_offset + calc_z0(pos.y, y1, z1, y2, z2) * factor; + return z_offset + zf * factor; } #if IS_CARTESIAN && DISABLED(SEGMENT_LEVELED_MOVES) - static void line_to_destination(const feedRate_t &scaled_fr_mm_s, uint8_t x_splits=0xFF, uint8_t y_splits=0xFF); + static void line_to_destination(const_feedRate_t scaled_fr_mm_s, uint8_t x_splits=0xFF, uint8_t y_splits=0xFF); #endif }; diff --git a/Marlin/src/feature/bedlevel/ubl/ubl.cpp b/Marlin/src/feature/bedlevel/ubl/ubl.cpp index 05b96daefa83..964f1123fe42 100644 --- a/Marlin/src/feature/bedlevel/ubl/ubl.cpp +++ b/Marlin/src/feature/bedlevel/ubl/ubl.cpp @@ -35,6 +35,7 @@ unified_bed_leveling ubl; #include "../../../module/planner.h" #include "../../../module/motion.h" #include "../../../module/probe.h" +#include "../../../module/temperature.h" #if ENABLED(EXTENSIBLE_UI) #include "../../../lcd/extui/ui_api.h" @@ -50,7 +51,7 @@ void unified_bed_leveling::report_current_mesh() { GRID_LOOP(x, y) if (!isnan(z_values[x][y])) { SERIAL_ECHO_START(); - SERIAL_ECHOPAIR(" M421 I", x, " J", y); + SERIAL_ECHOPGM(" M421 I", x, " J", y); SERIAL_ECHOLNPAIR_F_P(SP_Z_STR, z_values[x][y], 4); serial_delay(75); // Prevent Printrun from exploding } @@ -102,7 +103,7 @@ void unified_bed_leveling::invalidate() { set_all_mesh_points_to_value(NAN); } -void unified_bed_leveling::set_all_mesh_points_to_value(const float value) { +void unified_bed_leveling::set_all_mesh_points_to_value(const_float_t value) { GRID_LOOP(x, y) { z_values[x][y] = value; TERN_(EXTENSIBLE_UI, ExtUI::onMeshUpdate(x, y, value)); @@ -115,7 +116,7 @@ void unified_bed_leveling::set_all_mesh_points_to_value(const float value) { constexpr int16_t Z_STEPS_NAN = INT16_MAX; void unified_bed_leveling::set_store_from_mesh(const bed_mesh_t &in_values, mesh_store_t &stored_values) { - auto z_to_store = [](const float &z) { + auto z_to_store = [](const_float_t z) { if (isnan(z)) return Z_STEPS_NAN; const int32_t z_scaled = TRUNC(z * mesh_store_scaling); if (z_scaled == Z_STEPS_NAN || !WITHIN(z_scaled, INT16_MIN, INT16_MAX)) @@ -163,7 +164,7 @@ static void serial_echo_column_labels(const uint8_t sp) { * 2: TODO: Display on Graphical LCD * 4: Compact Human-Readable */ -void unified_bed_leveling::display_map(const int map_type) { +void unified_bed_leveling::display_map(const uint8_t map_type) { const bool was = gcode.set_autoreport_paused(true); constexpr uint8_t eachsp = 1 + 6 + 1, // [-3.567] @@ -179,10 +180,8 @@ void unified_bed_leveling::display_map(const int map_type) { SERIAL_EOL(); serial_echo_column_labels(eachsp - 2); } - else { - SERIAL_ECHOPGM(" for "); - SERIAL_ECHOPGM_P(csv ? PSTR("CSV:\n") : PSTR("LCD:\n")); - } + else + SERIAL_ECHOPGM(" for ", csv ? F("CSV:\n") : F("LCD:\n")); // Add XY probe offset from extruder because probe.probe_at_point() subtracts them when // moving to the XY position to be measured. This ensures better agreement between @@ -190,7 +189,7 @@ void unified_bed_leveling::display_map(const int map_type) { const xy_int8_t curr = closest_indexes(xy_pos_t(current_position) + probe.offset_xy); if (!lcd) SERIAL_EOL(); - for (int8_t j = GRID_MAX_POINTS_Y - 1; j >= 0; j--) { + for (int8_t j = (GRID_MAX_POINTS_Y) - 1; j >= 0; j--) { // Row Label (J index) if (human) { @@ -212,12 +211,12 @@ void unified_bed_leveling::display_map(const int map_type) { // TODO: Display on Graphical LCD } else if (isnan(f)) - SERIAL_ECHOPGM_P(human ? PSTR(" . ") : PSTR("NAN")); + SERIAL_ECHOF(human ? F(" . ") : F("NAN")); else if (human || csv) { if (human && f >= 0.0) SERIAL_CHAR(f > 0 ? '+' : ' '); // Display sign also for positive numbers (' ' for 0) SERIAL_ECHO_F(f, 3); // Positive: 5 digits, Negative: 6 digits } - if (csv && i < GRID_MAX_POINTS_X - 1) SERIAL_CHAR('\t'); + if (csv && i < (GRID_MAX_POINTS_X) - 1) SERIAL_CHAR('\t'); // Closing Brace or Space if (human) SERIAL_CHAR(is_current ? ']' : ' '); @@ -254,4 +253,48 @@ bool unified_bed_leveling::sanity_check() { return !!error_flag; } +#if ENABLED(UBL_MESH_WIZARD) + + /** + * M1004: UBL Mesh Wizard - One-click mesh creation with or without a probe + */ + void GcodeSuite::M1004() { + + #define ALIGN_GCODE TERN(Z_STEPPER_AUTO_ALIGN, "G34", "") + #define PROBE_GCODE TERN(HAS_BED_PROBE, "G29P1\nG29P3", "G29P4R") + + #if HAS_HOTEND + if (parser.seenval('H')) { // Handle H# parameter to set Hotend temp + const celsius_t hotend_temp = parser.value_int(); // Marlin never sends itself F or K, always C + thermalManager.setTargetHotend(hotend_temp, 0); + thermalManager.wait_for_hotend(false); + } + #endif + + #if HAS_HEATED_BED + if (parser.seenval('B')) { // Handle B# parameter to set Bed temp + const celsius_t bed_temp = parser.value_int(); // Marlin never sends itself F or K, always C + thermalManager.setTargetBed(bed_temp); + thermalManager.wait_for_bed(false); + } + #endif + + process_subcommands_now(FPSTR(G28_STR)); // Home + process_subcommands_now(F(ALIGN_GCODE "\n" // Align multi z axis if available + PROBE_GCODE "\n" // Build mesh with available hardware + "G29P3\nG29P3")); // Ensure mesh is complete by running smart fill twice + + if (parser.seenval('S')) { + char umw_gcode[32]; + sprintf_P(umw_gcode, PSTR("G29S%i"), parser.value_int()); + queue.inject(umw_gcode); + } + + process_subcommands_now(F("G29A\nG29F10\n" // Set UBL Active & Fade 10 + "M140S0\nM104S0\n" // Turn off heaters + "M500")); // Store settings + } + +#endif // UBL_MESH_WIZARD + #endif // AUTO_BED_LEVELING_UBL diff --git a/Marlin/src/feature/bedlevel/ubl/ubl.h b/Marlin/src/feature/bedlevel/ubl/ubl.h index d5da43a6a26a..f117c1af65d3 100644 --- a/Marlin/src/feature/bedlevel/ubl/ubl.h +++ b/Marlin/src/feature/bedlevel/ubl/ubl.h @@ -32,52 +32,55 @@ #define UBL_OK false #define UBL_ERR true -enum MeshPointType : char { INVALID, REAL, SET_IN_BITMAP }; +enum MeshPointType : char { INVALID, REAL, SET_IN_BITMAP, CLOSEST }; // External references struct mesh_index_pair; -#define MESH_X_DIST (float(MESH_MAX_X - (MESH_MIN_X)) / float(GRID_MAX_POINTS_X - 1)) -#define MESH_Y_DIST (float(MESH_MAX_Y - (MESH_MIN_Y)) / float(GRID_MAX_POINTS_Y - 1)) +#define MESH_X_DIST (float(MESH_MAX_X - (MESH_MIN_X)) / (GRID_MAX_CELLS_X)) +#define MESH_Y_DIST (float(MESH_MAX_Y - (MESH_MIN_Y)) / (GRID_MAX_CELLS_Y)) #if ENABLED(OPTIMIZED_MESH_STORAGE) typedef int16_t mesh_store_t[GRID_MAX_POINTS_X][GRID_MAX_POINTS_Y]; #endif +typedef struct { + bool C_seen; + int8_t KLS_storage_slot; + uint8_t R_repetition, + V_verbosity, + P_phase, + T_map_type; + float B_shim_thickness, + C_constant; + xy_pos_t XY_pos; + xy_bool_t XY_seen; + #if HAS_BED_PROBE + uint8_t J_grid_size; + #endif +} G29_parameters_t; + class unified_bed_leveling { private: - static int g29_verbose_level, - g29_phase_value, - g29_repetition_cnt, - g29_storage_slot, - g29_map_type; - static bool g29_c_flag; - static float g29_card_thickness, - g29_constant; - static xy_pos_t g29_pos; - static xy_bool_t xy_seen; - - #if HAS_BED_PROBE - static int g29_grid_size; - #endif + static G29_parameters_t param; #if IS_NEWPANEL - static void move_z_with_encoder(const float &multiplier); + static void move_z_with_encoder(const_float_t multiplier); static float measure_point_with_encoder(); static float measure_business_card_thickness(); - static void manually_probe_remaining_mesh(const xy_pos_t&, const float&, const float&, const bool) _O0; + static void manually_probe_remaining_mesh(const xy_pos_t&, const_float_t , const_float_t , const bool) _O0; static void fine_tune_mesh(const xy_pos_t &pos, const bool do_ubl_mesh_map) _O0; #endif - static bool g29_parameter_parsing() _O0; + static bool G29_parse_parameters() _O0; static void shift_mesh_height(); static void probe_entire_mesh(const xy_pos_t &near, const bool do_ubl_mesh_map, const bool stow_probe, const bool do_furthest) _O0; - static void tilt_mesh_based_on_3pts(const float &z1, const float &z2, const float &z3); + static void tilt_mesh_based_on_3pts(const_float_t z1, const_float_t z2, const_float_t z3); static void tilt_mesh_based_on_probed_grid(const bool do_ubl_mesh_map); static bool smart_fill_one(const uint8_t x, const uint8_t y, const int8_t xdir, const int8_t ydir); - static inline bool smart_fill_one(const xy_uint8_t &pos, const xy_uint8_t &dir) { + static bool smart_fill_one(const xy_uint8_t &pos, const xy_uint8_t &dir) { return smart_fill_one(pos.x, pos.y, dir.x, dir.y); } static void smart_fill_mesh(); @@ -95,17 +98,17 @@ class unified_bed_leveling { static void report_state(); static void save_ubl_active_state_and_disable(); static void restore_ubl_active_state_and_leave(); - static void display_map(const int) _O0; + static void display_map(const uint8_t) _O0; static mesh_index_pair find_closest_mesh_point_of_type(const MeshPointType, const xy_pos_t&, const bool=false, MeshFlags *done_flags=nullptr) _O0; static mesh_index_pair find_furthest_invalid_mesh_point() _O0; static void reset(); static void invalidate(); - static void set_all_mesh_points_to_value(const float value); - static void adjust_mesh_to_mean(const bool cflag, const float value); + static void set_all_mesh_points_to_value(const_float_t value); + static void adjust_mesh_to_mean(const bool cflag, const_float_t value); static bool sanity_check(); static void G29() _O0; // O0 for no optimization - static void smart_fill_wlsf(const float &) _O2; // O2 gives smaller code than Os on A2560 + static void smart_fill_wlsf(const_float_t ) _O2; // O2 gives smaller code than Os on A2560 static int8_t storage_slot; @@ -117,57 +120,57 @@ class unified_bed_leveling { static const float _mesh_index_to_xpos[GRID_MAX_POINTS_X], _mesh_index_to_ypos[GRID_MAX_POINTS_Y]; - #if HAS_LCD_MENU + #if HAS_MARLINUI_MENU static bool lcd_map_control; static void steppers_were_disabled(); #else - static inline void steppers_were_disabled() {} + static void steppers_were_disabled() {} #endif - static volatile int16_t encoder_diff; // Volatile because buttons may changed it at interrupt time + static volatile int16_t encoder_diff; // Volatile because buttons may change it at interrupt time unified_bed_leveling(); - FORCE_INLINE static void set_z(const int8_t px, const int8_t py, const float &z) { z_values[px][py] = z; } + FORCE_INLINE static void set_z(const int8_t px, const int8_t py, const_float_t z) { z_values[px][py] = z; } - static int8_t cell_index_x_raw(const float &x) { + static int8_t cell_index_x_raw(const_float_t x) { return FLOOR((x - (MESH_MIN_X)) * RECIPROCAL(MESH_X_DIST)); } - static int8_t cell_index_y_raw(const float &y) { + static int8_t cell_index_y_raw(const_float_t y) { return FLOOR((y - (MESH_MIN_Y)) * RECIPROCAL(MESH_Y_DIST)); } - static int8_t cell_index_x_valid(const float &x) { - return WITHIN(cell_index_x_raw(x), 0, (GRID_MAX_POINTS_X - 2)); + static int8_t cell_index_x_valid(const_float_t x) { + return WITHIN(cell_index_x_raw(x), 0, GRID_MAX_CELLS_X - 1); } - static int8_t cell_index_y_valid(const float &y) { - return WITHIN(cell_index_y_raw(y), 0, (GRID_MAX_POINTS_Y - 2)); + static int8_t cell_index_y_valid(const_float_t y) { + return WITHIN(cell_index_y_raw(y), 0, GRID_MAX_CELLS_Y - 1); } - static int8_t cell_index_x(const float &x) { - return constrain(cell_index_x_raw(x), 0, (GRID_MAX_POINTS_X) - 2); + static int8_t cell_index_x(const_float_t x) { + return constrain(cell_index_x_raw(x), 0, GRID_MAX_CELLS_X - 1); } - static int8_t cell_index_y(const float &y) { - return constrain(cell_index_y_raw(y), 0, (GRID_MAX_POINTS_Y) - 2); + static int8_t cell_index_y(const_float_t y) { + return constrain(cell_index_y_raw(y), 0, GRID_MAX_CELLS_Y - 1); } - static inline xy_int8_t cell_indexes(const float &x, const float &y) { + static xy_int8_t cell_indexes(const_float_t x, const_float_t y) { return { cell_index_x(x), cell_index_y(y) }; } - static inline xy_int8_t cell_indexes(const xy_pos_t &xy) { return cell_indexes(xy.x, xy.y); } + static xy_int8_t cell_indexes(const xy_pos_t &xy) { return cell_indexes(xy.x, xy.y); } - static int8_t closest_x_index(const float &x) { + static int8_t closest_x_index(const_float_t x) { const int8_t px = (x - (MESH_MIN_X) + (MESH_X_DIST) * 0.5) * RECIPROCAL(MESH_X_DIST); - return WITHIN(px, 0, GRID_MAX_POINTS_X - 1) ? px : -1; + return WITHIN(px, 0, (GRID_MAX_POINTS_X) - 1) ? px : -1; } - static int8_t closest_y_index(const float &y) { + static int8_t closest_y_index(const_float_t y) { const int8_t py = (y - (MESH_MIN_Y) + (MESH_Y_DIST) * 0.5) * RECIPROCAL(MESH_Y_DIST); - return WITHIN(py, 0, GRID_MAX_POINTS_Y - 1) ? py : -1; + return WITHIN(py, 0, (GRID_MAX_POINTS_Y) - 1) ? py : -1; } - static inline xy_int8_t closest_indexes(const xy_pos_t &xy) { + static xy_int8_t closest_indexes(const xy_pos_t &xy) { return { closest_x_index(xy.x), closest_y_index(xy.y) }; } @@ -186,7 +189,7 @@ class unified_bed_leveling { * It is fairly expensive with its 4 floating point additions and 2 floating point * multiplications. */ - FORCE_INLINE static float calc_z0(const float &a0, const float &a1, const float &z1, const float &a2, const float &z2) { + FORCE_INLINE static float calc_z0(const_float_t a0, const_float_t a1, const_float_t z1, const_float_t a2, const_float_t z2) { return z1 + (z2 - z1) * (a0 - a1) / (a2 - a1); } @@ -200,12 +203,12 @@ class unified_bed_leveling { * z_correction_for_x_on_horizontal_mesh_line is an optimization for * the case where the printer is making a vertical line that only crosses horizontal mesh lines. */ - static inline float z_correction_for_x_on_horizontal_mesh_line(const float &rx0, const int x1_i, const int yi) { - if (!WITHIN(x1_i, 0, GRID_MAX_POINTS_X - 1) || !WITHIN(yi, 0, GRID_MAX_POINTS_Y - 1)) { + static float z_correction_for_x_on_horizontal_mesh_line(const_float_t rx0, const int x1_i, const int yi) { + if (!WITHIN(x1_i, 0, (GRID_MAX_POINTS_X) - 1) || !WITHIN(yi, 0, (GRID_MAX_POINTS_Y) - 1)) { if (DEBUGGING(LEVELING)) { - if (WITHIN(x1_i, 0, GRID_MAX_POINTS_X - 1)) DEBUG_ECHOPGM("yi"); else DEBUG_ECHOPGM("x1_i"); - DEBUG_ECHOLNPAIR(" out of bounds in z_correction_for_x_on_horizontal_mesh_line(rx0=", rx0, ",x1_i=", x1_i, ",yi=", yi, ")"); + if (WITHIN(x1_i, 0, (GRID_MAX_POINTS_X) - 1)) DEBUG_ECHOPGM("yi"); else DEBUG_ECHOPGM("x1_i"); + DEBUG_ECHOLNPGM(" out of bounds in z_correction_for_x_on_horizontal_mesh_line(rx0=", rx0, ",x1_i=", x1_i, ",yi=", yi, ")"); } // The requested location is off the mesh. Return UBL_Z_RAISE_WHEN_OFF_MESH or NAN. @@ -215,20 +218,20 @@ class unified_bed_leveling { const float xratio = (rx0 - mesh_index_to_xpos(x1_i)) * RECIPROCAL(MESH_X_DIST), z1 = z_values[x1_i][yi]; - return z1 + xratio * (z_values[_MIN(x1_i, GRID_MAX_POINTS_X - 2) + 1][yi] - z1); // Don't allow x1_i+1 to be past the end of the array - // If it is, it is clamped to the last element of the - // z_values[][] array and no correction is applied. + return z1 + xratio * (z_values[_MIN(x1_i, (GRID_MAX_POINTS_X) - 2) + 1][yi] - z1); // Don't allow x1_i+1 to be past the end of the array + // If it is, it is clamped to the last element of the + // z_values[][] array and no correction is applied. } // // See comments above for z_correction_for_x_on_horizontal_mesh_line // - static inline float z_correction_for_y_on_vertical_mesh_line(const float &ry0, const int xi, const int y1_i) { - if (!WITHIN(xi, 0, GRID_MAX_POINTS_X - 1) || !WITHIN(y1_i, 0, GRID_MAX_POINTS_Y - 1)) { + static float z_correction_for_y_on_vertical_mesh_line(const_float_t ry0, const int xi, const int y1_i) { + if (!WITHIN(xi, 0, (GRID_MAX_POINTS_X) - 1) || !WITHIN(y1_i, 0, (GRID_MAX_POINTS_Y) - 1)) { if (DEBUGGING(LEVELING)) { - if (WITHIN(xi, 0, GRID_MAX_POINTS_X - 1)) DEBUG_ECHOPGM("y1_i"); else DEBUG_ECHOPGM("xi"); - DEBUG_ECHOLNPAIR(" out of bounds in z_correction_for_y_on_vertical_mesh_line(ry0=", ry0, ", xi=", xi, ", y1_i=", y1_i, ")"); + if (WITHIN(xi, 0, (GRID_MAX_POINTS_X) - 1)) DEBUG_ECHOPGM("y1_i"); else DEBUG_ECHOPGM("xi"); + DEBUG_ECHOLNPGM(" out of bounds in z_correction_for_y_on_vertical_mesh_line(ry0=", ry0, ", xi=", xi, ", y1_i=", y1_i, ")"); } // The requested location is off the mesh. Return UBL_Z_RAISE_WHEN_OFF_MESH or NAN. @@ -238,9 +241,9 @@ class unified_bed_leveling { const float yratio = (ry0 - mesh_index_to_ypos(y1_i)) * RECIPROCAL(MESH_Y_DIST), z1 = z_values[xi][y1_i]; - return z1 + yratio * (z_values[xi][_MIN(y1_i, GRID_MAX_POINTS_Y - 2) + 1] - z1); // Don't allow y1_i+1 to be past the end of the array - // If it is, it is clamped to the last element of the - // z_values[][] array and no correction is applied. + return z1 + yratio * (z_values[xi][_MIN(y1_i, (GRID_MAX_POINTS_Y) - 2) + 1] - z1); // Don't allow y1_i+1 to be past the end of the array + // If it is, it is clamped to the last element of the + // z_values[][] array and no correction is applied. } /** @@ -249,7 +252,7 @@ class unified_bed_leveling { * Z-Height at both ends. Then it does a linear interpolation of these heights based * on the Y position within the cell. */ - static float get_z_correction(const float &rx0, const float &ry0) { + static float get_z_correction(const_float_t rx0, const_float_t ry0) { const int8_t cx = cell_index_x(rx0), cy = cell_index_y(ry0); // return values are clamped /** @@ -261,24 +264,10 @@ class unified_bed_leveling { return UBL_Z_RAISE_WHEN_OFF_MESH; #endif - const float z1 = calc_z0(rx0, - mesh_index_to_xpos(cx), z_values[cx][cy], - mesh_index_to_xpos(cx + 1), z_values[_MIN(cx, GRID_MAX_POINTS_X - 2) + 1][cy]); - - const float z2 = calc_z0(rx0, - mesh_index_to_xpos(cx), z_values[cx][_MIN(cy, GRID_MAX_POINTS_Y - 2) + 1], - mesh_index_to_xpos(cx + 1), z_values[_MIN(cx, GRID_MAX_POINTS_X - 2) + 1][_MIN(cy, GRID_MAX_POINTS_Y - 2) + 1]); - - float z0 = calc_z0(ry0, - mesh_index_to_ypos(cy), z1, - mesh_index_to_ypos(cy + 1), z2); - - if (DEBUGGING(MESH_ADJUST)) { - DEBUG_ECHOPAIR(" raw get_z_correction(", rx0); - DEBUG_CHAR(','); DEBUG_ECHO(ry0); - DEBUG_ECHOPAIR_F(") = ", z0, 6); - DEBUG_ECHOLNPAIR_F(" >>>---> ", z0, 6); - } + const uint8_t mx = _MIN(cx, (GRID_MAX_POINTS_X) - 2) + 1, my = _MIN(cy, (GRID_MAX_POINTS_Y) - 2) + 1; + const float z1 = calc_z0(rx0, mesh_index_to_xpos(cx), z_values[cx][cy], mesh_index_to_xpos(cx + 1), z_values[mx][cy]); + const float z2 = calc_z0(rx0, mesh_index_to_xpos(cx), z_values[cx][my], mesh_index_to_xpos(cx + 1), z_values[mx][my]); + float z0 = calc_z0(ry0, mesh_index_to_ypos(cy), z1, mesh_index_to_ypos(cy + 1), z2); if (isnan(z0)) { // if part of the Mesh is undefined, it will show up as NAN z0 = 0.0; // in ubl.z_values[][] and propagate through the @@ -286,32 +275,32 @@ class unified_bed_leveling { // because part of the Mesh is undefined and we don't have the // information we need to complete the height correction. - if (DEBUGGING(MESH_ADJUST)) { - DEBUG_ECHOPAIR("??? Yikes! NAN in get_z_correction(", rx0); - DEBUG_CHAR(','); - DEBUG_ECHO(ry0); - DEBUG_CHAR(')'); - DEBUG_EOL(); - } + if (DEBUGGING(MESH_ADJUST)) DEBUG_ECHOLNPGM("??? Yikes! NAN in "); } + + if (DEBUGGING(MESH_ADJUST)) { + DEBUG_ECHOPGM("get_z_correction(", rx0, ", ", ry0); + DEBUG_ECHOLNPAIR_F(") => ", z0, 6); + } + return z0; } - static inline float get_z_correction(const xy_pos_t &pos) { return get_z_correction(pos.x, pos.y); } + static float get_z_correction(const xy_pos_t &pos) { return get_z_correction(pos.x, pos.y); } - static inline float mesh_index_to_xpos(const uint8_t i) { - return i < GRID_MAX_POINTS_X ? pgm_read_float(&_mesh_index_to_xpos[i]) : MESH_MIN_X + i * (MESH_X_DIST); + static float mesh_index_to_xpos(const uint8_t i) { + return i < (GRID_MAX_POINTS_X) ? pgm_read_float(&_mesh_index_to_xpos[i]) : MESH_MIN_X + i * (MESH_X_DIST); } - static inline float mesh_index_to_ypos(const uint8_t i) { - return i < GRID_MAX_POINTS_Y ? pgm_read_float(&_mesh_index_to_ypos[i]) : MESH_MIN_Y + i * (MESH_Y_DIST); + static float mesh_index_to_ypos(const uint8_t i) { + return i < (GRID_MAX_POINTS_Y) ? pgm_read_float(&_mesh_index_to_ypos[i]) : MESH_MIN_Y + i * (MESH_Y_DIST); } #if UBL_SEGMENTED - static bool line_to_destination_segmented(const feedRate_t &scaled_fr_mm_s); + static bool line_to_destination_segmented(const_feedRate_t scaled_fr_mm_s); #else - static void line_to_destination_cartesian(const feedRate_t &scaled_fr_mm_s, const uint8_t e); + static void line_to_destination_cartesian(const_feedRate_t scaled_fr_mm_s, const uint8_t e); #endif - static inline bool mesh_is_valid() { + static bool mesh_is_valid() { GRID_LOOP(x, y) if (isnan(z_values[x][y])) return false; return true; } diff --git a/Marlin/src/feature/bedlevel/ubl/ubl_G29.cpp b/Marlin/src/feature/bedlevel/ubl/ubl_G29.cpp index 8c70feb6616a..023d0c5b1d63 100644 --- a/Marlin/src/feature/bedlevel/ubl/ubl_G29.cpp +++ b/Marlin/src/feature/bedlevel/ubl/ubl_G29.cpp @@ -49,11 +49,15 @@ #include "../../../lcd/extui/ui_api.h" #endif +#if ENABLED(UBL_HILBERT_CURVE) + #include "../hilbert_curve.h" +#endif + #include #define UBL_G29_P31 -#if HAS_LCD_MENU +#if HAS_MARLINUI_MENU bool unified_bed_leveling::lcd_map_control = false; @@ -71,21 +75,6 @@ #define SIZE_OF_LITTLE_RAISE 1 #define BIG_RAISE_NOT_NEEDED 0 -int unified_bed_leveling::g29_verbose_level, - unified_bed_leveling::g29_phase_value, - unified_bed_leveling::g29_repetition_cnt, - unified_bed_leveling::g29_storage_slot = 0, - unified_bed_leveling::g29_map_type; -bool unified_bed_leveling::g29_c_flag; -float unified_bed_leveling::g29_card_thickness = 0, - unified_bed_leveling::g29_constant = 0; -xy_bool_t unified_bed_leveling::xy_seen; -xy_pos_t unified_bed_leveling::g29_pos; - -#if HAS_BED_PROBE - int unified_bed_leveling::g29_grid_size; -#endif - /** * G29: Unified Bed Leveling by Roxy * @@ -309,66 +298,63 @@ xy_pos_t unified_bed_leveling::g29_pos; * features of all three systems combined. */ +G29_parameters_t unified_bed_leveling::param; + void unified_bed_leveling::G29() { bool probe_deployed = false; - if (g29_parameter_parsing()) return; // Abort on parameter error + if (G29_parse_parameters()) return; // Abort on parameter error - const int8_t p_val = parser.intval('P', -1); - const bool may_move = p_val == 1 || p_val == 2 || p_val == 4 || parser.seen('J'); - TERN_(HAS_MULTI_HOTEND, const uint8_t old_tool_index = active_extruder); + const uint8_t p_val = parser.byteval('P'); + const bool may_move = p_val == 1 || p_val == 2 || p_val == 4 || parser.seen_test('J'); + #if HAS_MULTI_HOTEND + const uint8_t old_tool_index = active_extruder; + #endif // Check for commands that require the printer to be homed if (may_move) { planner.synchronize(); // Send 'N' to force homing before G29 (internal only) - if (axes_should_home() || parser.seen('N')) gcode.home_all_axes(); - TERN_(HAS_MULTI_HOTEND, if (active_extruder) tool_change(0)); + if (axes_should_home() || parser.seen_test('N')) gcode.home_all_axes(); + TERN_(HAS_MULTI_HOTEND, if (active_extruder != 0) tool_change(0, true)); } - // Invalidate Mesh Points. This command is a little bit asymmetrical because - // it directly specifies the repetition count and does not use the 'R' parameter. + // Invalidate one or more nearby mesh points, possibly all. if (parser.seen('I')) { - uint8_t cnt = 0; - g29_repetition_cnt = parser.has_value() ? parser.value_int() : 1; - if (g29_repetition_cnt >= GRID_MAX_POINTS) { - set_all_mesh_points_to_value(NAN); - } - else { - while (g29_repetition_cnt--) { - if (cnt > 20) { cnt = 0; idle(); } - const mesh_index_pair closest = find_closest_mesh_point_of_type(REAL, g29_pos); - const xy_int8_t &cpos = closest.pos; - if (cpos.x < 0) { - // No more REAL mesh points to invalidate, so we ASSUME the user - // meant to invalidate the ENTIRE mesh, which cannot be done with - // find_closest_mesh_point loop which only returns REAL points. - set_all_mesh_points_to_value(NAN); - SERIAL_ECHOLNPGM("Entire Mesh invalidated.\n"); - break; // No more invalid Mesh Points to populate - } - z_values[cpos.x][cpos.y] = NAN; - TERN_(EXTENSIBLE_UI, ExtUI::onMeshUpdate(cpos, 0.0f)); - cnt++; + uint8_t count = parser.has_value() ? parser.value_byte() : 1; + bool invalidate_all = count >= GRID_MAX_POINTS; + if (!invalidate_all) { + while (count--) { + if ((count & 0x0F) == 0x0F) idle(); + const mesh_index_pair closest = find_closest_mesh_point_of_type(REAL, param.XY_pos); + // No more REAL mesh points to invalidate? Assume the user meant + // to invalidate the ENTIRE mesh, which can't be done with + // find_closest_mesh_point (which only returns REAL points). + if (closest.pos.x < 0) { invalidate_all = true; break; } + z_values[closest.pos.x][closest.pos.y] = NAN; + TERN_(EXTENSIBLE_UI, ExtUI::onMeshUpdate(closest.pos, 0.0f)); } } - SERIAL_ECHOLNPGM("Locations invalidated.\n"); + if (invalidate_all) { + invalidate(); + SERIAL_ECHOPGM("Entire Mesh"); + } + else + SERIAL_ECHOPGM("Locations"); + SERIAL_ECHOLNPGM(" invalidated.\n"); } if (parser.seen('Q')) { - const int test_pattern = parser.has_value() ? parser.value_int() : -99; - if (!WITHIN(test_pattern, -1, 2)) { - SERIAL_ECHOLNPGM("Invalid test_pattern value. (-1 to 2)\n"); + const int16_t test_pattern = parser.has_value() ? parser.value_int() : -99; + if (!WITHIN(test_pattern, TERN0(UBL_DEVEL_DEBUGGING, -1), 2)) { + SERIAL_ECHOLNPGM("?Invalid (Q) test pattern. (" TERN(UBL_DEVEL_DEBUGGING, "-1", "0") " to 2)\n"); return; } - SERIAL_ECHOLNPGM("Loading test_pattern values.\n"); + SERIAL_ECHOLNPGM("Applying test pattern.\n"); switch (test_pattern) { - #if ENABLED(UBL_DEVEL_DEBUGGING) - case -1: - g29_eeprom_dump(); - break; - #endif + default: + case -1: TERN_(UBL_DEVEL_DEBUGGING, g29_eeprom_dump()); break; case 0: GRID_LOOP(x, y) { // Create a bowl shape similar to a poorly-calibrated Delta @@ -395,7 +381,7 @@ void unified_bed_leveling::G29() { // Allow the user to specify the height because 10mm is a little extreme in some cases. for (uint8_t x = (GRID_MAX_POINTS_X) / 3; x < 2 * (GRID_MAX_POINTS_X) / 3; x++) // Create a rectangular raised area in for (uint8_t y = (GRID_MAX_POINTS_Y) / 3; y < 2 * (GRID_MAX_POINTS_Y) / 3; y++) { // the center of the bed - z_values[x][y] += parser.seen('C') ? g29_constant : 9.99f; + z_values[x][y] += parser.seen_test('C') ? param.C_constant : 9.99f; TERN_(EXTENSIBLE_UI, ExtUI::onMeshUpdate(x, y, z_values[x][y])); } break; @@ -404,9 +390,9 @@ void unified_bed_leveling::G29() { #if HAS_BED_PROBE - if (parser.seen('J')) { + if (parser.seen_test('J')) { save_ubl_active_state_and_disable(); - tilt_mesh_based_on_probed_grid(g29_grid_size == 0); // Zero size does 3-Point + tilt_mesh_based_on_probed_grid(param.J_grid_size == 0); // Zero size does 3-Point restore_ubl_active_state_and_leave(); #if ENABLED(UBL_G29_J_RECENTER) do_blocking_move_to_xy(0.5f * ((MESH_MIN_X) + (MESH_MAX_X)), 0.5f * ((MESH_MIN_Y) + (MESH_MAX_Y))); @@ -417,13 +403,13 @@ void unified_bed_leveling::G29() { #endif // HAS_BED_PROBE - if (parser.seen('P')) { - if (WITHIN(g29_phase_value, 0, 1) && storage_slot == -1) { + if (parser.seen_test('P')) { + if (WITHIN(param.P_phase, 0, 1) && storage_slot == -1) { storage_slot = 0; SERIAL_ECHOLNPGM("Default storage slot 0 selected."); } - switch (g29_phase_value) { + switch (param.P_phase) { case 0: // // Zero Mesh Data @@ -438,18 +424,17 @@ void unified_bed_leveling::G29() { // // Invalidate Entire Mesh and Automatically Probe Mesh in areas that can be reached by the probe // - if (!parser.seen('C')) { + if (!parser.seen_test('C')) { invalidate(); SERIAL_ECHOLNPGM("Mesh invalidated. Probing mesh."); } - if (g29_verbose_level > 1) { - SERIAL_ECHOPAIR("Probing around (", g29_pos.x); + if (param.V_verbosity > 1) { + SERIAL_ECHOPGM("Probing around (", param.XY_pos.x); SERIAL_CHAR(','); - SERIAL_DECIMAL(g29_pos.y); + SERIAL_DECIMAL(param.XY_pos.y); SERIAL_ECHOLNPGM(").\n"); } - const xy_pos_t near_probe_xy = g29_pos + probe.offset_xy; - probe_entire_mesh(near_probe_xy, parser.seen('T'), parser.seen('E'), parser.seen('U')); + probe_entire_mesh(param.XY_pos, parser.seen_test('T'), parser.seen_test('E'), parser.seen_test('U')); report_current_position(); probe_deployed = true; @@ -458,14 +443,14 @@ void unified_bed_leveling::G29() { #endif // HAS_BED_PROBE case 2: { - #if HAS_LCD_MENU + #if HAS_MARLINUI_MENU // // Manually Probe Mesh in areas that can't be reached by the probe // SERIAL_ECHOLNPGM("Manually probing unreachable points."); do_z_clearance(Z_CLEARANCE_BETWEEN_PROBES); - if (parser.seen('C') && !xy_seen) { + if (parser.seen_test('C') && !param.XY_seen) { /** * Use a good default location for the path. @@ -474,7 +459,7 @@ void unified_bed_leveling::G29() { * It may make sense to have Delta printers default to the center of the bed. * Until that is decided, this can be forced with the X and Y parameters. */ - g29_pos.set( + param.XY_pos.set( #if IS_KINEMATIC X_HOME_POS, Y_HOME_POS #else @@ -485,21 +470,21 @@ void unified_bed_leveling::G29() { } if (parser.seen('B')) { - g29_card_thickness = parser.has_value() ? parser.value_float() : measure_business_card_thickness(); - if (ABS(g29_card_thickness) > 1.5f) { + param.B_shim_thickness = parser.has_value() ? parser.value_float() : measure_business_card_thickness(); + if (ABS(param.B_shim_thickness) > 1.5f) { SERIAL_ECHOLNPGM("?Error in Business Card measurement."); return; } probe_deployed = true; } - if (!position_is_reachable(g29_pos)) { + if (!position_is_reachable(param.XY_pos)) { SERIAL_ECHOLNPGM("XY outside printable radius."); return; } const float height = parser.floatval('H', Z_CLEARANCE_BETWEEN_PROBES); - manually_probe_remaining_mesh(g29_pos, height, g29_card_thickness, parser.seen('T')); + manually_probe_remaining_mesh(param.XY_pos, height, param.B_shim_thickness, parser.seen_test('T')); SERIAL_ECHOLNPGM("G29 P2 finished."); @@ -521,23 +506,23 @@ void unified_bed_leveling::G29() { * - Allow 'G29 P3' to choose a 'reasonable' constant. */ - if (g29_c_flag) { - if (g29_repetition_cnt >= GRID_MAX_POINTS) { - set_all_mesh_points_to_value(g29_constant); + if (param.C_seen) { + if (param.R_repetition >= GRID_MAX_POINTS) { + set_all_mesh_points_to_value(param.C_constant); } else { - while (g29_repetition_cnt--) { // this only populates reachable mesh points near - const mesh_index_pair closest = find_closest_mesh_point_of_type(INVALID, g29_pos); + while (param.R_repetition--) { // this only populates reachable mesh points near + const mesh_index_pair closest = find_closest_mesh_point_of_type(INVALID, param.XY_pos); const xy_int8_t &cpos = closest.pos; if (cpos.x < 0) { // No more REAL INVALID mesh points to populate, so we ASSUME // user meant to populate ALL INVALID mesh points to value - GRID_LOOP(x, y) if (isnan(z_values[x][y])) z_values[x][y] = g29_constant; + GRID_LOOP(x, y) if (isnan(z_values[x][y])) z_values[x][y] = param.C_constant; break; // No more invalid Mesh Points to populate } else { - z_values[cpos.x][cpos.y] = g29_constant; - TERN_(EXTENSIBLE_UI, ExtUI::onMeshUpdate(cpos, g29_constant)); + z_values[cpos.x][cpos.y] = param.C_constant; + TERN_(EXTENSIBLE_UI, ExtUI::onMeshUpdate(cpos, param.C_constant)); } } } @@ -570,15 +555,15 @@ void unified_bed_leveling::G29() { } case 4: // Fine Tune (i.e., Edit) the Mesh - #if HAS_LCD_MENU - fine_tune_mesh(g29_pos, parser.seen('T')); + #if HAS_MARLINUI_MENU + fine_tune_mesh(param.XY_pos, parser.seen_test('T')); #else SERIAL_ECHOLNPGM("?P4 is only available when an LCD is present."); return; #endif break; - case 5: adjust_mesh_to_mean(g29_c_flag, g29_constant); break; + case 5: adjust_mesh_to_mean(param.C_seen, param.C_constant); break; case 6: shift_mesh_height(); break; } @@ -590,7 +575,7 @@ void unified_bed_leveling::G29() { // Much of the 'What?' command can be eliminated. But until we are fully debugged, it is // good to have the extra information. Soon... we prune this to just a few items // - if (parser.seen('W')) g29_what_command(); + if (parser.seen_test('W')) g29_what_command(); // // When we are fully debugged, this may go away. But there are some valid @@ -608,7 +593,7 @@ void unified_bed_leveling::G29() { // if (parser.seen('L')) { // Load Current Mesh Data - g29_storage_slot = parser.has_value() ? parser.value_int() : storage_slot; + param.KLS_storage_slot = parser.has_value() ? (int8_t)parser.value_int() : storage_slot; int16_t a = settings.calc_num_meshes(); @@ -617,15 +602,15 @@ void unified_bed_leveling::G29() { return; } - if (!WITHIN(g29_storage_slot, 0, a - 1)) { - SERIAL_ECHOLNPAIR("?Invalid storage slot.\n?Use 0 to ", a - 1); + if (!WITHIN(param.KLS_storage_slot, 0, a - 1)) { + SERIAL_ECHOLNPGM("?Invalid storage slot.\n?Use 0 to ", a - 1); return; } - settings.load_mesh(g29_storage_slot); - storage_slot = g29_storage_slot; + settings.load_mesh(param.KLS_storage_slot); + storage_slot = param.KLS_storage_slot; - SERIAL_ECHOLNPGM("Done."); + SERIAL_ECHOLNPGM(STR_DONE); } // @@ -633,10 +618,10 @@ void unified_bed_leveling::G29() { // if (parser.seen('S')) { // Store (or Save) Current Mesh Data - g29_storage_slot = parser.has_value() ? parser.value_int() : storage_slot; + param.KLS_storage_slot = parser.has_value() ? (int8_t)parser.value_int() : storage_slot; - if (g29_storage_slot == -1) // Special case, the user wants to 'Export' the mesh to the - return report_current_mesh(); // host program to be saved on the user's computer + if (param.KLS_storage_slot == -1) // Special case: 'Export' the mesh to the + return report_current_mesh(); // host so it can be saved in a file. int16_t a = settings.calc_num_meshes(); @@ -645,23 +630,23 @@ void unified_bed_leveling::G29() { goto LEAVE; } - if (!WITHIN(g29_storage_slot, 0, a - 1)) { - SERIAL_ECHOLNPAIR("?Invalid storage slot.\n?Use 0 to ", a - 1); + if (!WITHIN(param.KLS_storage_slot, 0, a - 1)) { + SERIAL_ECHOLNPGM("?Invalid storage slot.\n?Use 0 to ", a - 1); goto LEAVE; } - settings.store_mesh(g29_storage_slot); - storage_slot = g29_storage_slot; + settings.store_mesh(param.KLS_storage_slot); + storage_slot = param.KLS_storage_slot; - SERIAL_ECHOLNPGM("Done."); + SERIAL_ECHOLNPGM(STR_DONE); } - if (parser.seen('T')) - display_map(g29_map_type); + if (parser.seen_test('T')) + display_map(param.T_map_type); LEAVE: - #if HAS_LCD_MENU + #if HAS_MARLINUI_MENU ui.reset_alert_level(); ui.quick_feedback(); ui.reset_status(); @@ -669,22 +654,27 @@ void unified_bed_leveling::G29() { #endif #ifdef Z_PROBE_END_SCRIPT - if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPAIR("Z Probe End Script: ", Z_PROBE_END_SCRIPT); + if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPGM("Z Probe End Script: ", Z_PROBE_END_SCRIPT); if (probe_deployed) { planner.synchronize(); - gcode.process_subcommands_now_P(PSTR(Z_PROBE_END_SCRIPT)); + gcode.process_subcommands_now(F(Z_PROBE_END_SCRIPT)); } #else UNUSED(probe_deployed); #endif - TERN_(HAS_MULTI_HOTEND, tool_change(old_tool_index)); + TERN_(HAS_MULTI_HOTEND, if (old_tool_index != 0) tool_change(old_tool_index)); return; } -void unified_bed_leveling::adjust_mesh_to_mean(const bool cflag, const float value) { +/** + * M420 C + * G29 P5 C : Adjust Mesh To Mean (and subtract the given offset). + * Find the mean average and shift the mesh to center on that value. + */ +void unified_bed_leveling::adjust_mesh_to_mean(const bool cflag, const_float_t offset) { float sum = 0; - int n = 0; + uint8_t n = 0; GRID_LOOP(x, y) if (!isnan(z_values[x][y])) { sum += z_values[x][y]; @@ -701,7 +691,7 @@ void unified_bed_leveling::adjust_mesh_to_mean(const bool cflag, const float val if (!isnan(z_values[x][y])) sum_of_diff_squared += sq(z_values[x][y] - mean); - SERIAL_ECHOLNPAIR("# of samples: ", n); + SERIAL_ECHOLNPGM("# of samples: ", n); SERIAL_ECHOLNPAIR_F("Mean Mesh Height: ", mean, 6); const float sigma = SQRT(sum_of_diff_squared / (n + 1)); @@ -710,42 +700,46 @@ void unified_bed_leveling::adjust_mesh_to_mean(const bool cflag, const float val if (cflag) GRID_LOOP(x, y) if (!isnan(z_values[x][y])) { - z_values[x][y] -= mean + value; + z_values[x][y] -= mean + offset; TERN_(EXTENSIBLE_UI, ExtUI::onMeshUpdate(x, y, z_values[x][y])); } } +/** + * G29 P6 C : Shift Mesh Height by a uniform constant. + */ void unified_bed_leveling::shift_mesh_height() { GRID_LOOP(x, y) if (!isnan(z_values[x][y])) { - z_values[x][y] += g29_constant; + z_values[x][y] += param.C_constant; TERN_(EXTENSIBLE_UI, ExtUI::onMeshUpdate(x, y, z_values[x][y])); } } #if HAS_BED_PROBE /** - * Probe all invalidated locations of the mesh that can be reached by the probe. - * This attempts to fill in locations closest to the nozzle's start location first. + * G29 P1 T V : Probe Entire Mesh + * Probe all invalidated locations of the mesh that can be reached by the probe. + * This attempts to fill in locations closest to the nozzle's start location first. */ void unified_bed_leveling::probe_entire_mesh(const xy_pos_t &nearby, const bool do_ubl_mesh_map, const bool stow_probe, const bool do_furthest) { probe.deploy(); // Deploy before ui.capture() to allow for PAUSE_BEFORE_DEPLOY_STOW - TERN_(HAS_LCD_MENU, ui.capture()); + TERN_(HAS_MARLINUI_MENU, ui.capture()); save_ubl_active_state_and_disable(); // No bed level correction so only raw data is obtained uint8_t count = GRID_MAX_POINTS; mesh_index_pair best; - TERN_(EXTENSIBLE_UI, ExtUI::onMeshUpdate(best.pos, ExtUI::MESH_START)); + TERN_(EXTENSIBLE_UI, ExtUI::onMeshUpdate(best.pos, ExtUI::G29_START)); do { - if (do_ubl_mesh_map) display_map(g29_map_type); + if (do_ubl_mesh_map) display_map(param.T_map_type); - const int point_num = (GRID_MAX_POINTS) - count + 1; - SERIAL_ECHOLNPAIR("Probing mesh point ", point_num, "/", GRID_MAX_POINTS, "."); - TERN_(HAS_DISPLAY, ui.status_printf_P(0, PSTR(S_FMT " %i/%i"), GET_TEXT(MSG_PROBING_MESH), point_num, int(GRID_MAX_POINTS))); + const uint8_t point_num = (GRID_MAX_POINTS - count) + 1; + SERIAL_ECHOLNPGM("Probing mesh point ", point_num, "/", GRID_MAX_POINTS, "."); + TERN_(HAS_STATUS_MESSAGE, ui.status_printf(0, F(S_FMT " %i/%i"), GET_TEXT(MSG_PROBING_POINT), point_num, int(GRID_MAX_POINTS))); - #if HAS_LCD_MENU + #if HAS_MARLINUI_MENU if (ui.button_pressed()) { ui.quick_feedback(false); // Preserve button state for click-and-hold SERIAL_ECHOLNPGM("\nMesh only partially populated.\n"); @@ -762,14 +756,14 @@ void unified_bed_leveling::shift_mesh_height() { : find_closest_mesh_point_of_type(INVALID, nearby, true); if (best.pos.x >= 0) { // mesh point found and is reachable by probe - TERN_(EXTENSIBLE_UI, ExtUI::onMeshUpdate(best.pos, ExtUI::PROBE_START)); + TERN_(EXTENSIBLE_UI, ExtUI::onMeshUpdate(best.pos, ExtUI::G29_POINT_START)); const float measured_z = probe.probe_at_point( best.meshpos(), - stow_probe ? PROBE_PT_STOW : PROBE_PT_RAISE, g29_verbose_level + stow_probe ? PROBE_PT_STOW : PROBE_PT_RAISE, param.V_verbosity ); z_values[best.pos.x][best.pos.y] = measured_z; #if ENABLED(EXTENSIBLE_UI) - ExtUI::onMeshUpdate(best.pos, ExtUI::PROBE_FINISH); + ExtUI::onMeshUpdate(best.pos, ExtUI::G29_POINT_FINISH); ExtUI::onMeshUpdate(best.pos, measured_z); #endif } @@ -777,12 +771,12 @@ void unified_bed_leveling::shift_mesh_height() { } while (best.pos.x >= 0 && --count); - TERN_(EXTENSIBLE_UI, ExtUI::onMeshUpdate(best.pos, ExtUI::MESH_FINISH)); + TERN_(EXTENSIBLE_UI, ExtUI::onMeshUpdate(best.pos, ExtUI::G29_FINISH)); // Release UI during stow to allow for PAUSE_BEFORE_DEPLOY_STOW - TERN_(HAS_LCD_MENU, ui.release()); + TERN_(HAS_MARLINUI_MENU, ui.release()); probe.stow(); - TERN_(HAS_LCD_MENU, ui.capture()); + TERN_(HAS_MARLINUI_MENU, ui.capture()); probe.move_z_after_probing(); @@ -796,11 +790,20 @@ void unified_bed_leveling::shift_mesh_height() { #endif // HAS_BED_PROBE -#if HAS_LCD_MENU +void set_message_with_feedback(FSTR_P const fstr) { + #if HAS_MARLINUI_MENU + ui.set_status(fstr); + ui.quick_feedback(); + #else + UNUSED(fstr); + #endif +} + +#if HAS_MARLINUI_MENU typedef void (*clickFunc_t)(); - bool click_and_hold(const clickFunc_t func=nullptr) { + bool _click_and_hold(const clickFunc_t func=nullptr) { if (ui.button_pressed()) { ui.quick_feedback(false); // Preserve button state for click-and-hold const millis_t nxt = millis() + 1500UL; @@ -818,7 +821,7 @@ void unified_bed_leveling::shift_mesh_height() { return false; } - void unified_bed_leveling::move_z_with_encoder(const float &multiplier) { + void unified_bed_leveling::move_z_with_encoder(const_float_t multiplier) { ui.wait_for_release(); while (!ui.button_pressed()) { idle(); @@ -832,7 +835,8 @@ void unified_bed_leveling::shift_mesh_height() { float unified_bed_leveling::measure_point_with_encoder() { KEEPALIVE_STATE(PAUSED_FOR_USER); - move_z_with_encoder(0.01f); + const float z_step = 0.01f; + move_z_with_encoder(z_step); return current_position.z; } @@ -847,7 +851,7 @@ void unified_bed_leveling::shift_mesh_height() { planner.synchronize(); SERIAL_ECHOPGM("Place shim under nozzle"); - LCD_MESSAGEPGM(MSG_UBL_BC_INSERT); + LCD_MESSAGE(MSG_UBL_BC_INSERT); ui.return_to_status(); echo_and_take_a_measurement(); @@ -856,7 +860,7 @@ void unified_bed_leveling::shift_mesh_height() { planner.synchronize(); SERIAL_ECHOPGM("Remove shim"); - LCD_MESSAGEPGM(MSG_UBL_BC_REMOVE); + LCD_MESSAGE(MSG_UBL_BC_REMOVE); echo_and_take_a_measurement(); const float z2 = measure_point_with_encoder(); @@ -864,7 +868,7 @@ void unified_bed_leveling::shift_mesh_height() { const float thickness = ABS(z1 - z2); - if (g29_verbose_level > 1) { + if (param.V_verbosity > 1) { SERIAL_ECHOPAIR_F("Business Card is ", thickness, 4); SERIAL_ECHOLNPGM("mm thick."); } @@ -874,7 +878,12 @@ void unified_bed_leveling::shift_mesh_height() { return thickness; } - void unified_bed_leveling::manually_probe_remaining_mesh(const xy_pos_t &pos, const float &z_clearance, const float &thick, const bool do_ubl_mesh_map) { + /** + * G29 P2 : Manually Probe Remaining Mesh Points. + * Move to INVALID points and + * NOTE: Blocks the G-code queue and captures Marlin UI during use. + */ + void unified_bed_leveling::manually_probe_remaining_mesh(const xy_pos_t &pos, const_float_t z_clearance, const_float_t thick, const bool do_ubl_mesh_map) { ui.capture(); save_ubl_active_state_and_disable(); // No bed level correction so only raw data is obtained @@ -897,7 +906,7 @@ void unified_bed_leveling::shift_mesh_height() { if (!position_is_reachable(ppos)) break; // SHOULD NOT OCCUR (find_closest_mesh_point only returns reachable points) - LCD_MESSAGEPGM(MSG_UBL_MOVING_TO_NEXT); + LCD_MESSAGE(MSG_UBL_MOVING_TO_NEXT); do_blocking_move_to(ppos); do_z_clearance(z_clearance); @@ -905,56 +914,49 @@ void unified_bed_leveling::shift_mesh_height() { KEEPALIVE_STATE(PAUSED_FOR_USER); ui.capture(); - if (do_ubl_mesh_map) display_map(g29_map_type); // show user where we're probing + if (do_ubl_mesh_map) display_map(param.T_map_type); // Show user where we're probing - if (parser.seen('B')) { - SERIAL_ECHOPGM_P(GET_TEXT(MSG_UBL_BC_INSERT)); - LCD_MESSAGEPGM(MSG_UBL_BC_INSERT); + if (parser.seen_test('B')) { + SERIAL_ECHOPGM("Place Shim & Measure"); + LCD_MESSAGE(MSG_UBL_BC_INSERT); } else { - SERIAL_ECHOPGM_P(GET_TEXT(MSG_UBL_BC_INSERT2)); - LCD_MESSAGEPGM(MSG_UBL_BC_INSERT2); + SERIAL_ECHOPGM("Measure"); + LCD_MESSAGE(MSG_UBL_BC_INSERT2); } - const float z_step = 0.01f; // existing behavior: 0.01mm per click, occasionally step - //const float z_step = planner.steps_to_mm[Z_AXIS]; // approx one step each click - + const float z_step = 0.01f; // 0.01mm per encoder tick, occasionally step move_z_with_encoder(z_step); - if (click_and_hold()) { + if (_click_and_hold([]{ SERIAL_ECHOLNPGM("\nMesh only partially populated."); do_z_clearance(Z_CLEARANCE_DEPLOY_PROBE); - return restore_ubl_active_state_and_leave(); - } + })) return restore_ubl_active_state_and_leave(); + // Store the Z position minus the shim height z_values[lpos.x][lpos.y] = current_position.z - thick; + + // Tell the external UI to update TERN_(EXTENSIBLE_UI, ExtUI::onMeshUpdate(location, z_values[lpos.x][lpos.y])); - if (g29_verbose_level > 2) + if (param.V_verbosity > 2) SERIAL_ECHOLNPAIR_F("Mesh Point Measured at: ", z_values[lpos.x][lpos.y], 6); SERIAL_FLUSH(); // Prevent host M105 buffer overrun. } while (location.valid()); - if (do_ubl_mesh_map) display_map(g29_map_type); // show user where we're probing + if (do_ubl_mesh_map) display_map(param.T_map_type); // show user where we're probing restore_ubl_active_state_and_leave(); do_blocking_move_to_xy_z(pos, Z_CLEARANCE_DEPLOY_PROBE); } - inline void set_message_with_feedback(PGM_P const msg_P) { - ui.set_status_P(msg_P); - ui.quick_feedback(); - } - - void abort_fine_tune() { - ui.return_to_status(); - do_z_clearance(Z_CLEARANCE_BETWEEN_PROBES); - set_message_with_feedback(GET_TEXT(MSG_EDITING_STOPPED)); - } - + /** + * G29 P4 : Mesh Fine-Tuning. Go to point(s) and adjust values with the LCD. + * NOTE: Blocks the G-code queue and captures Marlin UI during use. + */ void unified_bed_leveling::fine_tune_mesh(const xy_pos_t &pos, const bool do_ubl_mesh_map) { - if (!parser.seen('R')) // fine_tune_mesh() is special. If no repetition count flag is specified - g29_repetition_cnt = 1; // do exactly one mesh location. Otherwise use what the parser decided. + if (!parser.seen_test('R')) // fine_tune_mesh() is special. If no repetition count flag is specified + param.R_repetition = 1; // do exactly one mesh location. Otherwise use what the parser decided. #if ENABLED(UBL_MESH_EDIT_MOVES_Z) const float h_offset = parser.seenval('H') ? parser.value_linear_units() : MANUAL_PROBE_START_Z; @@ -973,7 +975,7 @@ void unified_bed_leveling::shift_mesh_height() { save_ubl_active_state_and_disable(); - LCD_MESSAGEPGM(MSG_UBL_FINE_TUNE_MESH); + LCD_MESSAGE(MSG_UBL_FINE_TUNE_MESH); ui.capture(); // Take over control of the LCD encoder do_blocking_move_to_xy_z(pos, Z_CLEARANCE_BETWEEN_PROBES); // Move to the given XY with probe clearance @@ -982,7 +984,7 @@ void unified_bed_leveling::shift_mesh_height() { const xy_int8_t &lpos = location.pos; #if IS_TFTGLCD_PANEL - lcd_mesh_edit_setup(0); // Change current screen before calling ui.ubl_plot + ui.ubl_mesh_edit_start(0); // Change current screen before calling ui.ubl_plot safe_delay(50); #endif @@ -1007,7 +1009,7 @@ void unified_bed_leveling::shift_mesh_height() { KEEPALIVE_STATE(PAUSED_FOR_USER); - if (do_ubl_mesh_map) display_map(g29_map_type); // Display the current point + if (do_ubl_mesh_map) display_map(param.T_map_type); // Display the current point #if IS_TFTGLCD_PANEL ui.ubl_plot(lpos.x, lpos.y); // update plot screen @@ -1019,13 +1021,13 @@ void unified_bed_leveling::shift_mesh_height() { if (isnan(new_z)) new_z = 0; // Invalid points begin at 0 new_z = FLOOR(new_z * 1000) * 0.001f; // Chop off digits after the 1000ths place - lcd_mesh_edit_setup(new_z); + ui.ubl_mesh_edit_start(new_z); SET_SOFT_ENDSTOP_LOOSE(true); do { - idle(); - new_z = lcd_mesh_edit(); + idle_no_sleep(); + new_z = ui.ubl_mesh_value(); TERN_(UBL_MESH_EDIT_MOVES_Z, do_blocking_move_to_z(h_offset + new_z)); // Move the nozzle as the point is edited SERIAL_FLUSH(); // Prevent host M105 buffer overrun. } while (!ui.button_pressed()); @@ -1034,22 +1036,32 @@ void unified_bed_leveling::shift_mesh_height() { if (!lcd_map_control) ui.return_to_status(); // Just editing a single point? Return to status - if (click_and_hold(abort_fine_tune)) break; // Button held down? Abort editing + // Button held down? Abort editing + if (_click_and_hold([]{ + ui.return_to_status(); + do_z_clearance(Z_CLEARANCE_BETWEEN_PROBES); + set_message_with_feedback(GET_TEXT_F(MSG_EDITING_STOPPED)); + })) break; + + // TODO: Disable leveling here so the Z value becomes the 'native' Z value. z_values[lpos.x][lpos.y] = new_z; // Save the updated Z value + + // TODO: Re-enable leveling here so Z is correctly based on the updated mesh. + TERN_(EXTENSIBLE_UI, ExtUI::onMeshUpdate(location, new_z)); serial_delay(20); // No switch noise ui.refresh(); - } while (lpos.x >= 0 && --g29_repetition_cnt > 0); + } while (lpos.x >= 0 && --param.R_repetition > 0); - if (do_ubl_mesh_map) display_map(g29_map_type); + if (do_ubl_mesh_map) display_map(param.T_map_type); restore_ubl_active_state_and_leave(); do_blocking_move_to_xy_z(pos, Z_CLEARANCE_BETWEEN_PROBES); - LCD_MESSAGEPGM(MSG_UBL_DONE_EDITING_MESH); + LCD_MESSAGE(MSG_UBL_DONE_EDITING_MESH); SERIAL_ECHOLNPGM("Done Editing Mesh"); if (lcd_map_control) @@ -1058,33 +1070,36 @@ void unified_bed_leveling::shift_mesh_height() { ui.return_to_status(); } -#endif // HAS_LCD_MENU +#endif // HAS_MARLINUI_MENU -bool unified_bed_leveling::g29_parameter_parsing() { +/** + * Parse and validate most G29 parameters, store for use by G29 functions. + */ +bool unified_bed_leveling::G29_parse_parameters() { bool err_flag = false; - TERN_(HAS_LCD_MENU, set_message_with_feedback(GET_TEXT(MSG_UBL_DOING_G29))); + set_message_with_feedback(GET_TEXT_F(MSG_UBL_DOING_G29)); - g29_constant = 0; - g29_repetition_cnt = 0; + param.C_constant = 0; + param.R_repetition = 0; if (parser.seen('R')) { - g29_repetition_cnt = parser.has_value() ? parser.value_int() : GRID_MAX_POINTS; - NOMORE(g29_repetition_cnt, GRID_MAX_POINTS); - if (g29_repetition_cnt < 1) { + param.R_repetition = parser.has_value() ? parser.value_byte() : GRID_MAX_POINTS; + NOMORE(param.R_repetition, GRID_MAX_POINTS); + if (param.R_repetition < 1) { SERIAL_ECHOLNPGM("?(R)epetition count invalid (1+).\n"); return UBL_ERR; } } - g29_verbose_level = parser.seen('V') ? parser.value_int() : 0; - if (!WITHIN(g29_verbose_level, 0, 4)) { + param.V_verbosity = parser.byteval('V'); + if (!WITHIN(param.V_verbosity, 0, 4)) { SERIAL_ECHOLNPGM("?(V)erbose level implausible (0-4).\n"); err_flag = true; } if (parser.seen('P')) { - const int pv = parser.value_int(); + const uint8_t pv = parser.value_byte(); #if !HAS_BED_PROBE if (pv == 1) { SERIAL_ECHOLNPGM("G29 P1 requires a probe.\n"); @@ -1093,8 +1108,8 @@ bool unified_bed_leveling::g29_parameter_parsing() { else #endif { - g29_phase_value = pv; - if (!WITHIN(g29_phase_value, 0, 6)) { + param.P_phase = pv; + if (!WITHIN(param.P_phase, 0, 6)) { SERIAL_ECHOLNPGM("?(P)hase value invalid (0-6).\n"); err_flag = true; } @@ -1103,8 +1118,8 @@ bool unified_bed_leveling::g29_parameter_parsing() { if (parser.seen('J')) { #if HAS_BED_PROBE - g29_grid_size = parser.has_value() ? parser.value_int() : 0; - if (g29_grid_size && !WITHIN(g29_grid_size, 2, 9)) { + param.J_grid_size = parser.value_byte(); + if (param.J_grid_size && !WITHIN(param.J_grid_size, 2, 9)) { SERIAL_ECHOLNPGM("?Invalid grid size (J) specified (2-9).\n"); err_flag = true; } @@ -1114,23 +1129,24 @@ bool unified_bed_leveling::g29_parameter_parsing() { #endif } - xy_seen.x = parser.seenval('X'); - float sx = xy_seen.x ? parser.value_float() : current_position.x; - xy_seen.y = parser.seenval('Y'); - float sy = xy_seen.y ? parser.value_float() : current_position.y; + param.XY_seen.x = parser.seenval('X'); + float sx = param.XY_seen.x ? parser.value_float() : current_position.x; + param.XY_seen.y = parser.seenval('Y'); + float sy = param.XY_seen.y ? parser.value_float() : current_position.y; - if (xy_seen.x != xy_seen.y) { + if (param.XY_seen.x != param.XY_seen.y) { SERIAL_ECHOLNPGM("Both X & Y locations must be specified.\n"); err_flag = true; } // If X or Y are not valid, use center of the bed values - if (!COORDINATE_OKAY(sx, X_MIN_BED, X_MAX_BED)) sx = X_CENTER; - if (!COORDINATE_OKAY(sy, Y_MIN_BED, Y_MAX_BED)) sy = Y_CENTER; + // (for UBL_HILBERT_CURVE default to lower-left corner instead) + if (!COORDINATE_OKAY(sx, X_MIN_BED, X_MAX_BED)) sx = TERN(UBL_HILBERT_CURVE, 0, X_CENTER); + if (!COORDINATE_OKAY(sy, Y_MIN_BED, Y_MAX_BED)) sy = TERN(UBL_HILBERT_CURVE, 0, Y_CENTER); if (err_flag) return UBL_ERR; - g29_pos.set(sx, sy); + param.XY_pos.set(sx, sy); /** * Activate or deactivate UBL @@ -1138,22 +1154,22 @@ bool unified_bed_leveling::g29_parameter_parsing() { * Leveling is being enabled here with old data, possibly * none. Error handling should disable for safety... */ - if (parser.seen('A')) { - if (parser.seen('D')) { + if (parser.seen_test('A')) { + if (parser.seen_test('D')) { SERIAL_ECHOLNPGM("?Can't activate and deactivate at the same time.\n"); return UBL_ERR; } set_bed_leveling_enabled(true); report_state(); } - else if (parser.seen('D')) { + else if (parser.seen_test('D')) { set_bed_leveling_enabled(false); report_state(); } // Set global 'C' flag and its value - if ((g29_c_flag = parser.seen('C'))) - g29_constant = parser.value_float(); + if ((param.C_seen = parser.seen('C'))) + param.C_constant = parser.value_float(); #if ENABLED(ENABLE_LEVELING_FADE_HEIGHT) if (parser.seenval('F')) { @@ -1166,8 +1182,8 @@ bool unified_bed_leveling::g29_parameter_parsing() { } #endif - g29_map_type = parser.intval('T'); - if (!WITHIN(g29_map_type, 0, 2)) { + param.T_map_type = parser.byteval('T'); + if (!WITHIN(param.T_map_type, 0, 2)) { SERIAL_ECHOLNPGM("Invalid map type.\n"); return UBL_ERR; } @@ -1185,7 +1201,7 @@ void unified_bed_leveling::save_ubl_active_state_and_disable() { ubl_state_recursion_chk++; if (ubl_state_recursion_chk != 1) { SERIAL_ECHOLNPGM("save_ubl_active_state_and_disabled() called multiple times in a row."); - TERN_(HAS_LCD_MENU, set_message_with_feedback(GET_TEXT(MSG_UBL_SAVE_ERROR))); + set_message_with_feedback(GET_TEXT_F(MSG_UBL_SAVE_ERROR)); return; } #endif @@ -1194,11 +1210,11 @@ void unified_bed_leveling::save_ubl_active_state_and_disable() { } void unified_bed_leveling::restore_ubl_active_state_and_leave() { - TERN_(HAS_LCD_MENU, ui.release()); + TERN_(HAS_MARLINUI_MENU, ui.release()); #if ENABLED(UBL_DEVEL_DEBUGGING) if (--ubl_state_recursion_chk) { SERIAL_ECHOLNPGM("restore_ubl_active_state_and_leave() called too many times."); - TERN_(HAS_LCD_MENU, set_message_with_feedback(GET_TEXT(MSG_UBL_RESTORE_ERROR))); + set_message_with_feedback(GET_TEXT_F(MSG_UBL_RESTORE_ERROR)); return; } #endif @@ -1256,46 +1272,92 @@ mesh_index_pair unified_bed_leveling::find_furthest_invalid_mesh_point() { return farthest; } -mesh_index_pair unified_bed_leveling::find_closest_mesh_point_of_type(const MeshPointType type, const xy_pos_t &pos, const bool probe_relative/*=false*/, MeshFlags *done_flags/*=nullptr*/) { - mesh_index_pair closest; - closest.invalidate(); - closest.distance = -99999.9f; - - // Get the reference position, either nozzle or probe - const xy_pos_t ref = probe_relative ? pos + probe.offset_xy : pos; +#if ENABLED(UBL_HILBERT_CURVE) - float best_so_far = 99999.99f; + typedef struct { + MeshPointType type; + MeshFlags *done_flags; + bool probe_relative; + mesh_index_pair closest; + } find_closest_t; - GRID_LOOP(i, j) { - if ( (type == (isnan(z_values[i][j]) ? INVALID : REAL)) - || (type == SET_IN_BITMAP && !done_flags->marked(i, j)) + static bool test_func(uint8_t i, uint8_t j, void *data) { + find_closest_t *d = (find_closest_t*)data; + if ( d->type == CLOSEST || d->type == (isnan(ubl.z_values[i][j]) ? INVALID : REAL) + || (d->type == SET_IN_BITMAP && !d->done_flags->marked(i, j)) ) { // Found a Mesh Point of the specified type! - const xy_pos_t mpos = { mesh_index_to_xpos(i), mesh_index_to_ypos(j) }; + const xy_pos_t mpos = { ubl.mesh_index_to_xpos(i), ubl.mesh_index_to_ypos(j) }; // If using the probe as the reference there are some unreachable locations. // Also for round beds, there are grid points outside the bed the nozzle can't reach. // Prune them from the list and ignore them till the next Phase (manual nozzle probing). - if (!(probe_relative ? probe.can_reach(mpos) : position_is_reachable(mpos))) - continue; + if (!(d->probe_relative ? probe.can_reach(mpos) : position_is_reachable(mpos))) + return false; + d->closest.pos.set(i, j); + return true; + } + return false; + } - // Reachable. Check if it's the best_so_far location to the nozzle. +#endif + +mesh_index_pair unified_bed_leveling::find_closest_mesh_point_of_type(const MeshPointType type, const xy_pos_t &pos, const bool probe_relative/*=false*/, MeshFlags *done_flags/*=nullptr*/) { + + #if ENABLED(UBL_HILBERT_CURVE) + + find_closest_t d; + d.type = type; + d.done_flags = done_flags; + d.probe_relative = probe_relative; + d.closest.invalidate(); + hilbert_curve::search_from_closest(pos, test_func, &d); + return d.closest; + + #else + + mesh_index_pair closest; + closest.invalidate(); + closest.distance = -99999.9f; - const xy_pos_t diff = current_position - mpos; - const float distance = (ref - mpos).magnitude() + diff.magnitude() * 0.1f; + // Get the reference position, either nozzle or probe + const xy_pos_t ref = probe_relative ? pos + probe.offset_xy : pos; - // factor in the distance from the current location for the normal case - // so the nozzle isn't running all over the bed. - if (distance < best_so_far) { - best_so_far = distance; // Found a closer location with the desired value type. - closest.pos.set(i, j); - closest.distance = best_so_far; + float best_so_far = 99999.99f; + + GRID_LOOP(i, j) { + if ( type == CLOSEST || type == (isnan(z_values[i][j]) ? INVALID : REAL) + || (type == SET_IN_BITMAP && !done_flags->marked(i, j)) + ) { + // Found a Mesh Point of the specified type! + const xy_pos_t mpos = { mesh_index_to_xpos(i), mesh_index_to_ypos(j) }; + + // If using the probe as the reference there are some unreachable locations. + // Also for round beds, there are grid points outside the bed the nozzle can't reach. + // Prune them from the list and ignore them till the next Phase (manual nozzle probing). + + if (!(probe_relative ? probe.can_reach(mpos) : position_is_reachable(mpos))) + continue; + + // Reachable. Check if it's the best_so_far location to the nozzle. + + const xy_pos_t diff = current_position - mpos; + const float distance = (ref - mpos).magnitude() + diff.magnitude() * 0.1f; + + // factor in the distance from the current location for the normal case + // so the nozzle isn't running all over the bed. + if (distance < best_so_far) { + best_so_far = distance; // Found a closer location with the desired value type. + closest.pos.set(i, j); + closest.distance = best_so_far; + } } - } - } // GRID_LOOP + } // GRID_LOOP + + return closest; - return closest; + #endif } /** @@ -1359,8 +1421,8 @@ void unified_bed_leveling::smart_fill_mesh() { void unified_bed_leveling::tilt_mesh_based_on_probed_grid(const bool do_3_pt_leveling) { const float x_min = probe.min_x(), x_max = probe.max_x(), y_min = probe.min_y(), y_max = probe.max_y(), - dx = (x_max - x_min) / (g29_grid_size - 1), - dy = (y_max - y_min) / (g29_grid_size - 1); + dx = (x_max - x_min) / (param.J_grid_size - 1), + dy = (y_max - y_min) / (param.J_grid_size - 1); xy_float_t points[3]; probe.get_three_points(points); @@ -1377,9 +1439,9 @@ void unified_bed_leveling::smart_fill_mesh() { if (do_3_pt_leveling) { SERIAL_ECHOLNPGM("Tilting mesh (1/3)"); - TERN_(HAS_DISPLAY, ui.status_printf_P(0, PSTR(S_FMT " 1/3"), GET_TEXT(MSG_LCD_TILTING_MESH))); + TERN_(HAS_STATUS_MESSAGE, ui.status_printf(0, F(S_FMT " 1/3"), GET_TEXT(MSG_LCD_TILTING_MESH))); - measured_z = probe.probe_at_point(points[0], PROBE_PT_RAISE, g29_verbose_level); + measured_z = probe.probe_at_point(points[0], PROBE_PT_RAISE, param.V_verbosity); if (isnan(measured_z)) abort_flag = true; else { @@ -1387,18 +1449,18 @@ void unified_bed_leveling::smart_fill_mesh() { #ifdef VALIDATE_MESH_TILT z1 = measured_z; #endif - if (g29_verbose_level > 3) { + if (param.V_verbosity > 3) { serial_spaces(16); - SERIAL_ECHOLNPAIR("Corrected_Z=", measured_z); + SERIAL_ECHOLNPGM("Corrected_Z=", measured_z); } incremental_LSF(&lsf_results, points[0], measured_z); } if (!abort_flag) { SERIAL_ECHOLNPGM("Tilting mesh (2/3)"); - TERN_(HAS_DISPLAY, ui.status_printf_P(0, PSTR(S_FMT " 2/3"), GET_TEXT(MSG_LCD_TILTING_MESH))); + TERN_(HAS_STATUS_MESSAGE, ui.status_printf(0, F(S_FMT " 2/3"), GET_TEXT(MSG_LCD_TILTING_MESH))); - measured_z = probe.probe_at_point(points[1], PROBE_PT_RAISE, g29_verbose_level); + measured_z = probe.probe_at_point(points[1], PROBE_PT_RAISE, param.V_verbosity); #ifdef VALIDATE_MESH_TILT z2 = measured_z; #endif @@ -1406,9 +1468,9 @@ void unified_bed_leveling::smart_fill_mesh() { abort_flag = true; else { measured_z -= get_z_correction(points[1]); - if (g29_verbose_level > 3) { + if (param.V_verbosity > 3) { serial_spaces(16); - SERIAL_ECHOLNPAIR("Corrected_Z=", measured_z); + SERIAL_ECHOLNPGM("Corrected_Z=", measured_z); } incremental_LSF(&lsf_results, points[1], measured_z); } @@ -1416,9 +1478,9 @@ void unified_bed_leveling::smart_fill_mesh() { if (!abort_flag) { SERIAL_ECHOLNPGM("Tilting mesh (3/3)"); - TERN_(HAS_DISPLAY, ui.status_printf_P(0, PSTR(S_FMT " 3/3"), GET_TEXT(MSG_LCD_TILTING_MESH))); + TERN_(HAS_STATUS_MESSAGE, ui.status_printf(0, F(S_FMT " 3/3"), GET_TEXT(MSG_LCD_TILTING_MESH))); - measured_z = probe.probe_at_point(points[2], PROBE_PT_STOW, g29_verbose_level); + measured_z = probe.probe_at_point(points[2], PROBE_PT_LAST_STOW, param.V_verbosity); #ifdef VALIDATE_MESH_TILT z3 = measured_z; #endif @@ -1426,9 +1488,9 @@ void unified_bed_leveling::smart_fill_mesh() { abort_flag = true; else { measured_z -= get_z_correction(points[2]); - if (g29_verbose_level > 3) { + if (param.V_verbosity > 3) { serial_spaces(16); - SERIAL_ECHOLNPAIR("Corrected_Z=", measured_z); + SERIAL_ECHOLNPGM("Corrected_Z=", measured_z); } incremental_LSF(&lsf_results, points[2], measured_z); } @@ -1446,20 +1508,20 @@ void unified_bed_leveling::smart_fill_mesh() { bool zig_zag = false; - const uint16_t total_points = sq(g29_grid_size); + const uint16_t total_points = sq(param.J_grid_size); uint16_t point_num = 1; xy_pos_t rpos; - LOOP_L_N(ix, g29_grid_size) { + LOOP_L_N(ix, param.J_grid_size) { rpos.x = x_min + ix * dx; - LOOP_L_N(iy, g29_grid_size) { - rpos.y = y_min + dy * (zig_zag ? g29_grid_size - 1 - iy : iy); + LOOP_L_N(iy, param.J_grid_size) { + rpos.y = y_min + dy * (zig_zag ? param.J_grid_size - 1 - iy : iy); if (!abort_flag) { - SERIAL_ECHOLNPAIR("Tilting mesh point ", point_num, "/", total_points, "\n"); - TERN_(HAS_DISPLAY, ui.status_printf_P(0, PSTR(S_FMT " %i/%i"), GET_TEXT(MSG_LCD_TILTING_MESH), point_num, total_points)); + SERIAL_ECHOLNPGM("Tilting mesh point ", point_num, "/", total_points, "\n"); + TERN_(HAS_STATUS_MESSAGE, ui.status_printf(0, F(S_FMT " %i/%i"), GET_TEXT(MSG_LCD_TILTING_MESH), point_num, total_points)); - measured_z = probe.probe_at_point(rpos, parser.seen('E') ? PROBE_PT_STOW : PROBE_PT_RAISE, g29_verbose_level); // TODO: Needs error handling + measured_z = probe.probe_at_point(rpos, parser.seen_test('E') ? PROBE_PT_STOW : PROBE_PT_RAISE, param.V_verbosity); // TODO: Needs error handling abort_flag = isnan(measured_z); @@ -1482,9 +1544,9 @@ void unified_bed_leveling::smart_fill_mesh() { if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPAIR_F(" final >>>---> ", measured_z, 7); - if (g29_verbose_level > 3) { + if (param.V_verbosity > 3) { serial_spaces(16); - SERIAL_ECHOLNPAIR("Corrected_Z=", measured_z); + SERIAL_ECHOLNPGM("Corrected_Z=", measured_z); } incremental_LSF(&lsf_results, rpos, measured_z); } @@ -1505,7 +1567,7 @@ void unified_bed_leveling::smart_fill_mesh() { vector_3 normal = vector_3(lsf_results.A, lsf_results.B, 1).get_normal(); - if (g29_verbose_level > 2) { + if (param.V_verbosity > 2) { SERIAL_ECHOPAIR_F("bed plane normal = [", normal.x, 7); SERIAL_CHAR(','); SERIAL_ECHO_F(normal.y, 7); @@ -1531,7 +1593,7 @@ void unified_bed_leveling::smart_fill_mesh() { DEBUG_DELAY(20); } - apply_rotation_xyz(rotation, mx, my, mz); + rotation.apply_rotation_xyz(mx, my, mz); if (DEBUGGING(LEVELING)) { DEBUG_ECHOPAIR_F("after rotation = [", mx, 7); @@ -1548,7 +1610,7 @@ void unified_bed_leveling::smart_fill_mesh() { } if (DEBUGGING(LEVELING)) { - rotation.debug(PSTR("rotation matrix:\n")); + rotation.debug(F("rotation matrix:\n")); DEBUG_ECHOPAIR_F("LSF Results A=", lsf_results.A, 7); DEBUG_ECHOPAIR_F(" B=", lsf_results.B, 7); DEBUG_ECHOLNPAIR_F(" D=", lsf_results.D, 7); @@ -1572,22 +1634,22 @@ void unified_bed_leveling::smart_fill_mesh() { */ #ifdef VALIDATE_MESH_TILT auto d_from = []{ DEBUG_ECHOPGM("D from "); }; - auto normed = [&](const xy_pos_t &pos, const float &zadd) { + auto normed = [&](const xy_pos_t &pos, const_float_t zadd) { return normal.x * pos.x + normal.y * pos.y + zadd; }; - auto debug_pt = [](PGM_P const pre, const xy_pos_t &pos, const float &zadd) { - d_from(); SERIAL_ECHOPGM_P(pre); + auto debug_pt = [](FSTR_P const pre, const xy_pos_t &pos, const_float_t zadd) { + d_from(); SERIAL_ECHOF(pre); DEBUG_ECHO_F(normed(pos, zadd), 6); DEBUG_ECHOLNPAIR_F(" Z error = ", zadd - get_z_correction(pos), 6); }; - debug_pt(PSTR("1st point: "), probe_pt[0], normal.z * z1); - debug_pt(PSTR("2nd point: "), probe_pt[1], normal.z * z2); - debug_pt(PSTR("3rd point: "), probe_pt[2], normal.z * z3); + debug_pt(F("1st point: "), probe_pt[0], normal.z * z1); + debug_pt(F("2nd point: "), probe_pt[1], normal.z * z2); + debug_pt(F("3rd point: "), probe_pt[2], normal.z * z3); d_from(); DEBUG_ECHOPGM("safe home with Z="); DEBUG_ECHOLNPAIR_F("0 : ", normed(safe_homing_xy, 0), 6); d_from(); DEBUG_ECHOPGM("safe home with Z="); DEBUG_ECHOLNPAIR_F("mesh value ", normed(safe_homing_xy, get_z_correction(safe_homing_xy)), 6); - DEBUG_ECHOPAIR(" Z error = (", Z_SAFE_HOMING_X_POINT, ",", Z_SAFE_HOMING_Y_POINT); + DEBUG_ECHOPGM(" Z error = (", Z_SAFE_HOMING_X_POINT, ",", Z_SAFE_HOMING_Y_POINT); DEBUG_ECHOLNPAIR_F(") = ", get_z_correction(safe_homing_xy), 6); #endif } // DEBUGGING(LEVELING) @@ -1597,7 +1659,7 @@ void unified_bed_leveling::smart_fill_mesh() { #endif // HAS_BED_PROBE #if ENABLED(UBL_G29_P31) - void unified_bed_leveling::smart_fill_wlsf(const float &weight_factor) { + void unified_bed_leveling::smart_fill_wlsf(const_float_t weight_factor) { // For each undefined mesh point, compute a distance-weighted least squares fit // from all the originally populated mesh points, weighted toward the point @@ -1661,7 +1723,7 @@ void unified_bed_leveling::smart_fill_mesh() { if (storage_slot == -1) SERIAL_ECHOPGM("No Mesh Loaded."); else - SERIAL_ECHOPAIR("Mesh ", storage_slot, " Loaded."); + SERIAL_ECHOPGM("Mesh ", storage_slot, " Loaded."); SERIAL_EOL(); serial_delay(50); @@ -1669,20 +1731,20 @@ void unified_bed_leveling::smart_fill_mesh() { SERIAL_ECHOLNPAIR_F("Fade Height M420 Z", planner.z_fade_height, 4); #endif - adjust_mesh_to_mean(g29_c_flag, g29_constant); + adjust_mesh_to_mean(param.C_seen, param.C_constant); #if HAS_BED_PROBE SERIAL_ECHOLNPAIR_F("Probe Offset M851 Z", probe.offset.z, 7); #endif - SERIAL_ECHOLNPAIR("MESH_MIN_X " STRINGIFY(MESH_MIN_X) "=", MESH_MIN_X); serial_delay(50); - SERIAL_ECHOLNPAIR("MESH_MIN_Y " STRINGIFY(MESH_MIN_Y) "=", MESH_MIN_Y); serial_delay(50); - SERIAL_ECHOLNPAIR("MESH_MAX_X " STRINGIFY(MESH_MAX_X) "=", MESH_MAX_X); serial_delay(50); - SERIAL_ECHOLNPAIR("MESH_MAX_Y " STRINGIFY(MESH_MAX_Y) "=", MESH_MAX_Y); serial_delay(50); - SERIAL_ECHOLNPAIR("GRID_MAX_POINTS_X ", GRID_MAX_POINTS_X); serial_delay(50); - SERIAL_ECHOLNPAIR("GRID_MAX_POINTS_Y ", GRID_MAX_POINTS_Y); serial_delay(50); - SERIAL_ECHOLNPAIR("MESH_X_DIST ", MESH_X_DIST); - SERIAL_ECHOLNPAIR("MESH_Y_DIST ", MESH_Y_DIST); serial_delay(50); + SERIAL_ECHOLNPGM("MESH_MIN_X " STRINGIFY(MESH_MIN_X) "=", MESH_MIN_X); serial_delay(50); + SERIAL_ECHOLNPGM("MESH_MIN_Y " STRINGIFY(MESH_MIN_Y) "=", MESH_MIN_Y); serial_delay(50); + SERIAL_ECHOLNPGM("MESH_MAX_X " STRINGIFY(MESH_MAX_X) "=", MESH_MAX_X); serial_delay(50); + SERIAL_ECHOLNPGM("MESH_MAX_Y " STRINGIFY(MESH_MAX_Y) "=", MESH_MAX_Y); serial_delay(50); + SERIAL_ECHOLNPGM("GRID_MAX_POINTS_X ", GRID_MAX_POINTS_X); serial_delay(50); + SERIAL_ECHOLNPGM("GRID_MAX_POINTS_Y ", GRID_MAX_POINTS_Y); serial_delay(50); + SERIAL_ECHOLNPGM("MESH_X_DIST ", MESH_X_DIST); + SERIAL_ECHOLNPGM("MESH_Y_DIST ", MESH_Y_DIST); serial_delay(50); SERIAL_ECHOPGM("X-Axis Mesh Points at: "); LOOP_L_N(i, GRID_MAX_POINTS_X) { @@ -1701,27 +1763,27 @@ void unified_bed_leveling::smart_fill_mesh() { SERIAL_EOL(); #if HAS_KILL - SERIAL_ECHOLNPAIR("Kill pin on :", KILL_PIN, " state:", kill_state()); + SERIAL_ECHOLNPGM("Kill pin on :", KILL_PIN, " state:", kill_state()); #endif SERIAL_EOL(); serial_delay(50); #if ENABLED(UBL_DEVEL_DEBUGGING) - SERIAL_ECHOLNPAIR("ubl_state_at_invocation :", ubl_state_at_invocation, "\nubl_state_recursion_chk :", ubl_state_recursion_chk); + SERIAL_ECHOLNPGM("ubl_state_at_invocation :", ubl_state_at_invocation, "\nubl_state_recursion_chk :", ubl_state_recursion_chk); serial_delay(50); - SERIAL_ECHOLNPAIR("Meshes go from ", hex_address((void*)settings.meshes_start_index()), " to ", hex_address((void*)settings.meshes_end_index())); + SERIAL_ECHOLNPGM("Meshes go from ", hex_address((void*)settings.meshes_start_index()), " to ", hex_address((void*)settings.meshes_end_index())); serial_delay(50); - SERIAL_ECHOLNPAIR("sizeof(ubl) : ", sizeof(ubl)); SERIAL_EOL(); - SERIAL_ECHOLNPAIR("z_value[][] size: ", sizeof(z_values)); SERIAL_EOL(); + SERIAL_ECHOLNPGM("sizeof(ubl) : ", sizeof(ubl)); SERIAL_EOL(); + SERIAL_ECHOLNPGM("z_value[][] size: ", sizeof(z_values)); SERIAL_EOL(); serial_delay(25); - SERIAL_ECHOLNPAIR("EEPROM free for UBL: ", hex_address((void*)(settings.meshes_end_index() - settings.meshes_start_index()))); + SERIAL_ECHOLNPGM("EEPROM free for UBL: ", hex_address((void*)(settings.meshes_end_index() - settings.meshes_start_index()))); serial_delay(50); - SERIAL_ECHOLNPAIR("EEPROM can hold ", settings.calc_num_meshes(), " meshes.\n"); + SERIAL_ECHOLNPGM("EEPROM can hold ", settings.calc_num_meshes(), " meshes.\n"); serial_delay(25); #endif // UBL_DEVEL_DEBUGGING @@ -1767,17 +1829,17 @@ void unified_bed_leveling::smart_fill_mesh() { return; } - if (!parser.has_value() || !WITHIN(g29_storage_slot, 0, a - 1)) { - SERIAL_ECHOLNPAIR("?Invalid storage slot.\n?Use 0 to ", a - 1); + if (!parser.has_value() || !WITHIN(parser.value_int(), 0, a - 1)) { + SERIAL_ECHOLNPGM("?Invalid storage slot.\n?Use 0 to ", a - 1); return; } - g29_storage_slot = parser.value_int(); + param.KLS_storage_slot = (int8_t)parser.value_int(); float tmp_z_values[GRID_MAX_POINTS_X][GRID_MAX_POINTS_Y]; - settings.load_mesh(g29_storage_slot, &tmp_z_values); + settings.load_mesh(param.KLS_storage_slot, &tmp_z_values); - SERIAL_ECHOLNPAIR("Subtracting mesh in slot ", g29_storage_slot, " from current mesh."); + SERIAL_ECHOLNPGM("Subtracting mesh in slot ", param.KLS_storage_slot, " from current mesh."); GRID_LOOP(x, y) { z_values[x][y] -= tmp_z_values[x][y]; diff --git a/Marlin/src/feature/bedlevel/ubl/ubl_motion.cpp b/Marlin/src/feature/bedlevel/ubl/ubl_motion.cpp index 33b4f03ac290..f7e98c9fa77d 100644 --- a/Marlin/src/feature/bedlevel/ubl/ubl_motion.cpp +++ b/Marlin/src/feature/bedlevel/ubl/ubl_motion.cpp @@ -19,6 +19,7 @@ * along with this program. If not, see . * */ + #include "../../../inc/MarlinConfig.h" #if ENABLED(AUTO_BED_LEVELING_UBL) @@ -37,7 +38,7 @@ #if !UBL_SEGMENTED - void unified_bed_leveling::line_to_destination_cartesian(const feedRate_t &scaled_fr_mm_s, const uint8_t extruder) { + void unified_bed_leveling::line_to_destination_cartesian(const_feedRate_t scaled_fr_mm_s, const uint8_t extruder) { /** * Much of the nozzle movement will be within the same cell. So we will do as little computation * as possible to determine if this is the case. If this move is within the same cell, we will @@ -113,20 +114,22 @@ const xy_float_t ad = sign * dist; const bool use_x_dist = ad.x > ad.y; - float on_axis_distance = use_x_dist ? dist.x : dist.y, - e_position = end.e - start.e, - z_position = end.z - start.z; + float on_axis_distance = use_x_dist ? dist.x : dist.y; - const float e_normalized_dist = e_position / on_axis_distance, // Allow divide by zero - z_normalized_dist = z_position / on_axis_distance; + const float z_normalized_dist = (end.z - start.z) / on_axis_distance; // Allow divide by zero + #if HAS_EXTRUDERS + const float e_normalized_dist = (end.e - start.e) / on_axis_distance; + const bool inf_normalized_flag = isinf(e_normalized_dist); + #endif xy_int8_t icell = istart; const float ratio = dist.y / dist.x, // Allow divide by zero c = start.y - ratio * start.x; - const bool inf_normalized_flag = isinf(e_normalized_dist), - inf_ratio_flag = isinf(ratio); + const bool inf_ratio_flag = isinf(ratio); + + xyze_pos_t dest; // Stores XYZE for segmented moves /** * Handle vertical lines that stay within one column. @@ -143,34 +146,36 @@ * For others the next X is the same so this can continue. * Calculate X at the next Y mesh line. */ - const float rx = inf_ratio_flag ? start.x : (next_mesh_line_y - c) / ratio; + dest.x = inf_ratio_flag ? start.x : (next_mesh_line_y - c) / ratio; - float z0 = z_correction_for_x_on_horizontal_mesh_line(rx, icell.x, icell.y) + float z0 = z_correction_for_x_on_horizontal_mesh_line(dest.x, icell.x, icell.y) * planner.fade_scaling_factor_for_z(end.z); // Undefined parts of the Mesh in z_values[][] are NAN. // Replace NAN corrections with 0.0 to prevent NAN propagation. if (isnan(z0)) z0 = 0.0; - const float ry = mesh_index_to_ypos(icell.y); + dest.y = mesh_index_to_ypos(icell.y); /** * Without this check, it's possible to generate a zero length move, as in the case where * the line is heading down, starting exactly on a mesh line boundary. Since this is rare * it might be fine to remove this check and let planner.buffer_segment() filter it out. */ - if (ry != start.y) { + if (dest.y != start.y) { if (!inf_normalized_flag) { // fall-through faster than branch - on_axis_distance = use_x_dist ? rx - start.x : ry - start.y; - e_position = start.e + on_axis_distance * e_normalized_dist; - z_position = start.z + on_axis_distance * z_normalized_dist; + on_axis_distance = use_x_dist ? dest.x - start.x : dest.y - start.y; + TERN_(HAS_EXTRUDERS, dest.e = start.e + on_axis_distance * e_normalized_dist); + dest.z = start.z + on_axis_distance * z_normalized_dist; } else { - e_position = end.e; - z_position = end.z; + TERN_(HAS_EXTRUDERS, dest.e = end.e); + dest.z = end.z; } - planner.buffer_segment(rx, ry, z_position + z0, e_position, scaled_fr_mm_s, extruder); + dest.z += z0; + planner.buffer_segment(dest, scaled_fr_mm_s, extruder); + } //else printf("FIRST MOVE PRUNED "); } @@ -188,12 +193,13 @@ */ if (iadd.y == 0) { // Horizontal line? icell.x += ineg.x; // Heading left? Just go to the left edge of the cell for the first move. + while (icell.x != iend.x + ineg.x) { icell.x += iadd.x; - const float rx = mesh_index_to_xpos(icell.x); - const float ry = ratio * rx + c; // Calculate Y at the next X mesh line + dest.x = mesh_index_to_xpos(icell.x); + dest.y = ratio * dest.x + c; // Calculate Y at the next X mesh line - float z0 = z_correction_for_y_on_vertical_mesh_line(ry, icell.x, icell.y) + float z0 = z_correction_for_y_on_vertical_mesh_line(dest.y, icell.x, icell.y) * planner.fade_scaling_factor_for_z(end.z); // Undefined parts of the Mesh in z_values[][] are NAN. @@ -205,19 +211,20 @@ * the line is heading left, starting exactly on a mesh line boundary. Since this is rare * it might be fine to remove this check and let planner.buffer_segment() filter it out. */ - if (rx != start.x) { + if (dest.x != start.x) { if (!inf_normalized_flag) { - on_axis_distance = use_x_dist ? rx - start.x : ry - start.y; - e_position = start.e + on_axis_distance * e_normalized_dist; // is based on X or Y because this is a horizontal move - z_position = start.z + on_axis_distance * z_normalized_dist; + on_axis_distance = use_x_dist ? dest.x - start.x : dest.y - start.y; + TERN_(HAS_EXTRUDERS, dest.e = start.e + on_axis_distance * e_normalized_dist); // Based on X or Y because the move is horizontal + dest.z = start.z + on_axis_distance * z_normalized_dist; } else { - e_position = end.e; - z_position = end.z; + TERN_(HAS_EXTRUDERS, dest.e = end.e); + dest.z = end.z; } - if (!planner.buffer_segment(rx, ry, z_position + z0, e_position, scaled_fr_mm_s, extruder)) - break; + dest.z += z0; + if (!planner.buffer_segment(dest, scaled_fr_mm_s, extruder)) break; + } //else printf("FIRST MOVE PRUNED "); } @@ -239,57 +246,65 @@ while (cnt) { const float next_mesh_line_x = mesh_index_to_xpos(icell.x + iadd.x), - next_mesh_line_y = mesh_index_to_ypos(icell.y + iadd.y), - ry = ratio * next_mesh_line_x + c, // Calculate Y at the next X mesh line - rx = (next_mesh_line_y - c) / ratio; // Calculate X at the next Y mesh line - // (No need to worry about ratio == 0. - // In that case, it was already detected - // as a vertical line move above.) - - if (neg.x == (rx > next_mesh_line_x)) { // Check if we hit the Y line first + next_mesh_line_y = mesh_index_to_ypos(icell.y + iadd.y); + + dest.y = ratio * next_mesh_line_x + c; // Calculate Y at the next X mesh line + dest.x = (next_mesh_line_y - c) / ratio; // Calculate X at the next Y mesh line + // (No need to worry about ratio == 0. + // In that case, it was already detected + // as a vertical line move above.) + + if (neg.x == (dest.x > next_mesh_line_x)) { // Check if we hit the Y line first // Yes! Crossing a Y Mesh Line next - float z0 = z_correction_for_x_on_horizontal_mesh_line(rx, icell.x - ineg.x, icell.y + iadd.y) + float z0 = z_correction_for_x_on_horizontal_mesh_line(dest.x, icell.x - ineg.x, icell.y + iadd.y) * planner.fade_scaling_factor_for_z(end.z); // Undefined parts of the Mesh in z_values[][] are NAN. // Replace NAN corrections with 0.0 to prevent NAN propagation. if (isnan(z0)) z0 = 0.0; + dest.y = next_mesh_line_y; + if (!inf_normalized_flag) { - on_axis_distance = use_x_dist ? rx - start.x : next_mesh_line_y - start.y; - e_position = start.e + on_axis_distance * e_normalized_dist; - z_position = start.z + on_axis_distance * z_normalized_dist; + on_axis_distance = use_x_dist ? dest.x - start.x : dest.y - start.y; + TERN_(HAS_EXTRUDERS, dest.e = start.e + on_axis_distance * e_normalized_dist); + dest.z = start.z + on_axis_distance * z_normalized_dist; } else { - e_position = end.e; - z_position = end.z; + TERN_(HAS_EXTRUDERS, dest.e = end.e); + dest.z = end.z; } - if (!planner.buffer_segment(rx, next_mesh_line_y, z_position + z0, e_position, scaled_fr_mm_s, extruder)) - break; + + dest.z += z0; + if (!planner.buffer_segment(dest, scaled_fr_mm_s, extruder)) break; + icell.y += iadd.y; cnt.y--; } else { // Yes! Crossing a X Mesh Line next - float z0 = z_correction_for_y_on_vertical_mesh_line(ry, icell.x + iadd.x, icell.y - ineg.y) + float z0 = z_correction_for_y_on_vertical_mesh_line(dest.y, icell.x + iadd.x, icell.y - ineg.y) * planner.fade_scaling_factor_for_z(end.z); // Undefined parts of the Mesh in z_values[][] are NAN. // Replace NAN corrections with 0.0 to prevent NAN propagation. if (isnan(z0)) z0 = 0.0; + dest.x = next_mesh_line_x; + if (!inf_normalized_flag) { - on_axis_distance = use_x_dist ? next_mesh_line_x - start.x : ry - start.y; - e_position = start.e + on_axis_distance * e_normalized_dist; - z_position = start.z + on_axis_distance * z_normalized_dist; + on_axis_distance = use_x_dist ? dest.x - start.x : dest.y - start.y; + TERN_(HAS_EXTRUDERS, dest.e = start.e + on_axis_distance * e_normalized_dist); + dest.z = start.z + on_axis_distance * z_normalized_dist; } else { - e_position = end.e; - z_position = end.z; + TERN_(HAS_EXTRUDERS, dest.e = end.e); + dest.z = end.z; } - if (!planner.buffer_segment(next_mesh_line_x, ry, z_position + z0, e_position, scaled_fr_mm_s, extruder)) - break; + dest.z += z0; + if (!planner.buffer_segment(dest, scaled_fr_mm_s, extruder)) break; + icell.x += iadd.x; cnt.x--; } @@ -309,6 +324,8 @@ #define DELTA_SEGMENT_MIN_LENGTH 0.25 // SCARA minimum segment size is 0.25mm #elif ENABLED(DELTA) #define DELTA_SEGMENT_MIN_LENGTH 0.10 // mm (still subject to DELTA_SEGMENTS_PER_SECOND) + #elif ENABLED(POLARGRAPH) + #define DELTA_SEGMENT_MIN_LENGTH 0.10 // mm (still subject to DELTA_SEGMENTS_PER_SECOND) #else // CARTESIAN #ifdef LEVELED_SEGMENT_LENGTH #define DELTA_SEGMENT_MIN_LENGTH LEVELED_SEGMENT_LENGTH @@ -323,7 +340,7 @@ * Returns true if did NOT move, false if moved (requires current_position update). */ - bool _O2 unified_bed_leveling::line_to_destination_segmented(const feedRate_t &scaled_fr_mm_s) { + bool _O2 unified_bed_leveling::line_to_destination_segmented(const_feedRate_t scaled_fr_mm_s) { if (!position_is_reachable(destination)) // fail if moving outside reachable boundary return true; // did not move, so current_position still accurate @@ -362,15 +379,11 @@ while (--segments) { raw += diff; planner.buffer_line(raw, scaled_fr_mm_s, active_extruder, segment_xyz_mm - #if ENABLED(SCARA_FEEDRATE_SCALING) - , inv_duration - #endif + OPTARG(SCARA_FEEDRATE_SCALING, inv_duration) ); } planner.buffer_line(destination, scaled_fr_mm_s, active_extruder, segment_xyz_mm - #if ENABLED(SCARA_FEEDRATE_SCALING) - , inv_duration - #endif + OPTARG(SCARA_FEEDRATE_SCALING, inv_duration) ); return false; // Did not set current from destination } @@ -397,8 +410,8 @@ int8_t((raw.x - (MESH_MIN_X)) * RECIPROCAL(MESH_X_DIST)), int8_t((raw.y - (MESH_MIN_Y)) * RECIPROCAL(MESH_Y_DIST)) }; - LIMIT(icell.x, 0, (GRID_MAX_POINTS_X) - 1); - LIMIT(icell.y, 0, (GRID_MAX_POINTS_Y) - 1); + LIMIT(icell.x, 0, GRID_MAX_CELLS_X); + LIMIT(icell.y, 0, GRID_MAX_CELLS_Y); float z_x0y0 = z_values[icell.x ][icell.y ], // z at lower left corner z_x1y0 = z_values[icell.x+1][icell.y ], // z at upper left corner @@ -442,11 +455,9 @@ #endif ; - planner.buffer_line(raw.x, raw.y, raw.z + z_cxcy, raw.e, scaled_fr_mm_s, active_extruder, segment_xyz_mm - #if ENABLED(SCARA_FEEDRATE_SCALING) - , inv_duration - #endif - ); + const float oldz = raw.z; raw.z += z_cxcy; + planner.buffer_line(raw, scaled_fr_mm_s, active_extruder, segment_xyz_mm OPTARG(SCARA_FEEDRATE_SCALING, inv_duration) ); + raw.z = oldz; if (segments == 0) // done with last segment return false; // didn't set current from destination diff --git a/Marlin/src/feature/binary_stream.h b/Marlin/src/feature/binary_stream.h index d092b7152f5d..417e39c74507 100644 --- a/Marlin/src/feature/binary_stream.h +++ b/Marlin/src/feature/binary_stream.h @@ -24,32 +24,29 @@ #include "../inc/MarlinConfig.h" #define BINARY_STREAM_COMPRESSION - #if ENABLED(BINARY_STREAM_COMPRESSION) #include "../libs/heatshrink/heatshrink_decoder.h" + // STM32 (and others?) require a word-aligned buffer for SD card transfers via DMA + static __attribute__((aligned(sizeof(size_t)))) uint8_t decode_buffer[512] = {}; + static heatshrink_decoder hsd; #endif -inline bool bs_serial_data_available(const uint8_t index) { +inline bool bs_serial_data_available(const serial_index_t index) { return SERIAL_IMPL.available(index); } -inline int bs_read_serial(const uint8_t index) { +inline int bs_read_serial(const serial_index_t index) { return SERIAL_IMPL.read(index); } -#if ENABLED(BINARY_STREAM_COMPRESSION) - static heatshrink_decoder hsd; - static uint8_t decode_buffer[512] = {}; -#endif - class SDFileTransferProtocol { private: struct Packet { struct [[gnu::packed]] Open { - static bool validate(char* buffer, size_t length) { + static bool validate(char *buffer, size_t length) { return (length > sizeof(Open) && buffer[length - 1] == '\0'); } - static Open& decode(char* buffer) { + static Open& decode(char *buffer) { data = &buffer[2]; return *reinterpret_cast(buffer); } @@ -62,7 +59,7 @@ class SDFileTransferProtocol { }; }; - static bool file_open(char* filename) { + static bool file_open(char *filename) { if (!dummy_transfer) { card.mount(); card.openFileWrite(filename); @@ -74,7 +71,7 @@ class SDFileTransferProtocol { return true; } - static bool file_write(char* buffer, const size_t length) { + static bool file_write(char *buffer, const size_t length) { #if ENABLED(BINARY_STREAM_COMPRESSION) if (compression) { size_t total_processed = 0, processed_count = 0; @@ -145,15 +142,15 @@ class SDFileTransferProtocol { } } - static void process(uint8_t packet_type, char* buffer, const uint16_t length) { + static void process(uint8_t packet_type, char *buffer, const uint16_t length) { transfer_timeout = millis() + TIMEOUT; switch (static_cast(packet_type)) { case FileTransfer::QUERY: - SERIAL_ECHOPAIR("PFT:version:", VERSION_MAJOR, ".", VERSION_MINOR, ".", VERSION_PATCH); + SERIAL_ECHOPGM("PFT:version:", VERSION_MAJOR, ".", VERSION_MINOR, ".", VERSION_PATCH); #if ENABLED(BINARY_STREAM_COMPRESSION) - SERIAL_ECHOLNPAIR(":compresion:heatshrink,", HEATSHRINK_STATIC_WINDOW_BITS, ",", HEATSHRINK_STATIC_LOOKAHEAD_BITS); + SERIAL_ECHOLNPGM(":compression:heatshrink,", HEATSHRINK_STATIC_WINDOW_BITS, ",", HEATSHRINK_STATIC_LOOKAHEAD_BITS); #else - SERIAL_ECHOLNPGM(":compresion:none"); + SERIAL_ECHOLNPGM(":compression:none"); #endif break; case FileTransfer::OPEN: @@ -325,7 +322,7 @@ class BinaryStream { if (packet.header.checksum == packet.header_checksum) { // The SYNC control packet is a special case in that it doesn't require the stream sync to be correct if (static_cast(packet.header.protocol()) == Protocol::CONTROL && static_cast(packet.header.type()) == ProtocolControl::SYNC) { - SERIAL_ECHOLNPAIR("ss", sync, ",", buffer_size, ",", VERSION_MAJOR, ".", VERSION_MINOR, ".", VERSION_PATCH); + SERIAL_ECHOLNPGM("ss", sync, ",", buffer_size, ",", VERSION_MAJOR, ".", VERSION_MINOR, ".", VERSION_PATCH); stream_state = StreamState::PACKET_RESET; break; } @@ -340,7 +337,7 @@ class BinaryStream { stream_state = StreamState::PACKET_PROCESS; } else if (packet.header.sync == sync - 1) { // ok response must have been lost - SERIAL_ECHOLNPAIR("ok", packet.header.sync); // transmit valid packet received and drop the payload + SERIAL_ECHOLNPGM("ok", packet.header.sync); // transmit valid packet received and drop the payload stream_state = StreamState::PACKET_RESET; } else if (packet_retries) { @@ -396,7 +393,7 @@ class BinaryStream { packet_retries = 0; bytes_received += packet.header.size; - SERIAL_ECHOLNPAIR("ok", packet.header.sync); // transmit valid packet received + SERIAL_ECHOLNPGM("ok", packet.header.sync); // transmit valid packet received dispatch(); stream_state = StreamState::PACKET_RESET; break; @@ -405,7 +402,7 @@ class BinaryStream { packet_retries++; stream_state = StreamState::PACKET_RESET; SERIAL_ECHO_MSG("Resend request ", packet_retries); - SERIAL_ECHOLNPAIR("rs", sync); + SERIAL_ECHOLNPGM("rs", sync); } else stream_state = StreamState::PACKET_ERROR; @@ -415,7 +412,7 @@ class BinaryStream { stream_state = StreamState::PACKET_RESEND; break; case StreamState::PACKET_ERROR: - SERIAL_ECHOLNPAIR("fe", packet.header.sync); + SERIAL_ECHOLNPGM("fe", packet.header.sync); reset(); // reset everything, resync required stream_state = StreamState::PACKET_RESET; break; diff --git a/Marlin/src/feature/bltouch.cpp b/Marlin/src/feature/bltouch.cpp index 7fccc52d0584..b1cc30bee0d9 100644 --- a/Marlin/src/feature/bltouch.cpp +++ b/Marlin/src/feature/bltouch.cpp @@ -28,7 +28,12 @@ BLTouch bltouch; -bool BLTouch::last_written_mode; // Initialized by settings.load, 0 = Open Drain; 1 = 5V Drain +bool BLTouch::od_5v_mode; // Initialized by settings.load, 0 = Open Drain; 1 = 5V Drain +#ifdef BLTOUCH_HS_MODE + bool BLTouch::high_speed_mode; // Initialized by settings.load, 0 = Low Speed; 1 = High Speed +#else + constexpr bool BLTouch::high_speed_mode; +#endif #include "../module/servo.h" #include "../module/probe.h" @@ -39,7 +44,7 @@ void stop(); #include "../core/debug_out.h" bool BLTouch::command(const BLTCommand cmd, const millis_t &ms) { - if (DEBUGGING(LEVELING)) SERIAL_ECHOLNPAIR("BLTouch Command :", cmd); + if (DEBUGGING(LEVELING)) SERIAL_ECHOLNPGM("BLTouch Command :", cmd); MOVE_SERVO(Z_PROBE_SERVO_NR, cmd); safe_delay(_MAX(ms, (uint32_t)BLTOUCH_DELAY)); // BLTOUCH_DELAY is also the *minimum* delay return triggered(); @@ -63,18 +68,17 @@ void BLTouch::init(const bool set_voltage/*=false*/) { #else - if (DEBUGGING(LEVELING)) { - DEBUG_ECHOLNPAIR("last_written_mode - ", last_written_mode); - DEBUG_ECHOLNPGM("config mode - " - #if ENABLED(BLTOUCH_SET_5V_MODE) - "BLTOUCH_SET_5V_MODE" - #else - "OD" - #endif - ); - } + #ifdef DEBUG_OUT + if (DEBUGGING(LEVELING)) { + PGMSTR(mode0, "OD"); + PGMSTR(mode1, "5V"); + DEBUG_ECHOPGM("BLTouch Mode: "); + DEBUG_ECHOPGM_P(bltouch.od_5v_mode ? mode1 : mode0); + DEBUG_ECHOLNPGM(" (Default " TERN(BLTOUCH_SET_5V_MODE, "5V", "OD") ")"); + } + #endif - const bool should_set = last_written_mode != ENABLED(BLTOUCH_SET_5V_MODE); + const bool should_set = od_5v_mode != ENABLED(BLTOUCH_SET_5V_MODE); #endif @@ -175,7 +179,7 @@ bool BLTouch::status_proc() { _set_SW_mode(); // Incidentally, _set_SW_mode() will also RESET any active alarm const bool tr = triggered(); // If triggered in SW mode, the pin is up, it is STOWED - if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPAIR("BLTouch is ", tr); + if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPGM("BLTouch is ", tr); if (tr) _stow(); else _deploy(); // Turn off SW mode, reset any trigger, honor pin state return !tr; @@ -187,13 +191,13 @@ void BLTouch::mode_conv_proc(const bool M5V) { * BLTOUCH V3.0: This will set the mode (twice) and sadly, a STOW is needed at the end, because of the deploy * BLTOUCH V3.1: This will set the mode and store it in the eeprom. The STOW is not needed but does not hurt */ - if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPAIR("BLTouch Set Mode - ", M5V); + if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPGM("BLTouch Set Mode - ", M5V); _deploy(); if (M5V) _set_5V_mode(); else _set_OD_mode(); _mode_store(); if (M5V) _set_5V_mode(); else _set_OD_mode(); _stow(); - last_written_mode = M5V; + od_5v_mode = M5V; } #endif // BLTOUCH diff --git a/Marlin/src/feature/bltouch.h b/Marlin/src/feature/bltouch.h index 8bd41f03e404..fa857bb96ab6 100644 --- a/Marlin/src/feature/bltouch.h +++ b/Marlin/src/feature/bltouch.h @@ -23,10 +23,6 @@ #include "../inc/MarlinConfigPre.h" -#if DISABLED(BLTOUCH_HS_MODE) - #define BLTOUCH_SLOW_MODE 1 -#endif - // BLTouch commands are sent as servo angles typedef unsigned char BLTCommand; @@ -70,37 +66,46 @@ typedef unsigned char BLTCommand; class BLTouch { public: + static void init(const bool set_voltage=false); - static bool last_written_mode; // Initialized by settings.load, 0 = Open Drain; 1 = 5V Drain + static bool od_5v_mode; // Initialized by settings.load, 0 = Open Drain; 1 = 5V Drain + + #ifdef BLTOUCH_HS_MODE + static bool high_speed_mode; // Initialized by settings.load, 0 = Low Speed; 1 = High Speed + #else + static constexpr bool high_speed_mode = false; + #endif + + static float z_extra_clearance() { return high_speed_mode ? 7 : 0; } // DEPLOY and STOW are wrapped for error handling - these are used by homing and by probing - FORCE_INLINE static bool deploy() { return deploy_proc(); } - FORCE_INLINE static bool stow() { return stow_proc(); } - FORCE_INLINE static bool status() { return status_proc(); } + static bool deploy() { return deploy_proc(); } + static bool stow() { return stow_proc(); } + static bool status() { return status_proc(); } // Native BLTouch commands ("Underscore"...), used in lcd menus and internally - FORCE_INLINE static void _reset() { command(BLTOUCH_RESET, BLTOUCH_RESET_DELAY); } + static void _reset() { command(BLTOUCH_RESET, BLTOUCH_RESET_DELAY); } - FORCE_INLINE static void _selftest() { command(BLTOUCH_SELFTEST, BLTOUCH_DELAY); } + static void _selftest() { command(BLTOUCH_SELFTEST, BLTOUCH_DELAY); } - FORCE_INLINE static void _set_SW_mode() { command(BLTOUCH_SW_MODE, BLTOUCH_DELAY); } - FORCE_INLINE static void _reset_SW_mode() { if (triggered()) _stow(); else _deploy(); } + static void _set_SW_mode() { command(BLTOUCH_SW_MODE, BLTOUCH_DELAY); } + static void _reset_SW_mode() { if (triggered()) _stow(); else _deploy(); } - FORCE_INLINE static void _set_5V_mode() { command(BLTOUCH_5V_MODE, BLTOUCH_SET5V_DELAY); } - FORCE_INLINE static void _set_OD_mode() { command(BLTOUCH_OD_MODE, BLTOUCH_SETOD_DELAY); } - FORCE_INLINE static void _mode_store() { command(BLTOUCH_MODE_STORE, BLTOUCH_MODE_STORE_DELAY); } + static void _set_5V_mode() { command(BLTOUCH_5V_MODE, BLTOUCH_SET5V_DELAY); } + static void _set_OD_mode() { command(BLTOUCH_OD_MODE, BLTOUCH_SETOD_DELAY); } + static void _mode_store() { command(BLTOUCH_MODE_STORE, BLTOUCH_MODE_STORE_DELAY); } - FORCE_INLINE static void _deploy() { command(BLTOUCH_DEPLOY, BLTOUCH_DEPLOY_DELAY); } - FORCE_INLINE static void _stow() { command(BLTOUCH_STOW, BLTOUCH_STOW_DELAY); } + static void _deploy() { command(BLTOUCH_DEPLOY, BLTOUCH_DEPLOY_DELAY); } + static void _stow() { command(BLTOUCH_STOW, BLTOUCH_STOW_DELAY); } - FORCE_INLINE static void mode_conv_5V() { mode_conv_proc(true); } - FORCE_INLINE static void mode_conv_OD() { mode_conv_proc(false); } + static void mode_conv_5V() { mode_conv_proc(true); } + static void mode_conv_OD() { mode_conv_proc(false); } static bool triggered(); private: - FORCE_INLINE static bool _deploy_query_alarm() { return command(BLTOUCH_DEPLOY, BLTOUCH_DEPLOY_DELAY); } - FORCE_INLINE static bool _stow_query_alarm() { return command(BLTOUCH_STOW, BLTOUCH_STOW_DELAY) == STOW_ALARM; } + static bool _deploy_query_alarm() { return command(BLTOUCH_DEPLOY, BLTOUCH_DEPLOY_DELAY); } + static bool _stow_query_alarm() { return command(BLTOUCH_STOW, BLTOUCH_STOW_DELAY) == STOW_ALARM; } static void clear(); static bool command(const BLTCommand cmd, const millis_t &ms); diff --git a/Marlin/src/feature/cancel_object.cpp b/Marlin/src/feature/cancel_object.cpp index e2e429ea10e1..bffd2bb72020 100644 --- a/Marlin/src/feature/cancel_object.cpp +++ b/Marlin/src/feature/cancel_object.cpp @@ -19,6 +19,7 @@ * along with this program. If not, see . * */ + #include "../inc/MarlinConfig.h" #if ENABLED(CANCEL_OBJECTS) @@ -43,9 +44,9 @@ void CancelObject::set_active_object(const int8_t obj) { else skipping = false; - #if HAS_DISPLAY + #if BOTH(HAS_STATUS_MESSAGE, CANCEL_OBJECTS_REPORTING) if (active_object >= 0) - ui.status_printf_P(0, PSTR(S_FMT " %i"), GET_TEXT(MSG_PRINTING_OBJECT), int(active_object)); + ui.status_printf(0, F(S_FMT " %i"), GET_TEXT(MSG_PRINTING_OBJECT), int(active_object)); else ui.reset_status(); #endif diff --git a/Marlin/src/feature/cancel_object.h b/Marlin/src/feature/cancel_object.h index 1d2d77f20334..62548a371937 100644 --- a/Marlin/src/feature/cancel_object.h +++ b/Marlin/src/feature/cancel_object.h @@ -32,10 +32,10 @@ class CancelObject { static void cancel_object(const int8_t obj); static void uncancel_object(const int8_t obj); static void report(); - static inline bool is_canceled(const int8_t obj) { return TEST(canceled, obj); } - static inline void clear_active_object() { set_active_object(-1); } - static inline void cancel_active_object() { cancel_object(active_object); } - static inline void reset() { canceled = 0x0000; object_count = 0; clear_active_object(); } + static bool is_canceled(const int8_t obj) { return TEST(canceled, obj); } + static void clear_active_object() { set_active_object(-1); } + static void cancel_active_object() { cancel_object(active_object); } + static void reset() { canceled = 0x0000; object_count = 0; clear_active_object(); } }; extern CancelObject cancelable; diff --git a/Marlin/src/feature/caselight.cpp b/Marlin/src/feature/caselight.cpp index d4cc6b150466..eb580a6d6269 100644 --- a/Marlin/src/feature/caselight.cpp +++ b/Marlin/src/feature/caselight.cpp @@ -28,10 +28,6 @@ CaseLight caselight; -#if CASE_LIGHT_IS_COLOR_LED - #include "leds/leds.h" -#endif - #if CASELIGHT_USES_BRIGHTNESS && !defined(CASE_LIGHT_DEFAULT_BRIGHTNESS) #define CASE_LIGHT_DEFAULT_BRIGHTNESS 0 // For use on PWM pin as non-PWM just sets a default #endif @@ -43,17 +39,8 @@ CaseLight caselight; bool CaseLight::on = CASE_LIGHT_DEFAULT_ON; #if CASE_LIGHT_IS_COLOR_LED - LEDColor CaseLight::color = - #ifdef CASE_LIGHT_DEFAULT_COLOR - CASE_LIGHT_DEFAULT_COLOR - #else - { 255, 255, 255, 255 } - #endif - ; -#endif - -#ifndef INVERT_CASE_LIGHT - #define INVERT_CASE_LIGHT false + constexpr uint8_t init_case_light[] = CASE_LIGHT_DEFAULT_COLOR; + LEDColor CaseLight::color = { init_case_light[0], init_case_light[1], init_case_light[2] OPTARG(HAS_WHITE_LED, init_case_light[3]) }; #endif void CaseLight::update(const bool sflag) { @@ -72,21 +59,27 @@ void CaseLight::update(const bool sflag) { if (sflag && on) brightness = brightness_sav; // Restore last brightness for M355 S1 - const uint8_t i = on ? brightness : 0, n10ct = INVERT_CASE_LIGHT ? 255 - i : i; + const uint8_t i = on ? brightness : 0, n10ct = ENABLED(INVERT_CASE_LIGHT) ? 255 - i : i; + UNUSED(n10ct); #endif #if CASE_LIGHT_IS_COLOR_LED - - leds.set_color( - MakeLEDColor(color.r, color.g, color.b, color.w, n10ct), - false - ); - + #if ENABLED(CASE_LIGHT_USE_NEOPIXEL) + if (on) + // Use current color of (NeoPixel) leds and new brightness level + leds.set_color(LEDColor(leds.color.r, leds.color.g, leds.color.b OPTARG(HAS_WHITE_LED, leds.color.w) OPTARG(NEOPIXEL_LED, n10ct))); + else + // Switch off leds + leds.set_off(); + #else + // Use CaseLight color (CASE_LIGHT_DEFAULT_COLOR) and new brightness level + leds.set_color(LEDColor(color.r, color.g, color.b OPTARG(HAS_WHITE_LED, color.w) OPTARG(NEOPIXEL_LED, n10ct))); + #endif #else // !CASE_LIGHT_IS_COLOR_LED #if CASELIGHT_USES_BRIGHTNESS if (pin_is_pwm()) - analogWrite(pin_t(CASE_LIGHT_PIN), ( + hal.set_pwm_duty(pin_t(CASE_LIGHT_PIN), ( #if CASE_LIGHT_MAX_PWM == 255 n10ct #else @@ -96,7 +89,7 @@ void CaseLight::update(const bool sflag) { else #endif { - const bool s = on ? !INVERT_CASE_LIGHT : INVERT_CASE_LIGHT; + const bool s = on ? TERN(INVERT_CASE_LIGHT, LOW, HIGH) : TERN(INVERT_CASE_LIGHT, HIGH, LOW); WRITE(CASE_LIGHT_PIN, s ? HIGH : LOW); } diff --git a/Marlin/src/feature/caselight.h b/Marlin/src/feature/caselight.h index 25bcb486faaf..17e1222acbfa 100644 --- a/Marlin/src/feature/caselight.h +++ b/Marlin/src/feature/caselight.h @@ -27,14 +27,12 @@ #include "leds/leds.h" // for LEDColor #endif -#if DISABLED(CASE_LIGHT_NO_BRIGHTNESS) || ENABLED(CASE_LIGHT_USE_NEOPIXEL) - #define CASELIGHT_USES_BRIGHTNESS 1 -#endif - class CaseLight { public: static bool on; - TERN_(CASELIGHT_USES_BRIGHTNESS, static uint8_t brightness); + #if ENABLED(CASELIGHT_USES_BRIGHTNESS) + static uint8_t brightness; + #endif static bool pin_is_pwm() { return TERN0(NEED_CASE_LIGHT_PIN, PWM_PIN(CASE_LIGHT_PIN)); } static bool has_brightness() { return TERN0(CASELIGHT_USES_BRIGHTNESS, TERN(CASE_LIGHT_USE_NEOPIXEL, true, pin_is_pwm())); } @@ -47,11 +45,13 @@ class CaseLight { } static void update(const bool sflag); - static inline void update_brightness() { update(false); } - static inline void update_enabled() { update(true); } + static void update_brightness() { update(false); } + static void update_enabled() { update(true); } -private: - TERN_(CASE_LIGHT_IS_COLOR_LED, static LEDColor color); + #if ENABLED(CASE_LIGHT_IS_COLOR_LED) + private: + static LEDColor color; + #endif }; extern CaseLight caselight; diff --git a/Marlin/src/feature/closedloop.cpp b/Marlin/src/feature/closedloop.cpp index 8a97f0c0cd7b..1b9f711a6bbd 100644 --- a/Marlin/src/feature/closedloop.cpp +++ b/Marlin/src/feature/closedloop.cpp @@ -19,6 +19,7 @@ * along with this program. If not, see . * */ + #include "../inc/MarlinConfig.h" #if ENABLED(EXTERNAL_CLOSED_LOOP_CONTROLLER) diff --git a/Marlin/src/feature/controllerfan.cpp b/Marlin/src/feature/controllerfan.cpp index 020646775253..f42bf52ae40a 100644 --- a/Marlin/src/feature/controllerfan.cpp +++ b/Marlin/src/feature/controllerfan.cpp @@ -25,7 +25,7 @@ #if ENABLED(USE_CONTROLLER_FAN) #include "controllerfan.h" -#include "../module/stepper/indirection.h" +#include "../module/stepper.h" #include "../module/temperature.h" ControllerFan controllerFan; @@ -54,31 +54,15 @@ void ControllerFan::update() { if (ELAPSED(ms, nextMotorCheck)) { nextMotorCheck = ms + 2500UL; // Not a time critical function, so only check every 2.5s - #define MOTOR_IS_ON(A,B) (A##_ENABLE_READ() == bool(B##_ENABLE_ON)) - #define _OR_ENABLED_E(N) || MOTOR_IS_ON(E##N,E) - - const bool motor_on = ( - ( DISABLED(CONTROLLER_FAN_IGNORE_Z) && - ( MOTOR_IS_ON(Z,Z) - || TERN0(HAS_Z2_ENABLE, MOTOR_IS_ON(Z2,Z)) - || TERN0(HAS_Z3_ENABLE, MOTOR_IS_ON(Z3,Z)) - || TERN0(HAS_Z4_ENABLE, MOTOR_IS_ON(Z4,Z)) - ) - ) || ( - DISABLED(CONTROLLER_FAN_USE_Z_ONLY) && - ( MOTOR_IS_ON(X,X) || MOTOR_IS_ON(Y,Y) - || TERN0(HAS_X2_ENABLE, MOTOR_IS_ON(X2,X)) - || TERN0(HAS_Y2_ENABLE, MOTOR_IS_ON(Y2,Y)) - #if E_STEPPERS - REPEAT(E_STEPPERS, _OR_ENABLED_E) - #endif - ) - ) - ); - - // If any of the drivers or the heated bed are enabled... - if (motor_on || TERN0(HAS_HEATED_BED, thermalManager.temp_bed.soft_pwm_amount > 0)) - lastMotorOn = ms; //... set time to NOW so the fan will turn on + // If any triggers for the controller fan are true... + // - At least one stepper driver is enabled + // - The heated bed is enabled + // - TEMP_SENSOR_BOARD is reporting >= CONTROLLER_FAN_MIN_BOARD_TEMP + const ena_mask_t axis_mask = TERN(CONTROLLER_FAN_USE_Z_ONLY, _BV(Z_AXIS), (ena_mask_t)~TERN0(CONTROLLER_FAN_IGNORE_Z, _BV(Z_AXIS))); + if ( (stepper.axis_enabled.bits & axis_mask) + || TERN0(HAS_HEATED_BED, thermalManager.temp_bed.soft_pwm_amount > 0) + || TERN0(HAS_CONTROLLER_FAN_MIN_BOARD_TEMP, thermalManager.wholeDegBoard() >= CONTROLLER_FAN_MIN_BOARD_TEMP) + ) lastMotorOn = ms; //... set time to NOW so the fan will turn on // Fan Settings. Set fan > 0: // - If AutoMode is on and steppers have been enabled for CONTROLLERFAN_IDLE_TIME seconds. @@ -88,9 +72,14 @@ void ControllerFan::update() { ? settings.active_speed : settings.idle_speed ); - // Allow digital or PWM fan output (see M42 handling) - WRITE(CONTROLLER_FAN_PIN, speed); - analogWrite(pin_t(CONTROLLER_FAN_PIN), speed); + #if ENABLED(FAN_SOFT_PWM) + thermalManager.soft_pwm_controller_speed = speed; + #else + if (PWM_PIN(CONTROLLER_FAN_PIN)) + hal.set_pwm_duty(pin_t(CONTROLLER_FAN_PIN), speed); + else + WRITE(CONTROLLER_FAN_PIN, speed > 0); + #endif } } diff --git a/Marlin/src/feature/controllerfan.h b/Marlin/src/feature/controllerfan.h index 55f2d5cfc7aa..55eb2359b067 100644 --- a/Marlin/src/feature/controllerfan.h +++ b/Marlin/src/feature/controllerfan.h @@ -60,9 +60,9 @@ class ControllerFan { #else static const controllerFan_settings_t &settings; #endif - static inline bool state() { return speed > 0; } - static inline void init() { reset(); } - static inline void reset() { TERN_(CONTROLLER_FAN_EDITABLE, settings = controllerFan_defaults); } + static bool state() { return speed > 0; } + static void init() { reset(); } + static void reset() { TERN_(CONTROLLER_FAN_EDITABLE, settings = controllerFan_defaults); } static void setup(); static void update(); }; diff --git a/Marlin/src/feature/cooler.cpp b/Marlin/src/feature/cooler.cpp new file mode 100644 index 000000000000..e0f99777d19a --- /dev/null +++ b/Marlin/src/feature/cooler.cpp @@ -0,0 +1,48 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2021 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + * + */ + +#include "../inc/MarlinConfig.h" + +#if EITHER(HAS_COOLER, LASER_COOLANT_FLOW_METER) + +#include "cooler.h" +Cooler cooler; + +#if HAS_COOLER + uint8_t Cooler::mode = 0; + uint16_t Cooler::capacity; + uint16_t Cooler::load; + bool Cooler::enabled = false; +#endif + +#if ENABLED(LASER_COOLANT_FLOW_METER) + bool Cooler::flowmeter = false; + millis_t Cooler::flowmeter_next_ms; // = 0 + volatile uint16_t Cooler::flowpulses; + float Cooler::flowrate; + #if ENABLED(FLOWMETER_SAFETY) + bool Cooler::flowsafety_enabled = true; + bool Cooler::flowfault = false; + #endif +#endif + +#endif // HAS_COOLER || LASER_COOLANT_FLOW_METER diff --git a/Marlin/src/feature/cooler.h b/Marlin/src/feature/cooler.h new file mode 100644 index 000000000000..9891514e23c9 --- /dev/null +++ b/Marlin/src/feature/cooler.h @@ -0,0 +1,109 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2021 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + * + */ +#pragma once + +#include "../inc/MarlinConfigPre.h" + +#ifndef FLOWMETER_PPL + #define FLOWMETER_PPL 5880 // Pulses per liter +#endif +#ifndef FLOWMETER_INTERVAL + #define FLOWMETER_INTERVAL 1000 // milliseconds +#endif + +// Cooling device + +class Cooler { +public: + static uint16_t capacity; // Cooling capacity in watts + static uint16_t load; // Cooling load in watts + + static bool enabled; + static void enable() { enabled = true; } + static void disable() { enabled = false; } + static void toggle() { enabled = !enabled; } + + static uint8_t mode; // 0 = CO2 Liquid cooling, 1 = Laser Diode TEC Heatsink Cooling + static void set_mode(const uint8_t m) { mode = m; } + + #if ENABLED(LASER_COOLANT_FLOW_METER) + static float flowrate; // Flow meter reading in liters-per-minute. + static bool flowmeter; // Flag to monitor the flow + static volatile uint16_t flowpulses; // Flowmeter IRQ pulse count + static millis_t flowmeter_next_ms; // Next time at which to calculate flow + + static void set_flowmeter(const bool sflag) { + if (flowmeter != sflag) { + flowmeter = sflag; + if (sflag) { + flowpulses = 0; + flowmeter_next_ms = millis() + FLOWMETER_INTERVAL; + } + } + } + + // To calculate flow we only need to count pulses + static void flowmeter_ISR() { flowpulses++; } + + // Enable / Disable the flow meter interrupt + static void flowmeter_interrupt_enable() { + attachInterrupt(digitalPinToInterrupt(FLOWMETER_PIN), flowmeter_ISR, RISING); + } + static void flowmeter_interrupt_disable() { + detachInterrupt(digitalPinToInterrupt(FLOWMETER_PIN)); + } + + // Enable / Disable the flow meter interrupt + static void flowmeter_enable() { set_flowmeter(true); flowpulses = 0; flowmeter_interrupt_enable(); } + static void flowmeter_disable() { set_flowmeter(false); flowmeter_interrupt_disable(); flowpulses = 0; } + + // Get the total flow (in liters per minute) since the last reading + static void calc_flowrate() { + // flowrate = (litres) * (seconds) = litres per minute + flowrate = (flowpulses / (float)FLOWMETER_PPL) * ((1000.0f / (float)FLOWMETER_INTERVAL) * 60.0f); + flowpulses = 0; + } + + // Userland task to update the flow meter + static void flowmeter_task(const millis_t ms=millis()) { + if (!flowmeter) // !! The flow meter must always be on !! + flowmeter_enable(); // Init and prime + if (ELAPSED(ms, flowmeter_next_ms)) { + calc_flowrate(); + flowmeter_next_ms = ms + FLOWMETER_INTERVAL; + } + } + + #if ENABLED(FLOWMETER_SAFETY) + static bool flowfault; // Flag that the cooler is in a fault state + static bool flowsafety_enabled; // Flag to disable the cutter if flow rate is too low + static void flowsafety_toggle() { flowsafety_enabled = !flowsafety_enabled; } + static bool check_flow_too_low() { + const bool too_low = flowsafety_enabled && flowrate < (FLOWMETER_MIN_LITERS_PER_MINUTE); + flowfault = too_low; + return too_low; + } + #endif + #endif +}; + +extern Cooler cooler; diff --git a/Marlin/src/feature/dac/dac_dac084s085.cpp b/Marlin/src/feature/dac/dac_dac084s085.cpp index 649aa5561bfa..b88aaf802bdf 100644 --- a/Marlin/src/feature/dac/dac_dac084s085.cpp +++ b/Marlin/src/feature/dac/dac_dac084s085.cpp @@ -20,35 +20,35 @@ void dac084s085::begin() { uint8_t externalDac_buf[] = { 0x20, 0x00 }; // all off // All SPI chip-select HIGH - SET_OUTPUT(DAC0_SYNC); + SET_OUTPUT(DAC0_SYNC_PIN); #if HAS_MULTI_EXTRUDER - SET_OUTPUT(DAC1_SYNC); + SET_OUTPUT(DAC1_SYNC_PIN); #endif cshigh(); spiBegin(); //init onboard DAC DELAY_US(2); - WRITE(DAC0_SYNC, LOW); + WRITE(DAC0_SYNC_PIN, LOW); DELAY_US(2); - WRITE(DAC0_SYNC, HIGH); + WRITE(DAC0_SYNC_PIN, HIGH); DELAY_US(2); - WRITE(DAC0_SYNC, LOW); + WRITE(DAC0_SYNC_PIN, LOW); spiSend(SPI_CHAN_DAC, externalDac_buf, COUNT(externalDac_buf)); - WRITE(DAC0_SYNC, HIGH); + WRITE(DAC0_SYNC_PIN, HIGH); #if HAS_MULTI_EXTRUDER //init Piggy DAC DELAY_US(2); - WRITE(DAC1_SYNC, LOW); + WRITE(DAC1_SYNC_PIN, LOW); DELAY_US(2); - WRITE(DAC1_SYNC, HIGH); + WRITE(DAC1_SYNC_PIN, HIGH); DELAY_US(2); - WRITE(DAC1_SYNC, LOW); + WRITE(DAC1_SYNC_PIN, LOW); spiSend(SPI_CHAN_DAC, externalDac_buf, COUNT(externalDac_buf)); - WRITE(DAC1_SYNC, HIGH); + WRITE(DAC1_SYNC_PIN, HIGH); #endif return; @@ -66,18 +66,18 @@ void dac084s085::setValue(const uint8_t channel, const uint8_t value) { cshigh(); if (channel > 3) { // DAC Piggy E1,E2,E3 - WRITE(DAC1_SYNC, LOW); + WRITE(DAC1_SYNC_PIN, LOW); DELAY_US(2); - WRITE(DAC1_SYNC, HIGH); + WRITE(DAC1_SYNC_PIN, HIGH); DELAY_US(2); - WRITE(DAC1_SYNC, LOW); + WRITE(DAC1_SYNC_PIN, LOW); } else { // DAC onboard X,Y,Z,E0 - WRITE(DAC0_SYNC, LOW); + WRITE(DAC0_SYNC_PIN, LOW); DELAY_US(2); - WRITE(DAC0_SYNC, HIGH); + WRITE(DAC0_SYNC_PIN, HIGH); DELAY_US(2); - WRITE(DAC0_SYNC, LOW); + WRITE(DAC0_SYNC_PIN, LOW); } DELAY_US(2); @@ -85,13 +85,13 @@ void dac084s085::setValue(const uint8_t channel, const uint8_t value) { } void dac084s085::cshigh() { - WRITE(DAC0_SYNC, HIGH); + WRITE(DAC0_SYNC_PIN, HIGH); #if HAS_MULTI_EXTRUDER - WRITE(DAC1_SYNC, HIGH); + WRITE(DAC1_SYNC_PIN, HIGH); #endif - WRITE(SPI_EEPROM1_CS, HIGH); - WRITE(SPI_EEPROM2_CS, HIGH); - WRITE(SPI_FLASH_CS, HIGH); + WRITE(SPI_EEPROM1_CS_PIN, HIGH); + WRITE(SPI_EEPROM2_CS_PIN, HIGH); + WRITE(SPI_FLASH_CS_PIN, HIGH); WRITE(SD_SS_PIN, HIGH); } diff --git a/Marlin/src/feature/dac/dac_mcp4728.cpp b/Marlin/src/feature/dac/dac_mcp4728.cpp index 4f33c4e0502a..6f5a9ee691ac 100644 --- a/Marlin/src/feature/dac/dac_mcp4728.cpp +++ b/Marlin/src/feature/dac/dac_mcp4728.cpp @@ -32,7 +32,7 @@ #include "../../inc/MarlinConfig.h" -#if ENABLED(HAS_MOTOR_CURRENT_DAC) +#if HAS_MOTOR_CURRENT_DAC #include "dac_mcp4728.h" @@ -66,14 +66,14 @@ uint8_t MCP4728::analogWrite(const uint8_t channel, const uint16_t value) { } /** - * Write all input resistor values to EEPROM using SequencialWrite method. + * Write all input resistor values to EEPROM using SequentialWrite method. * This will update both input register and EEPROM value * This will also write current Vref, PowerDown, Gain settings to EEPROM */ uint8_t MCP4728::eepromWrite() { Wire.beginTransmission(I2C_ADDRESS(DAC_DEV_ADDRESS)); Wire.write(SEQWRITE); - LOOP_XYZE(i) { + LOOP_LOGICAL_AXES(i) { Wire.write(DAC_STEPPER_VREF << 7 | DAC_STEPPER_GAIN << 4 | highByte(dac_values[i])); Wire.write(lowByte(dac_values[i])); } @@ -81,7 +81,7 @@ uint8_t MCP4728::eepromWrite() { } /** - * Write Voltage reference setting to all input regiters + * Write Voltage reference setting to all input registers */ uint8_t MCP4728::setVref_all(const uint8_t value) { Wire.beginTransmission(I2C_ADDRESS(DAC_DEV_ADDRESS)); @@ -89,7 +89,7 @@ uint8_t MCP4728::setVref_all(const uint8_t value) { return Wire.endTransmission(); } /** - * Write Gain setting to all input regiters + * Write Gain setting to all input registers */ uint8_t MCP4728::setGain_all(const uint8_t value) { Wire.beginTransmission(I2C_ADDRESS(DAC_DEV_ADDRESS)); @@ -129,13 +129,13 @@ void MCP4728::setDrvPct(xyze_uint_t &pct) { } /** - * FastWrite input register values - All DAC ouput update. refer to DATASHEET 5.6.1 + * FastWrite input register values - All DAC output update. refer to DATASHEET 5.6.1 * DAC Input and PowerDown bits update. * No EEPROM update */ uint8_t MCP4728::fastWrite() { Wire.beginTransmission(I2C_ADDRESS(DAC_DEV_ADDRESS)); - LOOP_XYZE(i) { + LOOP_LOGICAL_AXES(i) { Wire.write(highByte(dac_values[i])); Wire.write(lowByte(dac_values[i])); } diff --git a/Marlin/src/feature/dac/stepper_dac.cpp b/Marlin/src/feature/dac/stepper_dac.cpp index 5170a3551108..ff730e93c62c 100644 --- a/Marlin/src/feature/dac/stepper_dac.cpp +++ b/Marlin/src/feature/dac/stepper_dac.cpp @@ -26,7 +26,7 @@ #include "../../inc/MarlinConfig.h" -#if ENABLED(HAS_MOTOR_CURRENT_DAC) +#if HAS_MOTOR_CURRENT_DAC #include "stepper_dac.h" #include "../../MarlinCore.h" // for SP_X_LBL... @@ -51,7 +51,7 @@ int StepperDAC::init() { mcp4728.setVref_all(DAC_STEPPER_VREF); mcp4728.setGain_all(DAC_STEPPER_GAIN); - if (mcp4728.getDrvPct(0) < 1 || mcp4728.getDrvPct(1) < 1 || mcp4728.getDrvPct(2) < 1 || mcp4728.getDrvPct(3) < 1 ) { + if (mcp4728.getDrvPct(0) < 1 || mcp4728.getDrvPct(1) < 1 || mcp4728.getDrvPct(2) < 1 || mcp4728.getDrvPct(3) < 1) { mcp4728.setDrvPct(dac_channel_pct); mcp4728.eepromWrite(); } @@ -77,7 +77,7 @@ static float dac_amps(int8_t n) { return mcp4728.getValue(dac_order[n]) * 0.125 uint8_t StepperDAC::get_current_percent(const AxisEnum axis) { return mcp4728.getDrvPct(dac_order[axis]); } void StepperDAC::set_current_percents(xyze_uint8_t &pct) { - LOOP_XYZE(i) dac_channel_pct[i] = pct[dac_order[i]]; + LOOP_LOGICAL_AXES(i) dac_channel_pct[i] = pct[dac_order[i]]; mcp4728.setDrvPct(dac_channel_pct); } @@ -85,10 +85,16 @@ void StepperDAC::print_values() { if (!dac_present) return; SERIAL_ECHO_MSG("Stepper current values in % (Amps):"); SERIAL_ECHO_START(); - SERIAL_ECHOPAIR_P( SP_X_LBL, dac_perc(X_AXIS), PSTR(" ("), dac_amps(X_AXIS), PSTR(")")); - SERIAL_ECHOPAIR_P( SP_Y_LBL, dac_perc(Y_AXIS), PSTR(" ("), dac_amps(Y_AXIS), PSTR(")")); - SERIAL_ECHOPAIR_P( SP_Z_LBL, dac_perc(Z_AXIS), PSTR(" ("), dac_amps(Z_AXIS), PSTR(")")); - SERIAL_ECHOLNPAIR_P(SP_E_LBL, dac_perc(E_AXIS), PSTR(" ("), dac_amps(E_AXIS), PSTR(")")); + SERIAL_ECHOPGM_P(SP_X_LBL, dac_perc(X_AXIS), PSTR(" ("), dac_amps(X_AXIS), PSTR(")")); + #if HAS_Y_AXIS + SERIAL_ECHOPGM_P(SP_Y_LBL, dac_perc(Y_AXIS), PSTR(" ("), dac_amps(Y_AXIS), PSTR(")")); + #endif + #if HAS_Z_AXIS + SERIAL_ECHOPGM_P(SP_Z_LBL, dac_perc(Z_AXIS), PSTR(" ("), dac_amps(Z_AXIS), PSTR(")")); + #endif + #if HAS_EXTRUDERS + SERIAL_ECHOLNPGM_P(SP_E_LBL, dac_perc(E_AXIS), PSTR(" ("), dac_amps(E_AXIS), PSTR(")")); + #endif } void StepperDAC::commit_eeprom() { diff --git a/Marlin/src/feature/dac/stepper_dac.h b/Marlin/src/feature/dac/stepper_dac.h index 6836335e9806..26a0f2f95cc8 100644 --- a/Marlin/src/feature/dac/stepper_dac.h +++ b/Marlin/src/feature/dac/stepper_dac.h @@ -34,7 +34,7 @@ class StepperDAC { static void set_current_value(const uint8_t channel, uint16_t val); static void print_values(); static void commit_eeprom(); - static uint8_t get_current_percent(AxisEnum axis); + static uint8_t get_current_percent(const AxisEnum axis); static void set_current_percents(xyze_uint8_t &pct); }; diff --git a/Marlin/src/feature/digipot/digipot_mcp4451.cpp b/Marlin/src/feature/digipot/digipot_mcp4451.cpp index 1b4cf43923a8..ba5ecdad050a 100644 --- a/Marlin/src/feature/digipot/digipot_mcp4451.cpp +++ b/Marlin/src/feature/digipot/digipot_mcp4451.cpp @@ -40,6 +40,9 @@ #elif MB(AZTEEG_X5_MINI, AZTEEG_X5_MINI_WIFI) #define DIGIPOT_I2C_FACTOR 113.5f #define DIGIPOT_I2C_MAX_CURRENT 2.0f +#elif MB(AZTEEG_X5_GT) + #define DIGIPOT_I2C_FACTOR 51.0f + #define DIGIPOT_I2C_MAX_CURRENT 3.0f #else #define DIGIPOT_I2C_FACTOR 106.7f #define DIGIPOT_I2C_MAX_CURRENT 2.5f diff --git a/Marlin/src/feature/direct_stepping.cpp b/Marlin/src/feature/direct_stepping.cpp index 2698b53dd6f7..052e79de41e5 100644 --- a/Marlin/src/feature/direct_stepping.cpp +++ b/Marlin/src/feature/direct_stepping.cpp @@ -19,6 +19,7 @@ * along with this program. If not, see . * */ + #include "../inc/MarlinConfigPre.h" #if ENABLED(DIRECT_STEPPING) @@ -51,13 +52,13 @@ namespace DirectStepping { volatile bool SerialPageManager::fatal_error; template - volatile PageState SerialPageManager::page_states[Cfg::NUM_PAGES]; + volatile PageState SerialPageManager::page_states[Cfg::PAGE_COUNT]; template volatile bool SerialPageManager::page_states_dirty; template - uint8_t SerialPageManager::pages[Cfg::NUM_PAGES][Cfg::PAGE_SIZE]; + uint8_t SerialPageManager::pages[Cfg::PAGE_COUNT][Cfg::PAGE_SIZE]; template uint8_t SerialPageManager::checksum; @@ -73,7 +74,7 @@ namespace DirectStepping { template void SerialPageManager::init() { - for (int i = 0 ; i < Cfg::NUM_PAGES ; i++) + for (int i = 0 ; i < Cfg::PAGE_COUNT ; i++) page_states[i] = PageState::FREE; fatal_error = false; @@ -173,7 +174,7 @@ namespace DirectStepping { template void SerialPageManager::write_responses() { if (fatal_error) { - kill(GET_TEXT(MSG_BAD_PAGE)); + kill(GET_TEXT_F(MSG_BAD_PAGE)); return; } @@ -182,10 +183,10 @@ namespace DirectStepping { SERIAL_CHAR(Cfg::CONTROL_CHAR); constexpr int state_bits = 2; - constexpr int n_bytes = Cfg::NUM_PAGES >> state_bits; + constexpr int n_bytes = Cfg::PAGE_COUNT >> state_bits; volatile uint8_t bits_b[n_bytes] = { 0 }; - for (page_idx_t i = 0 ; i < Cfg::NUM_PAGES ; i++) { + for (page_idx_t i = 0 ; i < Cfg::PAGE_COUNT ; i++) { bits_b[i >> state_bits] |= page_states[i] << ((i * state_bits) & 0x7); } diff --git a/Marlin/src/feature/direct_stepping.h b/Marlin/src/feature/direct_stepping.h index b3007731cdb2..962310281edb 100644 --- a/Marlin/src/feature/direct_stepping.h +++ b/Marlin/src/feature/direct_stepping.h @@ -68,10 +68,10 @@ namespace DirectStepping { static State state; static volatile bool fatal_error; - static volatile PageState page_states[Cfg::NUM_PAGES]; + static volatile PageState page_states[Cfg::PAGE_COUNT]; static volatile bool page_states_dirty; - static uint8_t pages[Cfg::NUM_PAGES][Cfg::PAGE_SIZE]; + static uint8_t pages[Cfg::PAGE_COUNT][Cfg::PAGE_SIZE]; static uint8_t checksum; static write_byte_idx_t write_byte_idx; static page_idx_t write_page_idx; @@ -87,8 +87,8 @@ namespace DirectStepping { struct config_t { static constexpr char CONTROL_CHAR = '!'; - static constexpr int NUM_PAGES = num_pages; - static constexpr int NUM_AXES = num_axes; + static constexpr int PAGE_COUNT = num_pages; + static constexpr int AXIS_COUNT = num_axes; static constexpr int BITS_SEGMENT = bits_segment; static constexpr int DIRECTIONAL = dir ? 1 : 0; static constexpr int SEGMENTS = segments; @@ -96,10 +96,10 @@ namespace DirectStepping { static constexpr int NUM_SEGMENTS = _BV(BITS_SEGMENT); static constexpr int SEGMENT_STEPS = _BV(BITS_SEGMENT - DIRECTIONAL) - 1; static constexpr int TOTAL_STEPS = SEGMENT_STEPS * SEGMENTS; - static constexpr int PAGE_SIZE = (NUM_AXES * BITS_SEGMENT * SEGMENTS) / 8; + static constexpr int PAGE_SIZE = (AXIS_COUNT * BITS_SEGMENT * SEGMENTS) / 8; typedef typename TypeSelector<(PAGE_SIZE>256), uint16_t, uint8_t>::type write_byte_idx_t; - typedef typename TypeSelector<(NUM_PAGES>256), uint16_t, uint8_t>::type page_idx_t; + typedef typename TypeSelector<(PAGE_COUNT>256), uint16_t, uint8_t>::type page_idx_t; }; template diff --git a/Marlin/src/feature/e_parser.h b/Marlin/src/feature/e_parser.h index 659e516787fc..fda1ba144bc4 100644 --- a/Marlin/src/feature/e_parser.h +++ b/Marlin/src/feature/e_parser.h @@ -34,29 +34,41 @@ // External references extern bool wait_for_user, wait_for_heatup; +#if ENABLED(REALTIME_REPORTING_COMMANDS) + // From motion.h, which cannot be included here + void report_current_position_moving(); + void quickpause_stepper(); + void quickresume_stepper(); +#endif + +#if ENABLED(SOFT_RESET_VIA_SERIAL) + void HAL_reboot(); +#endif + class EmergencyParser { public: - // Currently looking for: M108, M112, M410, M876 - enum State : char { + // Currently looking for: M108, M112, M410, M876 S[0-9], S000, P000, R000 + enum State : uint8_t { EP_RESET, EP_N, EP_M, EP_M1, - EP_M10, - EP_M108, - EP_M11, - EP_M112, - EP_M4, - EP_M41, - EP_M410, + EP_M10, EP_M108, + EP_M11, EP_M112, + EP_M4, EP_M41, EP_M410, #if ENABLED(HOST_PROMPT_SUPPORT) - EP_M8, - EP_M87, - EP_M876, - EP_M876S, - EP_M876SN, + EP_M8, EP_M87, EP_M876, EP_M876S, EP_M876SN, + #endif + #if ENABLED(REALTIME_REPORTING_COMMANDS) + EP_S, EP_S0, EP_S00, EP_GRBL_STATUS, + EP_R, EP_R0, EP_R00, EP_GRBL_RESUME, + EP_P, EP_P0, EP_P00, EP_GRBL_PAUSE, + #endif + #if ENABLED(SOFT_RESET_VIA_SERIAL) + EP_ctrl, + EP_K, EP_KI, EP_KIL, EP_KILL, #endif EP_IGNORE // to '\n' }; @@ -71,7 +83,6 @@ class EmergencyParser { EmergencyParser() { enable(); } FORCE_INLINE static void enable() { enabled = true; } - FORCE_INLINE static void disable() { enabled = false; } FORCE_INLINE static void update(State &state, const uint8_t c) { @@ -79,21 +90,56 @@ class EmergencyParser { case EP_RESET: switch (c) { case ' ': case '\n': case '\r': break; - case 'N': state = EP_N; break; - case 'M': state = EP_M; break; - default: state = EP_IGNORE; + case 'N': state = EP_N; break; + case 'M': state = EP_M; break; + #if ENABLED(REALTIME_REPORTING_COMMANDS) + case 'S': state = EP_S; break; + case 'P': state = EP_P; break; + case 'R': state = EP_R; break; + #endif + #if ENABLED(SOFT_RESET_VIA_SERIAL) + case '^': state = EP_ctrl; break; + case 'K': state = EP_K; break; + #endif + default: state = EP_IGNORE; } break; case EP_N: switch (c) { case '0' ... '9': - case '-': case ' ': break; - case 'M': state = EP_M; break; - default: state = EP_IGNORE; + case '-': case ' ': break; + case 'M': state = EP_M; break; + #if ENABLED(REALTIME_REPORTING_COMMANDS) + case 'S': state = EP_S; break; + case 'P': state = EP_P; break; + case 'R': state = EP_R; break; + #endif + default: state = EP_IGNORE; } break; + #if ENABLED(REALTIME_REPORTING_COMMANDS) + case EP_S: state = (c == '0') ? EP_S0 : EP_IGNORE; break; + case EP_S0: state = (c == '0') ? EP_S00 : EP_IGNORE; break; + case EP_S00: state = (c == '0') ? EP_GRBL_STATUS : EP_IGNORE; break; + + case EP_R: state = (c == '0') ? EP_R0 : EP_IGNORE; break; + case EP_R0: state = (c == '0') ? EP_R00 : EP_IGNORE; break; + case EP_R00: state = (c == '0') ? EP_GRBL_RESUME : EP_IGNORE; break; + + case EP_P: state = (c == '0') ? EP_P0 : EP_IGNORE; break; + case EP_P0: state = (c == '0') ? EP_P00 : EP_IGNORE; break; + case EP_P00: state = (c == '0') ? EP_GRBL_PAUSE : EP_IGNORE; break; + #endif + + #if ENABLED(SOFT_RESET_VIA_SERIAL) + case EP_ctrl: state = (c == 'X') ? EP_KILL : EP_IGNORE; break; + case EP_K: state = (c == 'I') ? EP_KI : EP_IGNORE; break; + case EP_KI: state = (c == 'L') ? EP_KIL : EP_IGNORE; break; + case EP_KIL: state = (c == 'L') ? EP_KILL : EP_IGNORE; break; + #endif + case EP_M: switch (c) { case ' ': break; @@ -114,48 +160,34 @@ class EmergencyParser { } break; - case EP_M10: - state = (c == '8') ? EP_M108 : EP_IGNORE; - break; - - case EP_M11: - state = (c == '2') ? EP_M112 : EP_IGNORE; - break; - - case EP_M4: - state = (c == '1') ? EP_M41 : EP_IGNORE; - break; - - case EP_M41: - state = (c == '0') ? EP_M410 : EP_IGNORE; - break; + case EP_M10: state = (c == '8') ? EP_M108 : EP_IGNORE; break; + case EP_M11: state = (c == '2') ? EP_M112 : EP_IGNORE; break; + case EP_M4: state = (c == '1') ? EP_M41 : EP_IGNORE; break; + case EP_M41: state = (c == '0') ? EP_M410 : EP_IGNORE; break; #if ENABLED(HOST_PROMPT_SUPPORT) - case EP_M8: - state = (c == '7') ? EP_M87 : EP_IGNORE; - break; - case EP_M87: - state = (c == '6') ? EP_M876 : EP_IGNORE; - break; + case EP_M8: state = (c == '7') ? EP_M87 : EP_IGNORE; break; + case EP_M87: state = (c == '6') ? EP_M876 : EP_IGNORE; break; - case EP_M876: - switch (c) { - case ' ': break; - case 'S': state = EP_M876S; break; - default: state = EP_IGNORE; break; - } - break; + case EP_M876: + switch (c) { + case ' ': break; + case 'S': state = EP_M876S; break; + default: state = EP_IGNORE; break; + } + break; + + case EP_M876S: + switch (c) { + case ' ': break; + case '0' ... '9': + state = EP_M876SN; + M876_reason = uint8_t(c - '0'); + break; + } + break; - case EP_M876S: - switch (c) { - case ' ': break; - case '0' ... '9': - state = EP_M876SN; - M876_reason = (uint8_t)(c - '0'); - break; - } - break; #endif case EP_IGNORE: @@ -169,7 +201,15 @@ class EmergencyParser { case EP_M112: killed_by_M112 = true; break; case EP_M410: quickstop_by_M410 = true; break; #if ENABLED(HOST_PROMPT_SUPPORT) - case EP_M876SN: host_response_handler(M876_reason); break; + case EP_M876SN: hostui.handle_response(M876_reason); break; + #endif + #if ENABLED(REALTIME_REPORTING_COMMANDS) + case EP_GRBL_STATUS: report_current_position_moving(); break; + case EP_GRBL_PAUSE: quickpause_stepper(); break; + case EP_GRBL_RESUME: quickresume_stepper(); break; + #endif + #if ENABLED(SOFT_RESET_VIA_SERIAL) + case EP_KILL: HAL_reboot(); break; #endif default: break; } diff --git a/Marlin/src/feature/easythreed_ui.cpp b/Marlin/src/feature/easythreed_ui.cpp new file mode 100644 index 000000000000..b15daffc09be --- /dev/null +++ b/Marlin/src/feature/easythreed_ui.cpp @@ -0,0 +1,236 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2021 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + * + */ + +#include "../inc/MarlinConfigPre.h" + +#if ENABLED(EASYTHREED_UI) + +#include "easythreed_ui.h" +#include "pause.h" +#include "../module/temperature.h" +#include "../module/printcounter.h" +#include "../sd/cardreader.h" +#include "../gcode/queue.h" +#include "../module/motion.h" +#include "../module/planner.h" +#include "../MarlinCore.h" + +EasythreedUI easythreed_ui; + +#define BTN_DEBOUNCE_MS 20 + +void EasythreedUI::init() { + SET_INPUT_PULLUP(BTN_HOME); SET_OUTPUT(BTN_HOME_GND); + SET_INPUT_PULLUP(BTN_FEED); SET_OUTPUT(BTN_FEED_GND); + SET_INPUT_PULLUP(BTN_RETRACT); SET_OUTPUT(BTN_RETRACT_GND); + SET_INPUT_PULLUP(BTN_PRINT); + SET_OUTPUT(EASYTHREED_LED_PIN); +} + +void EasythreedUI::run() { + blinkLED(); + loadButton(); + printButton(); +} + +enum LEDInterval : uint16_t { + LED_OFF = 0, + LED_ON = 4000, + LED_BLINK_0 = 2500, + LED_BLINK_1 = 1500, + LED_BLINK_2 = 1000, + LED_BLINK_3 = 800, + LED_BLINK_4 = 500, + LED_BLINK_5 = 300, + LED_BLINK_6 = 150, + LED_BLINK_7 = 50 +}; + +uint16_t blink_interval_ms = LED_ON; // Status LED on Start button + +void EasythreedUI::blinkLED() { + static millis_t prev_blink_interval_ms = 0, blink_start_ms = 0; + + if (blink_interval_ms == LED_OFF) { WRITE(EASYTHREED_LED_PIN, HIGH); return; } // OFF + if (blink_interval_ms >= LED_ON) { WRITE(EASYTHREED_LED_PIN, LOW); return; } // ON + + const millis_t ms = millis(); + if (prev_blink_interval_ms != blink_interval_ms) { + prev_blink_interval_ms = blink_interval_ms; + blink_start_ms = ms; + } + if (PENDING(ms, blink_start_ms + blink_interval_ms)) + WRITE(EASYTHREED_LED_PIN, LOW); + else if (PENDING(ms, blink_start_ms + 2 * blink_interval_ms)) + WRITE(EASYTHREED_LED_PIN, HIGH); + else + blink_start_ms = ms; +} + +// +// Filament Load/Unload Button +// Load/Unload buttons are a 3 position switch with a common center ground. +// +void EasythreedUI::loadButton() { + if (printingIsActive()) return; + + enum FilamentStatus : uint8_t { FS_IDLE, FS_PRESS, FS_CHECK, FS_PROCEED }; + static uint8_t filament_status = FS_IDLE; + static millis_t filament_time = 0; + + switch (filament_status) { + + case FS_IDLE: + if (!READ(BTN_RETRACT) || !READ(BTN_FEED)) { // If feed/retract switch is toggled... + filament_status++; // ...proceed to next test. + filament_time = millis(); + } + break; + + case FS_PRESS: + if (ELAPSED(millis(), filament_time + BTN_DEBOUNCE_MS)) { // After a short debounce delay... + if (!READ(BTN_RETRACT) || !READ(BTN_FEED)) { // ...if switch still toggled... + thermalManager.setTargetHotend(EXTRUDE_MINTEMP + 10, 0); // Start heating up + blink_interval_ms = LED_BLINK_7; // Set the LED to blink fast + filament_status++; + } + else + filament_status = FS_IDLE; // Switch not toggled long enough + } + break; + + case FS_CHECK: + if (READ(BTN_RETRACT) && READ(BTN_FEED)) { // Switch in center position (stop) + blink_interval_ms = LED_ON; // LED on steady + filament_status = FS_IDLE; + thermalManager.disable_all_heaters(); + } + else if (thermalManager.hotEnoughToExtrude(0)) { // Is the hotend hot enough to move material? + filament_status++; // Proceed to feed / retract. + blink_interval_ms = LED_BLINK_5; // Blink ~3 times per second + } + break; + + case FS_PROCEED: { + // Feed or Retract just once. Hard abort all moves and return to idle on swicth release. + static bool flag = false; + if (READ(BTN_RETRACT) && READ(BTN_FEED)) { // Switch in center position (stop) + flag = false; // Restore flag to false + filament_status = FS_IDLE; // Go back to idle state + quickstop_stepper(); // Hard-stop all the steppers ... now! + thermalManager.disable_all_heaters(); // And disable all the heaters + blink_interval_ms = LED_ON; + } + else if (!flag) { + flag = true; + queue.inject(!READ(BTN_RETRACT) ? F("G91\nG0 E10 F180\nG0 E-120 F180\nM104 S0") : F("G91\nG0 E100 F120\nM104 S0")); + } + } break; + } + +} + +#if HAS_STEPPER_RESET + void disableStepperDrivers(); +#endif + +// +// Print Start/Pause/Resume Button +// +void EasythreedUI::printButton() { + enum KeyStatus : uint8_t { KS_IDLE, KS_PRESS, KS_PROCEED }; + static uint8_t key_status = KS_IDLE; + static millis_t key_time = 0; + + enum PrintFlag : uint8_t { PF_START, PF_PAUSE, PF_RESUME }; + static PrintFlag print_key_flag = PF_START; + + const millis_t ms = millis(); + + switch (key_status) { + case KS_IDLE: + if (!READ(BTN_PRINT)) { // Print/Pause/Resume button pressed? + key_time = ms; // Save start time + key_status++; // Go to debounce test + } + break; + + case KS_PRESS: + if (ELAPSED(ms, key_time + BTN_DEBOUNCE_MS)) // Wait for debounce interval to expire + key_status = READ(BTN_PRINT) ? KS_IDLE : KS_PROCEED; // Proceed if still pressed + break; + + case KS_PROCEED: + if (!READ(BTN_PRINT)) break; // Wait for the button to be released + key_status = KS_IDLE; // Ready for the next press + if (PENDING(ms, key_time + 1200 - BTN_DEBOUNCE_MS)) { // Register a press < 1.2 seconds + switch (print_key_flag) { + case PF_START: { // The "Print" button starts an SD card print + if (printingIsActive()) break; // Already printing? (find another line that checks for 'is planner doing anything else right now?') + blink_interval_ms = LED_BLINK_2; // Blink the indicator LED at 1 second intervals + print_key_flag = PF_PAUSE; // The "Print" button now pauses the print + card.mount(); // Force SD card to mount - now! + if (!card.isMounted) { // Failed to mount? + blink_interval_ms = LED_OFF; // Turn off LED + print_key_flag = PF_START; + return; // Bail out + } + card.ls(); // List all files to serial output + const uint16_t filecnt = card.countFilesInWorkDir(); // Count printable files in cwd + if (filecnt == 0) return; // None are printable? + card.selectFileByIndex(filecnt); // Select the last file according to current sort options + card.openAndPrintFile(card.filename); // Start printing it + break; + } + case PF_PAUSE: { // Pause printing (not currently firing) + if (!printingIsActive()) break; + blink_interval_ms = LED_ON; // Set indicator to steady ON + queue.inject(F("M25")); // Queue Pause + print_key_flag = PF_RESUME; // The "Print" button now resumes the print + break; + } + case PF_RESUME: { // Resume printing + if (printingIsActive()) break; + blink_interval_ms = LED_BLINK_2; // Blink the indicator LED at 1 second intervals + queue.inject(F("M24")); // Queue resume + print_key_flag = PF_PAUSE; // The "Print" button now pauses the print + break; + } + } + } + else { // Register a longer press + if (print_key_flag == PF_START && !printingIsActive()) { // While not printing, this moves Z up 10mm + blink_interval_ms = LED_ON; + queue.inject(F("G91\nG0 Z10 F600\nG90")); // Raise Z soon after returning to main loop + } + else { // While printing, cancel print + card.abortFilePrintSoon(); // There is a delay while the current steps play out + blink_interval_ms = LED_OFF; // Turn off LED + } + planner.synchronize(); // Wait for commands already in the planner to finish + TERN_(HAS_STEPPER_RESET, disableStepperDrivers()); // Disable all steppers - now! + print_key_flag = PF_START; // The "Print" button now starts a new print + } + break; + } +} +#endif // EASYTHREED_UI diff --git a/Marlin/src/feature/easythreed_ui.h b/Marlin/src/feature/easythreed_ui.h new file mode 100644 index 000000000000..af9ad2d090b1 --- /dev/null +++ b/Marlin/src/feature/easythreed_ui.h @@ -0,0 +1,35 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2021 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + * + */ +#pragma once + +class EasythreedUI { + public: + static void init(); + static void run(); + + private: + static void blinkLED(); + static void loadButton(); + static void printButton(); +}; + +extern EasythreedUI easythreed_ui; diff --git a/Marlin/src/feature/encoder_i2c.cpp b/Marlin/src/feature/encoder_i2c.cpp index cf5ebfd012ca..2ccd686a992d 100644 --- a/Marlin/src/feature/encoder_i2c.cpp +++ b/Marlin/src/feature/encoder_i2c.cpp @@ -47,9 +47,9 @@ void I2CPositionEncoder::init(const uint8_t address, const AxisEnum axis) { encoderAxis = axis; i2cAddress = address; - initialized++; + initialized = true; - SERIAL_ECHOLNPAIR("Setting up encoder on ", AS_CHAR(axis_codes[encoderAxis]), " axis, addr = ", address); + SERIAL_ECHOLNPGM("Setting up encoder on ", AS_CHAR(axis_codes[encoderAxis]), " axis, addr = ", address); position = get_position(); } @@ -67,7 +67,7 @@ void I2CPositionEncoder::update() { /* if (trusted) { //commented out as part of the note below trusted = false; - SERIAL_ECHOLNPAIR("Fault detected on ", AS_CHAR(axis_codes[encoderAxis]), " axis encoder. Disengaging error correction until module is trusted again."); + SERIAL_ECHOLNPGM("Fault detected on ", AS_CHAR(axis_codes[encoderAxis]), " axis encoder. Disengaging error correction until module is trusted again."); } */ return; @@ -92,9 +92,9 @@ void I2CPositionEncoder::update() { if (millis() - lastErrorTime > I2CPE_TIME_TRUSTED) { trusted = true; - SERIAL_ECHOLNPAIR("Untrusted encoder module on ", AS_CHAR(axis_codes[encoderAxis]), " axis has been fault-free for set duration, reinstating error correction."); + SERIAL_ECHOLNPGM("Untrusted encoder module on ", AS_CHAR(axis_codes[encoderAxis]), " axis has been fault-free for set duration, reinstating error correction."); - //the encoder likely lost its place when the error occured, so we'll reset and use the printer's + //the encoder likely lost its place when the error occurred, so we'll reset and use the printer's //idea of where it the axis is to re-initialize const float pos = planner.get_axis_position_mm(encoderAxis); int32_t positionInTicks = pos * get_ticks_unit(); @@ -103,10 +103,10 @@ void I2CPositionEncoder::update() { zeroOffset -= (positionInTicks - get_position()); #ifdef I2CPE_DEBUG - SERIAL_ECHOLNPAIR("Current position is ", pos); - SERIAL_ECHOLNPAIR("Position in encoder ticks is ", positionInTicks); - SERIAL_ECHOLNPAIR("New zero-offset of ", zeroOffset); - SERIAL_ECHOPAIR("New position reads as ", get_position()); + SERIAL_ECHOLNPGM("Current position is ", pos); + SERIAL_ECHOLNPGM("Position in encoder ticks is ", positionInTicks); + SERIAL_ECHOLNPGM("New zero-offset of ", zeroOffset); + SERIAL_ECHOPGM("New position reads as ", get_position()); SERIAL_CHAR('('); SERIAL_DECIMAL(mm_from_count(get_position())); SERIAL_ECHOLNPGM(")"); @@ -149,12 +149,12 @@ void I2CPositionEncoder::update() { const int32_t error = get_axis_error_steps(false); #endif - //SERIAL_ECHOLNPAIR("Axis error steps: ", error); + //SERIAL_ECHOLNPGM("Axis error steps: ", error); #ifdef I2CPE_ERR_THRESH_ABORT if (ABS(error) > I2CPE_ERR_THRESH_ABORT * planner.settings.axis_steps_per_mm[encoderAxis]) { - //kill(PSTR("Significant Error")); - SERIAL_ECHOLNPAIR("Axis error over threshold, aborting!", error); + //kill(F("Significant Error")); + SERIAL_ECHOLNPGM("Axis error over threshold, aborting!", error); safe_delay(5000); } #endif @@ -173,7 +173,7 @@ void I2CPositionEncoder::update() { LOOP_L_N(i, I2CPE_ERR_PRST_ARRAY_SIZE) sumP += errPrst[i]; const int32_t errorP = int32_t(sumP * RECIPROCAL(I2CPE_ERR_PRST_ARRAY_SIZE)); SERIAL_CHAR(axis_codes[encoderAxis]); - SERIAL_ECHOLNPAIR(" : CORRECT ERR ", errorP * planner.steps_to_mm[encoderAxis], "mm"); + SERIAL_ECHOLNPGM(" : CORRECT ERR ", errorP * planner.mm_per_step[encoderAxis], "mm"); babystep.add_steps(encoderAxis, -LROUND(errorP)); errPrstIdx = 0; } @@ -193,7 +193,7 @@ void I2CPositionEncoder::update() { const millis_t ms = millis(); if (ELAPSED(ms, nextErrorCountTime)) { SERIAL_CHAR(axis_codes[encoderAxis]); - SERIAL_ECHOLNPAIR(" : LARGE ERR ", error, "; diffSum=", diffSum); + SERIAL_ECHOLNPGM(" : LARGE ERR ", error, "; diffSum=", diffSum); errorCount++; nextErrorCountTime = ms + I2CPE_ERR_CNT_DEBOUNCE_MS; } @@ -209,12 +209,11 @@ void I2CPositionEncoder::set_homed() { delay(10); zeroOffset = get_raw_count(); - homed++; - trusted++; + homed = trusted = true; #ifdef I2CPE_DEBUG SERIAL_CHAR(axis_codes[encoderAxis]); - SERIAL_ECHOLNPAIR(" axis encoder homed, offset of ", zeroOffset, " ticks."); + SERIAL_ECHOLNPGM(" axis encoder homed, offset of ", zeroOffset, " ticks."); #endif } } @@ -233,7 +232,7 @@ bool I2CPositionEncoder::passes_test(const bool report) { if (report) { if (H != I2CPE_MAG_SIG_GOOD) SERIAL_ECHOPGM("Warning. "); SERIAL_CHAR(axis_codes[encoderAxis]); - serial_ternary(H == I2CPE_MAG_SIG_BAD, PSTR(" axis "), PSTR("magnetic strip "), PSTR("encoder ")); + serial_ternary(H == I2CPE_MAG_SIG_BAD, F(" axis "), F("magnetic strip "), F("encoder ")); switch (H) { case I2CPE_MAG_SIG_GOOD: case I2CPE_MAG_SIG_MID: @@ -254,7 +253,7 @@ float I2CPositionEncoder::get_axis_error_mm(const bool report) { if (report) { SERIAL_CHAR(axis_codes[encoderAxis]); - SERIAL_ECHOLNPAIR(" axis target=", target, "mm; actual=", actual, "mm; err=", error, "mm"); + SERIAL_ECHOLNPGM(" axis target=", target, "mm; actual=", actual, "mm; err=", error, "mm"); } return error; @@ -289,7 +288,7 @@ int32_t I2CPositionEncoder::get_axis_error_steps(const bool report) { if (report) { SERIAL_CHAR(axis_codes[encoderAxis]); - SERIAL_ECHOLNPAIR(" axis target=", target, "; actual=", encoderCountInStepperTicksScaled, "; err=", error); + SERIAL_ECHOLNPGM(" axis target=", target, "; actual=", encoderCountInStepperTicksScaled, "; err=", error); } if (suppressOutput) { @@ -328,7 +327,7 @@ int32_t I2CPositionEncoder::get_raw_count() { } bool I2CPositionEncoder::test_axis() { - //only works on XYZ cartesian machines for the time being + // Only works on XYZ Cartesian machines for the time being if (!(encoderAxis == X_AXIS || encoderAxis == Y_AXIS || encoderAxis == Z_AXIS)) return false; const float startPosition = soft_endstop.min[encoderAxis] + 10, @@ -338,7 +337,7 @@ bool I2CPositionEncoder::test_axis() { ec = false; xyze_pos_t startCoord, endCoord; - LOOP_XYZ(a) { + LOOP_LINEAR_AXES(a) { startCoord[a] = planner.get_axis_position_mm((AxisEnum)a); endCoord[a] = planner.get_axis_position_mm((AxisEnum)a); } @@ -346,9 +345,12 @@ bool I2CPositionEncoder::test_axis() { endCoord[encoderAxis] = endPosition; planner.synchronize(); - startCoord.e = planner.get_axis_position_mm(E_AXIS); - planner.buffer_line(startCoord, fr_mm_s, 0); - planner.synchronize(); + + #if HAS_EXTRUDERS + startCoord.e = planner.get_axis_position_mm(E_AXIS); + planner.buffer_line(startCoord, fr_mm_s, 0); + planner.synchronize(); + #endif // if the module isn't currently trusted, wait until it is (or until it should be if things are working) if (!trusted) { @@ -358,7 +360,7 @@ bool I2CPositionEncoder::test_axis() { } if (trusted) { // if trusted, commence test - endCoord.e = planner.get_axis_position_mm(E_AXIS); + TERN_(HAS_EXTRUDERS, endCoord.e = planner.get_axis_position_mm(E_AXIS)); planner.buffer_line(endCoord, fr_mm_s, 0); planner.synchronize(); } @@ -393,7 +395,7 @@ void I2CPositionEncoder::calibrate_steps_mm(const uint8_t iter) { travelDistance = endDistance - startDistance; xyze_pos_t startCoord, endCoord; - LOOP_XYZ(a) { + LOOP_LINEAR_AXES(a) { startCoord[a] = planner.get_axis_position_mm((AxisEnum)a); endCoord[a] = planner.get_axis_position_mm((AxisEnum)a); } @@ -403,7 +405,7 @@ void I2CPositionEncoder::calibrate_steps_mm(const uint8_t iter) { planner.synchronize(); LOOP_L_N(i, iter) { - startCoord.e = planner.get_axis_position_mm(E_AXIS); + TERN_(HAS_EXTRUDERS, startCoord.e = planner.get_axis_position_mm(E_AXIS)); planner.buffer_line(startCoord, fr_mm_s, 0); planner.synchronize(); @@ -412,7 +414,7 @@ void I2CPositionEncoder::calibrate_steps_mm(const uint8_t iter) { //do_blocking_move_to(endCoord); - endCoord.e = planner.get_axis_position_mm(E_AXIS); + TERN_(HAS_EXTRUDERS, endCoord.e = planner.get_axis_position_mm(E_AXIS)); planner.buffer_line(endCoord, fr_mm_s, 0); planner.synchronize(); @@ -422,15 +424,15 @@ void I2CPositionEncoder::calibrate_steps_mm(const uint8_t iter) { travelledDistance = mm_from_count(ABS(stopCount - startCount)); - SERIAL_ECHOLNPAIR("Attempted travel: ", travelDistance, "mm"); - SERIAL_ECHOLNPAIR(" Actual travel: ", travelledDistance, "mm"); + SERIAL_ECHOLNPGM("Attempted travel: ", travelDistance, "mm"); + SERIAL_ECHOLNPGM(" Actual travel: ", travelledDistance, "mm"); //Calculate new axis steps per unit old_steps_mm = planner.settings.axis_steps_per_mm[encoderAxis]; new_steps_mm = (old_steps_mm * travelDistance) / travelledDistance; - SERIAL_ECHOLNPAIR("Old steps/mm: ", old_steps_mm); - SERIAL_ECHOLNPAIR("New steps/mm: ", new_steps_mm); + SERIAL_ECHOLNPGM("Old steps/mm: ", old_steps_mm); + SERIAL_ECHOLNPGM("New steps/mm: ", new_steps_mm); //Save new value planner.settings.axis_steps_per_mm[encoderAxis] = new_steps_mm; @@ -447,7 +449,7 @@ void I2CPositionEncoder::calibrate_steps_mm(const uint8_t iter) { if (iter > 1) { total /= (float)iter; - SERIAL_ECHOLNPAIR("Average steps/mm: ", total); + SERIAL_ECHOLNPGM("Average steps/mm: ", total); } ec = oldec; @@ -498,9 +500,7 @@ void I2CPositionEncodersMgr::init() { encoders[i].set_active(encoders[i].passes_test(true)); - #if I2CPE_ENC_1_AXIS == E_AXIS - encoders[i].set_homed(); - #endif + TERN_(HAS_EXTRUDERS, if (I2CPE_ENC_1_AXIS == E_AXIS) encoders[i].set_homed()); #endif #if I2CPE_ENCODER_CNT > 1 @@ -529,9 +529,7 @@ void I2CPositionEncodersMgr::init() { encoders[i].set_active(encoders[i].passes_test(true)); - #if I2CPE_ENC_2_AXIS == E_AXIS - encoders[i].set_homed(); - #endif + TERN_(HAS_EXTRUDERS, if (I2CPE_ENC_2_AXIS == E_AXIS) encoders[i].set_homed()); #endif #if I2CPE_ENCODER_CNT > 2 @@ -558,11 +556,9 @@ void I2CPositionEncodersMgr::init() { encoders[i].set_ec_threshold(I2CPE_ENC_3_EC_THRESH); #endif - encoders[i].set_active(encoders[i].passes_test(true)); + encoders[i].set_active(encoders[i].passes_test(true)); - #if I2CPE_ENC_3_AXIS == E_AXIS - encoders[i].set_homed(); - #endif + TERN_(HAS_EXTRUDERS, if (I2CPE_ENC_3_AXIS == E_AXIS) encoders[i].set_homed()); #endif #if I2CPE_ENCODER_CNT > 3 @@ -591,9 +587,7 @@ void I2CPositionEncodersMgr::init() { encoders[i].set_active(encoders[i].passes_test(true)); - #if I2CPE_ENC_4_AXIS == E_AXIS - encoders[i].set_homed(); - #endif + TERN_(HAS_EXTRUDERS, if (I2CPE_ENC_4_AXIS == E_AXIS) encoders[i].set_homed()); #endif #if I2CPE_ENCODER_CNT > 4 @@ -622,9 +616,7 @@ void I2CPositionEncodersMgr::init() { encoders[i].set_active(encoders[i].passes_test(true)); - #if I2CPE_ENC_5_AXIS == E_AXIS - encoders[i].set_homed(); - #endif + TERN_(HAS_EXTRUDERS, if (I2CPE_ENC_5_AXIS == E_AXIS) encoders[i].set_homed()); #endif #if I2CPE_ENCODER_CNT > 5 @@ -653,9 +645,7 @@ void I2CPositionEncodersMgr::init() { encoders[i].set_active(encoders[i].passes_test(true)); - #if I2CPE_ENC_6_AXIS == E_AXIS - encoders[i].set_homed(); - #endif + TERN_(HAS_EXTRUDERS, if (I2CPE_ENC_6_AXIS == E_AXIS) encoders[i].set_homed()); #endif } @@ -685,18 +675,18 @@ void I2CPositionEncodersMgr::change_module_address(const uint8_t oldaddr, const // First check 'new' address is not in use Wire.beginTransmission(I2C_ADDRESS(newaddr)); if (!Wire.endTransmission()) { - SERIAL_ECHOLNPAIR("?There is already a device with that address on the I2C bus! (", newaddr, ")"); + SERIAL_ECHOLNPGM("?There is already a device with that address on the I2C bus! (", newaddr, ")"); return; } // Now check that we can find the module on the oldaddr address Wire.beginTransmission(I2C_ADDRESS(oldaddr)); if (Wire.endTransmission()) { - SERIAL_ECHOLNPAIR("?No module detected at this address! (", oldaddr, ")"); + SERIAL_ECHOLNPGM("?No module detected at this address! (", oldaddr, ")"); return; } - SERIAL_ECHOLNPAIR("Module found at ", oldaddr, ", changing address to ", newaddr); + SERIAL_ECHOLNPGM("Module found at ", oldaddr, ", changing address to ", newaddr); // Change the modules address Wire.beginTransmission(I2C_ADDRESS(oldaddr)); @@ -732,11 +722,11 @@ void I2CPositionEncodersMgr::report_module_firmware(const uint8_t address) { // First check there is a module Wire.beginTransmission(I2C_ADDRESS(address)); if (Wire.endTransmission()) { - SERIAL_ECHOLNPAIR("?No module detected at this address! (", address, ")"); + SERIAL_ECHOLNPGM("?No module detected at this address! (", address, ")"); return; } - SERIAL_ECHOLNPAIR("Requesting version info from module at address ", address, ":"); + SERIAL_ECHOLNPGM("Requesting version info from module at address ", address, ":"); Wire.beginTransmission(I2C_ADDRESS(address)); Wire.write(I2CPE_SET_REPORT_MODE); @@ -766,7 +756,7 @@ int8_t I2CPositionEncodersMgr::parse() { if (!parser.has_value()) { SERIAL_ECHOLNPGM("?A seen, but no address specified! [30-200]"); return I2CPE_PARSE_ERR; - }; + } I2CPE_addr = parser.value_byte(); if (!WITHIN(I2CPE_addr, 30, 200)) { // reserve the first 30 and last 55 @@ -783,13 +773,13 @@ int8_t I2CPositionEncodersMgr::parse() { else if (parser.seenval('I')) { if (!parser.has_value()) { - SERIAL_ECHOLNPAIR("?I seen, but no index specified! [0-", I2CPE_ENCODER_CNT - 1, "]"); + SERIAL_ECHOLNPGM("?I seen, but no index specified! [0-", I2CPE_ENCODER_CNT - 1, "]"); return I2CPE_PARSE_ERR; - }; + } I2CPE_idx = parser.value_byte(); if (I2CPE_idx >= I2CPE_ENCODER_CNT) { - SERIAL_ECHOLNPAIR("?Index out of range. [0-", I2CPE_ENCODER_CNT - 1, "]"); + SERIAL_ECHOLNPGM("?Index out of range. [0-", I2CPE_ENCODER_CNT - 1, "]"); return I2CPE_PARSE_ERR; } @@ -801,7 +791,7 @@ int8_t I2CPositionEncodersMgr::parse() { I2CPE_anyaxis = parser.seen_axis(); return I2CPE_PARSE_OK; -}; +} /** * M860: Report the position(s) of position encoder module(s). @@ -820,11 +810,11 @@ int8_t I2CPositionEncodersMgr::parse() { void I2CPositionEncodersMgr::M860() { if (parse()) return; - const bool hasU = parser.seen('U'), hasO = parser.seen('O'); + const bool hasU = parser.seen_test('U'), hasO = parser.seen_test('O'); if (I2CPE_idx == 0xFF) { - LOOP_XYZE(i) { - if (!I2CPE_anyaxis || parser.seen(axis_codes[i])) { + LOOP_LOGICAL_AXES(i) { + if (!I2CPE_anyaxis || parser.seen_test(axis_codes[i])) { const uint8_t idx = idx_from_axis(AxisEnum(i)); if ((int8_t)idx >= 0) report_position(idx, hasU, hasO); } @@ -850,7 +840,7 @@ void I2CPositionEncodersMgr::M861() { if (parse()) return; if (I2CPE_idx == 0xFF) { - LOOP_XYZE(i) { + LOOP_LOGICAL_AXES(i) { if (!I2CPE_anyaxis || parser.seen(axis_codes[i])) { const uint8_t idx = idx_from_axis(AxisEnum(i)); if ((int8_t)idx >= 0) report_status(idx); @@ -878,7 +868,7 @@ void I2CPositionEncodersMgr::M862() { if (parse()) return; if (I2CPE_idx == 0xFF) { - LOOP_XYZE(i) { + LOOP_LOGICAL_AXES(i) { if (!I2CPE_anyaxis || parser.seen(axis_codes[i])) { const uint8_t idx = idx_from_axis(AxisEnum(i)); if ((int8_t)idx >= 0) test_axis(idx); @@ -909,7 +899,7 @@ void I2CPositionEncodersMgr::M863() { const uint8_t iterations = constrain(parser.byteval('P', 1), 1, 10); if (I2CPE_idx == 0xFF) { - LOOP_XYZE(i) { + LOOP_LOGICAL_AXES(i) { if (!I2CPE_anyaxis || parser.seen(axis_codes[i])) { const uint8_t idx = idx_from_axis(AxisEnum(i)); if ((int8_t)idx >= 0) calibrate_steps_mm(idx, iterations); @@ -944,7 +934,7 @@ void I2CPositionEncodersMgr::M864() { if (!parser.has_value()) { SERIAL_ECHOLNPGM("?S seen, but no address specified! [30-200]"); return; - }; + } newAddress = parser.value_byte(); if (!WITHIN(newAddress, 30, 200)) { @@ -957,14 +947,14 @@ void I2CPositionEncodersMgr::M864() { return; } else { - if (parser.seen('X')) newAddress = I2CPE_PRESET_ADDR_X; - else if (parser.seen('Y')) newAddress = I2CPE_PRESET_ADDR_Y; - else if (parser.seen('Z')) newAddress = I2CPE_PRESET_ADDR_Z; - else if (parser.seen('E')) newAddress = I2CPE_PRESET_ADDR_E; + if (parser.seen_test('X')) newAddress = I2CPE_PRESET_ADDR_X; + else if (parser.seen_test('Y')) newAddress = I2CPE_PRESET_ADDR_Y; + else if (parser.seen_test('Z')) newAddress = I2CPE_PRESET_ADDR_Z; + else if (parser.seen_test('E')) newAddress = I2CPE_PRESET_ADDR_E; else return; } - SERIAL_ECHOLNPAIR("Changing module at address ", I2CPE_addr, " to address ", newAddress); + SERIAL_ECHOLNPGM("Changing module at address ", I2CPE_addr, " to address ", newAddress); change_module_address(I2CPE_addr, newAddress); } @@ -985,7 +975,7 @@ void I2CPositionEncodersMgr::M865() { if (parse()) return; if (!I2CPE_addr) { - LOOP_XYZE(i) { + LOOP_LOGICAL_AXES(i) { if (!I2CPE_anyaxis || parser.seen(axis_codes[i])) { const uint8_t idx = idx_from_axis(AxisEnum(i)); if ((int8_t)idx >= 0) report_module_firmware(encoders[idx].get_address()); @@ -1013,10 +1003,10 @@ void I2CPositionEncodersMgr::M865() { void I2CPositionEncodersMgr::M866() { if (parse()) return; - const bool hasR = parser.seen('R'); + const bool hasR = parser.seen_test('R'); if (I2CPE_idx == 0xFF) { - LOOP_XYZE(i) { + LOOP_LOGICAL_AXES(i) { if (!I2CPE_anyaxis || parser.seen(axis_codes[i])) { const uint8_t idx = idx_from_axis(AxisEnum(i)); if ((int8_t)idx >= 0) { @@ -1054,7 +1044,7 @@ void I2CPositionEncodersMgr::M867() { const int8_t onoff = parser.seenval('S') ? parser.value_int() : -1; if (I2CPE_idx == 0xFF) { - LOOP_XYZE(i) { + LOOP_LOGICAL_AXES(i) { if (!I2CPE_anyaxis || parser.seen(axis_codes[i])) { const uint8_t idx = idx_from_axis(AxisEnum(i)); if ((int8_t)idx >= 0) { @@ -1090,7 +1080,7 @@ void I2CPositionEncodersMgr::M868() { const float newThreshold = parser.seenval('T') ? parser.value_float() : -9999; if (I2CPE_idx == 0xFF) { - LOOP_XYZE(i) { + LOOP_LOGICAL_AXES(i) { if (!I2CPE_anyaxis || parser.seen(axis_codes[i])) { const uint8_t idx = idx_from_axis(AxisEnum(i)); if ((int8_t)idx >= 0) { @@ -1124,7 +1114,7 @@ void I2CPositionEncodersMgr::M869() { if (parse()) return; if (I2CPE_idx == 0xFF) { - LOOP_XYZE(i) { + LOOP_LOGICAL_AXES(i) { if (!I2CPE_anyaxis || parser.seen(axis_codes[i])) { const uint8_t idx = idx_from_axis(AxisEnum(i)); if ((int8_t)idx >= 0) report_error(idx); diff --git a/Marlin/src/feature/encoder_i2c.h b/Marlin/src/feature/encoder_i2c.h index e77113039197..50fb27a135ca 100644 --- a/Marlin/src/feature/encoder_i2c.h +++ b/Marlin/src/feature/encoder_i2c.h @@ -188,7 +188,7 @@ class I2CPositionEncoder { FORCE_INLINE void set_ec_method(const byte method) { ecMethod = method; } FORCE_INLINE float get_ec_threshold() { return ecThreshold; } - FORCE_INLINE void set_ec_threshold(const float newThreshold) { ecThreshold = newThreshold; } + FORCE_INLINE void set_ec_threshold(const_float_t newThreshold) { ecThreshold = newThreshold; } FORCE_INLINE int get_encoder_ticks_mm() { switch (type) { @@ -236,7 +236,7 @@ class I2CPositionEncodersMgr { static void report_status(const int8_t idx) { CHECK_IDX(); - SERIAL_ECHOLNPAIR("Encoder ", idx, ": "); + SERIAL_ECHOLNPGM("Encoder ", idx, ": "); encoders[idx].get_raw_count(); encoders[idx].passes_test(true); } @@ -261,32 +261,32 @@ class I2CPositionEncodersMgr { static void report_error_count(const int8_t idx, const AxisEnum axis) { CHECK_IDX(); - SERIAL_ECHOLNPAIR("Error count on ", AS_CHAR(axis_codes[axis]), " axis is ", encoders[idx].get_error_count()); + SERIAL_ECHOLNPGM("Error count on ", AS_CHAR(axis_codes[axis]), " axis is ", encoders[idx].get_error_count()); } static void reset_error_count(const int8_t idx, const AxisEnum axis) { CHECK_IDX(); encoders[idx].set_error_count(0); - SERIAL_ECHOLNPAIR("Error count on ", AS_CHAR(axis_codes[axis]), " axis has been reset."); + SERIAL_ECHOLNPGM("Error count on ", AS_CHAR(axis_codes[axis]), " axis has been reset."); } static void enable_ec(const int8_t idx, const bool enabled, const AxisEnum axis) { CHECK_IDX(); encoders[idx].set_ec_enabled(enabled); - SERIAL_ECHOPAIR("Error correction on ", AS_CHAR(axis_codes[axis])); + SERIAL_ECHOPGM("Error correction on ", AS_CHAR(axis_codes[axis])); SERIAL_ECHO_TERNARY(encoders[idx].get_ec_enabled(), " axis is ", "en", "dis", "abled.\n"); } static void set_ec_threshold(const int8_t idx, const float newThreshold, const AxisEnum axis) { CHECK_IDX(); encoders[idx].set_ec_threshold(newThreshold); - SERIAL_ECHOLNPAIR("Error correct threshold for ", AS_CHAR(axis_codes[axis]), " axis set to ", newThreshold, "mm."); + SERIAL_ECHOLNPGM("Error correct threshold for ", AS_CHAR(axis_codes[axis]), " axis set to ", newThreshold, "mm."); } static void get_ec_threshold(const int8_t idx, const AxisEnum axis) { CHECK_IDX(); const float threshold = encoders[idx].get_ec_threshold(); - SERIAL_ECHOLNPAIR("Error correct threshold for ", AS_CHAR(axis_codes[axis]), " axis is ", threshold, "mm."); + SERIAL_ECHOLNPGM("Error correct threshold for ", AS_CHAR(axis_codes[axis]), " axis is ", threshold, "mm."); } static int8_t idx_from_axis(const AxisEnum axis) { diff --git a/Marlin/src/feature/ethernet.cpp b/Marlin/src/feature/ethernet.cpp index ff3ba76b892f..c5bfa932cb5b 100644 --- a/Marlin/src/feature/ethernet.cpp +++ b/Marlin/src/feature/ethernet.cpp @@ -124,7 +124,7 @@ void MarlinEthernet::check() { if (!Ethernet.localIP()) break; SERIAL_ECHOPGM("Successfully started telnet server with IP "); - MYSERIAL0.println(Ethernet.localIP()); + MYSERIAL1.println(Ethernet.localIP()); linkState = LINKED; break; @@ -147,7 +147,7 @@ void MarlinEthernet::check() { " | Author: " STRING_CONFIG_H_AUTHOR ); #endif - telnetClient.println("Compiled: " __DATE__); + telnetClient.println(" Compiled: " __DATE__); SERIAL_ECHOLNPGM("Client connected"); have_telnet_client = true; diff --git a/Marlin/src/feature/fancheck.cpp b/Marlin/src/feature/fancheck.cpp new file mode 100644 index 000000000000..1b47fadecc31 --- /dev/null +++ b/Marlin/src/feature/fancheck.cpp @@ -0,0 +1,207 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2021 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + * + */ + +/** + * fancheck.cpp - fan tachometer check + */ + +#include "../inc/MarlinConfig.h" + +#if HAS_FANCHECK + +#include "fancheck.h" +#include "../module/temperature.h" + +#if HAS_AUTO_FAN && EXTRUDER_AUTO_FAN_SPEED != 255 && DISABLED(FOURWIRES_FANS) + bool FanCheck::measuring = false; +#endif +bool FanCheck::tacho_state[TACHO_COUNT]; +uint16_t FanCheck::edge_counter[TACHO_COUNT]; +uint8_t FanCheck::rps[TACHO_COUNT]; +FanCheck::TachoError FanCheck::error = FanCheck::TachoError::NONE; +bool FanCheck::enabled; + +void FanCheck::init() { + #define _TACHINIT(N) TERN(E##N##_FAN_TACHO_PULLUP, SET_INPUT_PULLUP, TERN(E##N##_FAN_TACHO_PULLDOWN, SET_INPUT_PULLDOWN, SET_INPUT))(E##N##_FAN_TACHO_PIN) + #if HAS_E0_FAN_TACHO + _TACHINIT(0); + #endif + #if HAS_E1_FAN_TACHO + _TACHINIT(1); + #endif + #if HAS_E2_FAN_TACHO + _TACHINIT(2); + #endif + #if HAS_E3_FAN_TACHO + _TACHINIT(3); + #endif + #if HAS_E4_FAN_TACHO + _TACHINIT(4); + #endif + #if HAS_E5_FAN_TACHO + _TACHINIT(5); + #endif + #if HAS_E6_FAN_TACHO + _TACHINIT(6); + #endif + #if HAS_E7_FAN_TACHO + _TACHINIT(7); + #endif +} + +void FanCheck::update_tachometers() { + bool status; + + #define _TACHO_CASE(N) case N: status = READ(E##N##_FAN_TACHO_PIN); break; + LOOP_L_N(f, TACHO_COUNT) { + switch (f) { + #if HAS_E0_FAN_TACHO + _TACHO_CASE(0) + #endif + #if HAS_E1_FAN_TACHO + _TACHO_CASE(1) + #endif + #if HAS_E2_FAN_TACHO + _TACHO_CASE(2) + #endif + #if HAS_E3_FAN_TACHO + _TACHO_CASE(3) + #endif + #if HAS_E4_FAN_TACHO + _TACHO_CASE(4) + #endif + #if HAS_E5_FAN_TACHO + _TACHO_CASE(5) + #endif + #if HAS_E6_FAN_TACHO + _TACHO_CASE(6) + #endif + #if HAS_E7_FAN_TACHO + _TACHO_CASE(7) + #endif + default: continue; + } + + if (status != tacho_state[f]) { + if (measuring) ++edge_counter[f]; + tacho_state[f] = status; + } + } +} + +void FanCheck::compute_speed(uint16_t elapsedTime) { + static uint8_t errors_count[TACHO_COUNT]; + static uint8_t fan_reported_errors_msk = 0; + + uint8_t fan_error_msk = 0; + LOOP_L_N(f, TACHO_COUNT) { + switch (f) { + TERN_(HAS_E0_FAN_TACHO, case 0:) + TERN_(HAS_E1_FAN_TACHO, case 1:) + TERN_(HAS_E2_FAN_TACHO, case 2:) + TERN_(HAS_E3_FAN_TACHO, case 3:) + TERN_(HAS_E4_FAN_TACHO, case 4:) + TERN_(HAS_E5_FAN_TACHO, case 5:) + TERN_(HAS_E6_FAN_TACHO, case 6:) + TERN_(HAS_E7_FAN_TACHO, case 7:) + // Compute fan speed + rps[f] = edge_counter[f] * float(250) / elapsedTime; + edge_counter[f] = 0; + + // Check fan speed + constexpr int8_t max_extruder_fan_errors = TERN(HAS_PWMFANCHECK, 10000, 5000) / Temperature::fan_update_interval_ms; + + if (rps[f] >= 20 || TERN0(HAS_AUTO_FAN, thermalManager.autofan_speed[f] == 0)) + errors_count[f] = 0; + else if (errors_count[f] < max_extruder_fan_errors) + ++errors_count[f]; + else if (enabled) + SBI(fan_error_msk, f); + break; + } + } + + // Drop the error when all fans are ok + if (!fan_error_msk && error == TachoError::REPORTED) error = TachoError::FIXED; + + if (error == TachoError::FIXED && !printJobOngoing() && !printingIsPaused()) { + error = TachoError::NONE; // if the issue has been fixed while the printer is idle, reenable immediately + ui.reset_alert_level(); + } + + if (fan_error_msk & ~fan_reported_errors_msk) { + // Handle new faults only + LOOP_L_N(f, TACHO_COUNT) if (TEST(fan_error_msk, f)) report_speed_error(f); + } + fan_reported_errors_msk = fan_error_msk; +} + +void FanCheck::report_speed_error(uint8_t fan) { + if (printJobOngoing()) { + if (error == TachoError::NONE) { + if (thermalManager.degTargetHotend(fan) != 0) { + kill(GET_TEXT_F(MSG_FAN_SPEED_FAULT)); + error = TachoError::REPORTED; + } + else + error = TachoError::DETECTED; // Plans error for next processed command + } + } + else if (!printingIsPaused()) { + thermalManager.setTargetHotend(0, fan); // Always disable heating + if (error == TachoError::NONE) error = TachoError::REPORTED; + } + + SERIAL_ERROR_MSG(STR_ERR_FANSPEED, fan); + LCD_ALERTMESSAGE(MSG_FAN_SPEED_FAULT); +} + +void FanCheck::print_fan_states() { + LOOP_L_N(s, 2) { + LOOP_L_N(f, TACHO_COUNT) { + switch (f) { + TERN_(HAS_E0_FAN_TACHO, case 0:) + TERN_(HAS_E1_FAN_TACHO, case 1:) + TERN_(HAS_E2_FAN_TACHO, case 2:) + TERN_(HAS_E3_FAN_TACHO, case 3:) + TERN_(HAS_E4_FAN_TACHO, case 4:) + TERN_(HAS_E5_FAN_TACHO, case 5:) + TERN_(HAS_E6_FAN_TACHO, case 6:) + TERN_(HAS_E7_FAN_TACHO, case 7:) + SERIAL_ECHOPGM("E", f); + if (s == 0) + SERIAL_ECHOPGM(":", 60 * rps[f], " RPM "); + else + SERIAL_ECHOPGM("@:", TERN(HAS_AUTO_FAN, thermalManager.autofan_speed[f], 255), " "); + break; + } + } + } + SERIAL_EOL(); +} + +#if ENABLED(AUTO_REPORT_FANS) + AutoReporter FanCheck::auto_reporter; + void FanCheck::AutoReportFan::report() { print_fan_states(); } +#endif + +#endif // HAS_FANCHECK diff --git a/Marlin/src/feature/fancheck.h b/Marlin/src/feature/fancheck.h new file mode 100644 index 000000000000..c8665a0e96e8 --- /dev/null +++ b/Marlin/src/feature/fancheck.h @@ -0,0 +1,89 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2021 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + * + */ +#pragma once + +#include "../inc/MarlinConfig.h" + +#if HAS_FANCHECK + +#include "../MarlinCore.h" +#include "../lcd/marlinui.h" + +#if ENABLED(AUTO_REPORT_FANS) + #include "../libs/autoreport.h" +#endif + +#if ENABLED(PARK_HEAD_ON_PAUSE) + #include "../gcode/queue.h" +#endif + +/** + * fancheck.h + */ +#define TACHO_COUNT TERN(HAS_E7_FAN_TACHO, 8, TERN(HAS_E6_FAN_TACHO, 7, TERN(HAS_E5_FAN_TACHO, 6, TERN(HAS_E4_FAN_TACHO, 5, TERN(HAS_E3_FAN_TACHO, 4, TERN(HAS_E2_FAN_TACHO, 3, TERN(HAS_E1_FAN_TACHO, 2, 1))))))) + +class FanCheck { + private: + + enum class TachoError : uint8_t { NONE, DETECTED, REPORTED, FIXED }; + + #if HAS_PWMFANCHECK + static bool measuring; // For future use (3 wires PWM controlled fans) + #else + static constexpr bool measuring = true; + #endif + static bool tacho_state[TACHO_COUNT]; + static uint16_t edge_counter[TACHO_COUNT]; + static uint8_t rps[TACHO_COUNT]; + static TachoError error; + + static void report_speed_error(uint8_t fan); + + public: + + static bool enabled; + + static void init(); + static void update_tachometers(); + static void compute_speed(uint16_t elapsedTime); + static void print_fan_states(); + #if HAS_PWMFANCHECK + static void toggle_measuring() { measuring = !measuring; } + static bool is_measuring() { return measuring; } + #endif + + static void check_deferred_error() { + if (error == TachoError::DETECTED) { + error = TachoError::REPORTED; + TERN(PARK_HEAD_ON_PAUSE, queue.inject(F("M125")), kill(GET_TEXT_F(MSG_FAN_SPEED_FAULT))); + } + } + + #if ENABLED(AUTO_REPORT_FANS) + struct AutoReportFan { static void report(); }; + static AutoReporter auto_reporter; + #endif +}; + +extern FanCheck fan_check; + +#endif // HAS_FANCHECK diff --git a/Marlin/src/feature/fanmux.h b/Marlin/src/feature/fanmux.h index b1b0c67a55e0..efb92cf1989c 100644 --- a/Marlin/src/feature/fanmux.h +++ b/Marlin/src/feature/fanmux.h @@ -25,5 +25,5 @@ * feature/fanmux.h - Cooling Fan Multiplexer support functions */ -extern void fanmux_switch(const uint8_t e); -extern void fanmux_init(); +void fanmux_switch(const uint8_t e); +void fanmux_init(); diff --git a/Marlin/src/feature/filwidth.h b/Marlin/src/feature/filwidth.h index ef3859df719a..e234380e981a 100644 --- a/Marlin/src/feature/filwidth.h +++ b/Marlin/src/feature/filwidth.h @@ -41,9 +41,9 @@ class FilamentWidthSensor { FilamentWidthSensor() { init(); } static void init(); - static inline void enable(const bool ena) { enabled = ena; } + static void enable(const bool ena) { enabled = ena; } - static inline void set_delay_cm(const uint8_t cm) { + static void set_delay_cm(const uint8_t cm) { meas_delay_cm = _MIN(cm, MAX_MEASUREMENT_DELAY); } @@ -67,18 +67,18 @@ class FilamentWidthSensor { } // Convert raw measurement to mm - static inline float raw_to_mm(const uint16_t v) { return v * 5.0f * RECIPROCAL(float(MAX_RAW_THERMISTOR_VALUE)); } - static inline float raw_to_mm() { return raw_to_mm(raw); } + static float raw_to_mm(const uint16_t v) { return v * 5.0f * RECIPROCAL(float(MAX_RAW_THERMISTOR_VALUE)); } + static float raw_to_mm() { return raw_to_mm(raw); } // A scaled reading is ready // Divide to get to 0-16384 range since we used 1/128 IIR filter approach - static inline void reading_ready() { raw = accum >> 10; } + static void reading_ready() { raw = accum >> 10; } // Update mm from the raw measurement - static inline void update_measured_mm() { measured_mm = raw_to_mm(); } + static void update_measured_mm() { measured_mm = raw_to_mm(); } // Update ring buffer used to delay filament measurements - static inline void advance_e(const float &e_move) { + static void advance_e(const_float_t e_move) { // Increment counters with the E distance e_count += e_move; @@ -106,7 +106,7 @@ class FilamentWidthSensor { } // Dynamically set the volumetric multiplier based on the delayed width measurement. - static inline void update_volumetric() { + static void update_volumetric() { if (enabled) { int8_t read_index = index_r - meas_delay_cm; if (read_index < 0) read_index += MMD_CM; // Loop around buffer if needed diff --git a/Marlin/src/feature/fwretract.cpp b/Marlin/src/feature/fwretract.cpp index 2a71af11d67d..4077d8d1c209 100644 --- a/Marlin/src/feature/fwretract.cpp +++ b/Marlin/src/feature/fwretract.cpp @@ -36,6 +36,8 @@ FWRetract fwretract; // Single instance - this calls the constructor #include "../module/planner.h" #include "../module/stepper.h" +#include "../gcode/gcode.h" + #if ENABLED(RETRACT_SYNC_MIXING) #include "mixing.h" #endif @@ -73,7 +75,7 @@ void FWRetract::reset() { LOOP_L_N(i, EXTRUDERS) { retracted[i] = false; - TERN_(HAS_MULTI_EXTRUDER, retracted_swap[i] = false); + E_TERN_(retracted_swap[i] = false); current_retract[i] = 0.0; } } @@ -89,11 +91,7 @@ void FWRetract::reset() { * Note: Auto-retract will apply the set Z hop in addition to any Z hop * included in the G-code. Use M207 Z0 to to prevent double hop. */ -void FWRetract::retract(const bool retracting - #if HAS_MULTI_EXTRUDER - , bool swapping/*=false*/ - #endif -) { +void FWRetract::retract(const bool retracting E_OPTARG(bool swapping/*=false*/)) { // Prevent two retracts or recovers in a row if (retracted[active_extruder] == retracting) return; @@ -108,20 +106,20 @@ void FWRetract::retract(const bool retracting #endif /* // debugging - SERIAL_ECHOLNPAIR( - "retracting ", retracting, + SERIAL_ECHOLNPGM( + "retracting ", AS_DIGIT(retracting), " swapping ", swapping, " active extruder ", active_extruder ); LOOP_L_N(i, EXTRUDERS) { - SERIAL_ECHOLNPAIR("retracted[", i, "] ", retracted[i]); + SERIAL_ECHOLNPGM("retracted[", i, "] ", AS_DIGIT(retracted[i])); #if HAS_MULTI_EXTRUDER - SERIAL_ECHOLNPAIR("retracted_swap[", i, "] ", retracted_swap[i]); + SERIAL_ECHOLNPGM("retracted_swap[", i, "] ", AS_DIGIT(retracted_swap[i])); #endif } - SERIAL_ECHOLNPAIR("current_position.z ", current_position.z); - SERIAL_ECHOLNPAIR("current_position.e ", current_position.e); - SERIAL_ECHOLNPAIR("current_hop ", current_hop); + SERIAL_ECHOLNPGM("current_position.z ", current_position.z); + SERIAL_ECHOLNPGM("current_position.e ", current_position.e); + SERIAL_ECHOLNPGM("current_hop ", current_hop); //*/ const float base_retract = TERN1(RETRACT_SYNC_MIXING, (MIXING_STEPPERS)) @@ -183,19 +181,90 @@ void FWRetract::retract(const bool retracting #endif /* // debugging - SERIAL_ECHOLNPAIR("retracting ", retracting); - SERIAL_ECHOLNPAIR("swapping ", swapping); - SERIAL_ECHOLNPAIR("active_extruder ", active_extruder); + SERIAL_ECHOLNPGM("retracting ", AS_DIGIT(retracting)); + SERIAL_ECHOLNPGM("swapping ", AS_DIGIT(swapping)); + SERIAL_ECHOLNPGM("active_extruder ", active_extruder); LOOP_L_N(i, EXTRUDERS) { - SERIAL_ECHOLNPAIR("retracted[", i, "] ", retracted[i]); + SERIAL_ECHOLNPGM("retracted[", i, "] ", AS_DIGIT(retracted[i])); #if HAS_MULTI_EXTRUDER - SERIAL_ECHOLNPAIR("retracted_swap[", i, "] ", retracted_swap[i]); + SERIAL_ECHOLNPGM("retracted_swap[", i, "] ", AS_DIGIT(retracted_swap[i])); #endif } - SERIAL_ECHOLNPAIR("current_position.z ", current_position.z); - SERIAL_ECHOLNPAIR("current_position.e ", current_position.e); - SERIAL_ECHOLNPAIR("current_hop ", current_hop); + SERIAL_ECHOLNPGM("current_position.z ", current_position.z); + SERIAL_ECHOLNPGM("current_position.e ", current_position.e); + SERIAL_ECHOLNPGM("current_hop ", current_hop); //*/ } +//extern const char SP_Z_STR[]; + +/** + * M207: Set firmware retraction values + * + * S[+units] retract_length + * W[+units] swap_retract_length (multi-extruder) + * F[units/min] retract_feedrate_mm_s + * Z[units] retract_zraise + */ +void FWRetract::M207() { + if (!parser.seen("FSWZ")) return M207_report(); + if (parser.seenval('S')) settings.retract_length = parser.value_axis_units(E_AXIS); + if (parser.seenval('F')) settings.retract_feedrate_mm_s = MMM_TO_MMS(parser.value_axis_units(E_AXIS)); + if (parser.seenval('Z')) settings.retract_zraise = parser.value_linear_units(); + if (parser.seenval('W')) settings.swap_retract_length = parser.value_axis_units(E_AXIS); +} + +void FWRetract::M207_report() { + SERIAL_ECHOLNPGM_P( + PSTR(" M207 S"), LINEAR_UNIT(settings.retract_length) + , PSTR(" W"), LINEAR_UNIT(settings.swap_retract_length) + , PSTR(" F"), LINEAR_UNIT(MMS_TO_MMM(settings.retract_feedrate_mm_s)) + , SP_Z_STR, LINEAR_UNIT(settings.retract_zraise) + ); +} + +/** + * M208: Set firmware un-retraction values + * + * S[+units] retract_recover_extra (in addition to M207 S*) + * W[+units] swap_retract_recover_extra (multi-extruder) + * F[units/min] retract_recover_feedrate_mm_s + * R[units/min] swap_retract_recover_feedrate_mm_s + */ +void FWRetract::M208() { + if (!parser.seen("FSRW")) return M208_report(); + if (parser.seen('S')) settings.retract_recover_extra = parser.value_axis_units(E_AXIS); + if (parser.seen('F')) settings.retract_recover_feedrate_mm_s = MMM_TO_MMS(parser.value_axis_units(E_AXIS)); + if (parser.seen('R')) settings.swap_retract_recover_feedrate_mm_s = MMM_TO_MMS(parser.value_axis_units(E_AXIS)); + if (parser.seen('W')) settings.swap_retract_recover_extra = parser.value_axis_units(E_AXIS); +} + +void FWRetract::M208_report() { + SERIAL_ECHOLNPGM( + " M208 S", LINEAR_UNIT(settings.retract_recover_extra) + , " W", LINEAR_UNIT(settings.swap_retract_recover_extra) + , " F", LINEAR_UNIT(MMS_TO_MMM(settings.retract_recover_feedrate_mm_s)) + ); +} + +#if ENABLED(FWRETRACT_AUTORETRACT) + + /** + * M209: Enable automatic retract (M209 S1) + * For slicers that don't support G10/11, reversed extrude-only + * moves will be classified as retraction. + */ + void FWRetract::M209() { + if (!parser.seen('S')) return M209_report(); + if (MIN_AUTORETRACT <= MAX_AUTORETRACT) + enable_autoretract(parser.value_bool()); + } + + void FWRetract::M209_report() { + SERIAL_ECHOLNPGM(" M209 S", AS_DIGIT(autoretract_enabled)); + } + +#endif // FWRETRACT_AUTORETRACT + + #endif // FWRETRACT diff --git a/Marlin/src/feature/fwretract.h b/Marlin/src/feature/fwretract.h index 134851965dc4..d6d0432e3aa7 100644 --- a/Marlin/src/feature/fwretract.h +++ b/Marlin/src/feature/fwretract.h @@ -74,11 +74,16 @@ class FWRetract { #endif } - static void retract(const bool retracting - #if HAS_MULTI_EXTRUDER - , bool swapping = false - #endif - ); + static void retract(const bool retracting E_OPTARG(bool swapping=false)); + + static void M207_report(); + static void M207(); + static void M208_report(); + static void M208(); + #if ENABLED(FWRETRACT_AUTORETRACT) + static void M209_report(); + static void M209(); + #endif }; extern FWRetract fwretract; diff --git a/Marlin/src/feature/host_actions.cpp b/Marlin/src/feature/host_actions.cpp index 2a0b3dc3d5b9..c03a6bc5976e 100644 --- a/Marlin/src/feature/host_actions.cpp +++ b/Marlin/src/feature/host_actions.cpp @@ -24,10 +24,10 @@ #if ENABLED(HOST_ACTION_COMMANDS) -#include "host_actions.h" - //#define DEBUG_HOST_ACTIONS +#include "host_actions.h" + #if ENABLED(ADVANCED_PAUSE_FEATURE) #include "pause.h" #include "../gcode/queue.h" @@ -37,37 +37,54 @@ #include "runout.h" #endif -void host_action(PGM_P const pstr, const bool eol) { - PORT_REDIRECT(SERIAL_ALL); +HostUI hostui; + +void HostUI::action(FSTR_P const fstr, const bool eol) { + PORT_REDIRECT(SerialMask::All); SERIAL_ECHOPGM("//action:"); - SERIAL_ECHOPGM_P(pstr); + SERIAL_ECHOF(fstr); if (eol) SERIAL_EOL(); } #ifdef ACTION_ON_KILL - void host_action_kill() { host_action(PSTR(ACTION_ON_KILL)); } + void HostUI::kill() { action(F(ACTION_ON_KILL)); } #endif #ifdef ACTION_ON_PAUSE - void host_action_pause(const bool eol/*=true*/) { host_action(PSTR(ACTION_ON_PAUSE), eol); } + void HostUI::pause(const bool eol/*=true*/) { action(F(ACTION_ON_PAUSE), eol); } #endif #ifdef ACTION_ON_PAUSED - void host_action_paused(const bool eol/*=true*/) { host_action(PSTR(ACTION_ON_PAUSED), eol); } + void HostUI::paused(const bool eol/*=true*/) { action(F(ACTION_ON_PAUSED), eol); } #endif #ifdef ACTION_ON_RESUME - void host_action_resume() { host_action(PSTR(ACTION_ON_RESUME)); } + void HostUI::resume() { action(F(ACTION_ON_RESUME)); } #endif #ifdef ACTION_ON_RESUMED - void host_action_resumed() { host_action(PSTR(ACTION_ON_RESUMED)); } + void HostUI::resumed() { action(F(ACTION_ON_RESUMED)); } #endif #ifdef ACTION_ON_CANCEL - void host_action_cancel() { host_action(PSTR(ACTION_ON_CANCEL)); } + void HostUI::cancel() { action(F(ACTION_ON_CANCEL)); } #endif #ifdef ACTION_ON_START - void host_action_start() { host_action(PSTR(ACTION_ON_START)); } + void HostUI::start() { action(F(ACTION_ON_START)); } +#endif + +#if ENABLED(G29_RETRY_AND_RECOVER) + #ifdef ACTION_ON_G29_RECOVER + void HostUI::g29_recover() { action(F(ACTION_ON_G29_RECOVER)); } + #endif + #ifdef ACTION_ON_G29_FAILURE + void HostUI::g29_failure() { action(F(ACTION_ON_G29_FAILURE)); } + #endif +#endif + +#ifdef SHUTDOWN_ACTION + void HostUI::shutdown() { action(F(SHUTDOWN_ACTION)); } #endif #if ENABLED(HOST_PROMPT_SUPPORT) + PromptReason HostUI::host_prompt_reason = PROMPT_NOT_DEFINED; + PGMSTR(CONTINUE_STR, "Continue"); PGMSTR(DISMISS_STR, "Dismiss"); @@ -75,64 +92,64 @@ void host_action(PGM_P const pstr, const bool eol) { extern bool wait_for_user; #endif - PromptReason host_prompt_reason = PROMPT_NOT_DEFINED; - - void host_action_notify(const char * const message) { - PORT_REDIRECT(SERIAL_ALL); - host_action(PSTR("notification "), false); - SERIAL_ECHOLN(message); + void HostUI::notify(const char * const cstr) { + PORT_REDIRECT(SerialMask::All); + action(F("notification "), false); + SERIAL_ECHOLN(cstr); } - void host_action_notify_P(PGM_P const message) { - PORT_REDIRECT(SERIAL_ALL); - host_action(PSTR("notification "), false); - SERIAL_ECHOLNPGM_P(message); + void HostUI::notify_P(PGM_P const pstr) { + PORT_REDIRECT(SerialMask::All); + action(F("notification "), false); + SERIAL_ECHOLNPGM_P(pstr); } - void host_action_prompt(PGM_P const ptype, const bool eol=true) { - PORT_REDIRECT(SERIAL_ALL); - host_action(PSTR("prompt_"), false); - SERIAL_ECHOPGM_P(ptype); + void HostUI::prompt(FSTR_P const ptype, const bool eol/*=true*/) { + PORT_REDIRECT(SerialMask::All); + action(F("prompt_"), false); + SERIAL_ECHOF(ptype); if (eol) SERIAL_EOL(); } - void host_action_prompt_plus(PGM_P const ptype, PGM_P const pstr, const char extra_char='\0') { - host_action_prompt(ptype, false); - PORT_REDIRECT(SERIAL_ALL); + void HostUI::prompt_plus(FSTR_P const ptype, FSTR_P const fstr, const char extra_char/*='\0'*/) { + prompt(ptype, false); + PORT_REDIRECT(SerialMask::All); SERIAL_CHAR(' '); - SERIAL_ECHOPGM_P(pstr); + SERIAL_ECHOF(fstr); if (extra_char != '\0') SERIAL_CHAR(extra_char); SERIAL_EOL(); } - void host_action_prompt_begin(const PromptReason reason, PGM_P const pstr, const char extra_char/*='\0'*/) { - host_action_prompt_end(); + void HostUI::prompt_begin(const PromptReason reason, FSTR_P const fstr, const char extra_char/*='\0'*/) { + prompt_end(); host_prompt_reason = reason; - host_action_prompt_plus(PSTR("begin"), pstr, extra_char); + prompt_plus(F("begin"), fstr, extra_char); } - void host_action_prompt_button(PGM_P const pstr) { host_action_prompt_plus(PSTR("button"), pstr); } - void host_action_prompt_end() { host_action_prompt(PSTR("end")); } - void host_action_prompt_show() { host_action_prompt(PSTR("show")); } - - void _host_prompt_show(PGM_P const btn1/*=nullptr*/, PGM_P const btn2/*=nullptr*/) { - if (btn1) host_action_prompt_button(btn1); - if (btn2) host_action_prompt_button(btn2); - host_action_prompt_show(); + void HostUI::prompt_button(FSTR_P const fstr) { prompt_plus(F("button"), fstr); } + void HostUI::prompt_end() { prompt(F("end")); } + void HostUI::prompt_show() { prompt(F("show")); } + + void HostUI::_prompt_show(FSTR_P const btn1, FSTR_P const btn2) { + if (btn1) prompt_button(btn1); + if (btn2) prompt_button(btn2); + prompt_show(); } - void host_prompt_do(const PromptReason reason, PGM_P const pstr, PGM_P const btn1/*=nullptr*/, PGM_P const btn2/*=nullptr*/) { - host_action_prompt_begin(reason, pstr); - _host_prompt_show(btn1, btn2); + void HostUI::prompt_do(const PromptReason reason, FSTR_P const fstr, FSTR_P const btn1/*=nullptr*/, FSTR_P const btn2/*=nullptr*/) { + prompt_begin(reason, fstr); + _prompt_show(btn1, btn2); } - void host_prompt_do(const PromptReason reason, PGM_P const pstr, const char extra_char, PGM_P const btn1/*=nullptr*/, PGM_P const btn2/*=nullptr*/) { - host_action_prompt_begin(reason, pstr, extra_char); - _host_prompt_show(btn1, btn2); + void HostUI::prompt_do(const PromptReason reason, FSTR_P const fstr, const char extra_char, FSTR_P const btn1/*=nullptr*/, FSTR_P const btn2/*=nullptr*/) { + prompt_begin(reason, fstr, extra_char); + _prompt_show(btn1, btn2); } - void filament_load_host_prompt() { - const bool disable_to_continue = TERN0(HAS_FILAMENT_SENSOR, runout.filament_ran_out); - host_prompt_do(PROMPT_FILAMENT_RUNOUT, PSTR("Paused"), PSTR("PurgeMore"), - disable_to_continue ? PSTR("DisableRunout") : CONTINUE_STR - ); - } + #if ENABLED(ADVANCED_PAUSE_FEATURE) + void HostUI::filament_load_prompt() { + const bool disable_to_continue = TERN0(HAS_FILAMENT_SENSOR, runout.filament_ran_out); + prompt_do(PROMPT_FILAMENT_RUNOUT, F("Paused"), F("PurgeMore"), + disable_to_continue ? F("DisableRunout") : FPSTR(CONTINUE_STR) + ); + } + #endif // // Handle responses from the host, such as: @@ -141,7 +158,7 @@ void host_action(PGM_P const pstr, const bool eol) { // - Resume Print response // - Dismissal of info // - void host_response_handler(const uint8_t response) { + void HostUI::handle_response(const uint8_t response) { const PromptReason hpr = host_prompt_reason; host_prompt_reason = PROMPT_NOT_DEFINED; // Reset now ahead of logic switch (hpr) { @@ -149,13 +166,13 @@ void host_action(PGM_P const pstr, const bool eol) { switch (response) { case 0: // "Purge More" button - #if BOTH(HAS_LCD_MENU, ADVANCED_PAUSE_FEATURE) + #if BOTH(M600_PURGE_MORE_RESUMABLE, ADVANCED_PAUSE_FEATURE) pause_menu_response = PAUSE_RESPONSE_EXTRUDE_MORE; // Simulate menu selection (menu exits, doesn't extrude more) #endif break; case 1: // "Continue" / "Disable Runout" button - #if BOTH(HAS_LCD_MENU, ADVANCED_PAUSE_FEATURE) + #if BOTH(M600_PURGE_MORE_RESUMABLE, ADVANCED_PAUSE_FEATURE) pause_menu_response = PAUSE_RESPONSE_RESUME_PRINT; // Simulate menu selection #endif #if HAS_FILAMENT_SENSOR diff --git a/Marlin/src/feature/host_actions.h b/Marlin/src/feature/host_actions.h index 065b59d755ed..41d66b82ec9b 100644 --- a/Marlin/src/feature/host_actions.h +++ b/Marlin/src/feature/host_actions.h @@ -24,34 +24,8 @@ #include "../inc/MarlinConfigPre.h" #include "../HAL/shared/Marduino.h" -void host_action(PGM_P const pstr, const bool eol=true); - -#ifdef ACTION_ON_KILL - void host_action_kill(); -#endif -#ifdef ACTION_ON_PAUSE - void host_action_pause(const bool eol=true); -#endif -#ifdef ACTION_ON_PAUSED - void host_action_paused(const bool eol=true); -#endif -#ifdef ACTION_ON_RESUME - void host_action_resume(); -#endif -#ifdef ACTION_ON_RESUMED - void host_action_resumed(); -#endif -#ifdef ACTION_ON_CANCEL - void host_action_cancel(); -#endif -#ifdef ACTION_ON_START - void host_action_start(); -#endif - #if ENABLED(HOST_PROMPT_SUPPORT) - extern const char CONTINUE_STR[], DISMISS_STR[]; - enum PromptReason : uint8_t { PROMPT_NOT_DEFINED, PROMPT_FILAMENT_RUNOUT, @@ -61,21 +35,80 @@ void host_action(PGM_P const pstr, const bool eol=true); PROMPT_INFO }; - extern PromptReason host_prompt_reason; +#endif - void host_response_handler(const uint8_t response); - void host_action_notify(const char * const message); - void host_action_notify_P(PGM_P const message); - void host_action_prompt_begin(const PromptReason reason, PGM_P const pstr, const char extra_char='\0'); - void host_action_prompt_button(PGM_P const pstr); - void host_action_prompt_end(); - void host_action_prompt_show(); - void host_prompt_do(const PromptReason reason, PGM_P const pstr, PGM_P const btn1=nullptr, PGM_P const btn2=nullptr); - void host_prompt_do(const PromptReason reason, PGM_P const pstr, const char extra_char, PGM_P const btn1=nullptr, PGM_P const btn2=nullptr); - inline void host_prompt_open(const PromptReason reason, PGM_P const pstr, PGM_P const btn1=nullptr, PGM_P const btn2=nullptr) { - if (host_prompt_reason == PROMPT_NOT_DEFINED) host_prompt_do(reason, pstr, btn1, btn2); - } +class HostUI { + public: - void filament_load_host_prompt(); + static void action(FSTR_P const fstr, const bool eol=true); -#endif + #ifdef ACTION_ON_KILL + static void kill(); + #endif + #ifdef ACTION_ON_PAUSE + static void pause(const bool eol=true); + #endif + #ifdef ACTION_ON_PAUSED + static void paused(const bool eol=true); + #endif + #ifdef ACTION_ON_RESUME + static void resume(); + #endif + #ifdef ACTION_ON_RESUMED + static void resumed(); + #endif + #ifdef ACTION_ON_CANCEL + static void cancel(); + #endif + #ifdef ACTION_ON_START + static void start(); + #endif + #ifdef SHUTDOWN_ACTION + static void shutdown(); + #endif + + #if ENABLED(G29_RETRY_AND_RECOVER) + #ifdef ACTION_ON_G29_RECOVER + static void g29_recover(); + #endif + #ifdef ACTION_ON_G29_FAILURE + static void g29_failure(); + #endif + #endif + + #if ENABLED(HOST_PROMPT_SUPPORT) + private: + static void prompt(FSTR_P const ptype, const bool eol=true); + static void prompt_plus(FSTR_P const ptype, FSTR_P const fstr, const char extra_char='\0'); + static void prompt_show(); + static void _prompt_show(FSTR_P const btn1, FSTR_P const btn2); + + public: + static PromptReason host_prompt_reason; + + static void handle_response(const uint8_t response); + + static void notify_P(PGM_P const message); + static void notify(FSTR_P const fmsg) { notify_P(FTOP(fmsg)); } + static void notify(const char * const message); + + static void prompt_begin(const PromptReason reason, FSTR_P const fstr, const char extra_char='\0'); + static void prompt_button(FSTR_P const fstr); + static void prompt_end(); + static void prompt_do(const PromptReason reason, FSTR_P const pstr, FSTR_P const btn1=nullptr, FSTR_P const btn2=nullptr); + static void prompt_do(const PromptReason reason, FSTR_P const pstr, const char extra_char, FSTR_P const btn1=nullptr, FSTR_P const btn2=nullptr); + static void prompt_open(const PromptReason reason, FSTR_P const pstr, FSTR_P const btn1=nullptr, FSTR_P const btn2=nullptr) { + if (host_prompt_reason == PROMPT_NOT_DEFINED) prompt_do(reason, pstr, btn1, btn2); + } + + #if ENABLED(ADVANCED_PAUSE_FEATURE) + static void filament_load_prompt(); + #endif + + #endif + +}; + +extern HostUI hostui; + +extern const char CONTINUE_STR[], DISMISS_STR[]; diff --git a/Marlin/src/feature/hotend_idle.cpp b/Marlin/src/feature/hotend_idle.cpp index 7f8f20a04793..4b137f42da7b 100644 --- a/Marlin/src/feature/hotend_idle.cpp +++ b/Marlin/src/feature/hotend_idle.cpp @@ -34,6 +34,7 @@ #include "../module/temperature.h" #include "../module/motion.h" +#include "../module/planner.h" #include "../lcd/marlinui.h" extern HotendIdleProtection hotend_idle; @@ -43,7 +44,8 @@ millis_t HotendIdleProtection::next_protect_ms = 0; void HotendIdleProtection::check_hotends(const millis_t &ms) { bool do_prot = false; HOTEND_LOOP() { - if (thermalManager.degHotend(e) >= HOTEND_IDLE_MIN_TRIGGER) { + const bool busy = (TERN0(HAS_RESUME_CONTINUE, wait_for_user) || planner.has_blocks_queued()); + if (thermalManager.degHotend(e) >= (HOTEND_IDLE_MIN_TRIGGER) && !busy) { do_prot = true; break; } } @@ -75,7 +77,7 @@ void HotendIdleProtection::check() { void HotendIdleProtection::timed_out() { next_protect_ms = 0; SERIAL_ECHOLNPGM("Hotend Idle Timeout"); - LCD_MESSAGEPGM(MSG_HOTEND_IDLE_TIMEOUT); + LCD_MESSAGE(MSG_HOTEND_IDLE_TIMEOUT); HOTEND_LOOP() { if ((HOTEND_IDLE_NOZZLE_TARGET) < thermalManager.degTargetHotend(e)) thermalManager.setTargetHotend(HOTEND_IDLE_NOZZLE_TARGET, e); diff --git a/Marlin/src/feature/joystick.cpp b/Marlin/src/feature/joystick.cpp index 3dca2eb2e9bd..7f91c1549b9e 100644 --- a/Marlin/src/feature/joystick.cpp +++ b/Marlin/src/feature/joystick.cpp @@ -68,13 +68,13 @@ Joystick joystick; void Joystick::report() { SERIAL_ECHOPGM("Joystick"); #if HAS_JOY_ADC_X - SERIAL_ECHOPAIR_P(SP_X_STR, JOY_X(x.raw)); + SERIAL_ECHOPGM_P(SP_X_STR, JOY_X(x.raw)); #endif #if HAS_JOY_ADC_Y - SERIAL_ECHOPAIR_P(SP_Y_STR, JOY_Y(y.raw)); + SERIAL_ECHOPGM_P(SP_Y_STR, JOY_Y(y.raw)); #endif #if HAS_JOY_ADC_Z - SERIAL_ECHOPAIR_P(SP_Z_STR, JOY_Z(z.raw)); + SERIAL_ECHOPGM_P(SP_Z_STR, JOY_Z(z.raw)); #endif #if HAS_JOY_ADC_EN SERIAL_ECHO_TERNARY(READ(JOY_EN_PIN), " EN=", "HIGH (dis", "LOW (en", "abled)"); @@ -163,13 +163,8 @@ Joystick joystick; // norm_jog values of [-1 .. 1] maps linearly to [-feedrate .. feedrate] xyz_float_t move_dist{0}; float hypot2 = 0; - LOOP_XYZ(i) if (norm_jog[i]) { - move_dist[i] = seg_time * norm_jog[i] * - #if ENABLED(EXTENSIBLE_UI) - manual_feedrate_mm_s[i]; - #else - planner.settings.max_feedrate_mm_s[i]; - #endif + LOOP_LINEAR_AXES(i) if (norm_jog[i]) { + move_dist[i] = seg_time * norm_jog[i] * TERN(EXTENSIBLE_UI, manual_feedrate_mm_s, planner.settings.max_feedrate_mm_s)[i]; hypot2 += sq(move_dist[i]); } diff --git a/Marlin/src/feature/joystick.h b/Marlin/src/feature/joystick.h index e8e218b2f940..91bf6bdc00d2 100644 --- a/Marlin/src/feature/joystick.h +++ b/Marlin/src/feature/joystick.h @@ -32,11 +32,19 @@ class Joystick { friend class Temperature; private: - TERN_(HAS_JOY_ADC_X, static temp_info_t x); - TERN_(HAS_JOY_ADC_Y, static temp_info_t y); - TERN_(HAS_JOY_ADC_Z, static temp_info_t z); + #if HAS_JOY_ADC_X + static temp_info_t x; + #endif + #if HAS_JOY_ADC_Y + static temp_info_t y; + #endif + #if HAS_JOY_ADC_Z + static temp_info_t z; + #endif public: - TERN_(JOYSTICK_DEBUG, static void report()); + #if ENABLED(JOYSTICK_DEBUG) + static void report(); + #endif static void calculate(xyz_float_t &norm_jog); static void inject_jog_moves(); }; diff --git a/Marlin/src/feature/leds/leds.cpp b/Marlin/src/feature/leds/leds.cpp index ef0561a435f2..2a53a7c884e6 100644 --- a/Marlin/src/feature/leds/leds.cpp +++ b/Marlin/src/feature/leds/leds.cpp @@ -42,18 +42,19 @@ #include "pca9533.h" #endif -#if ENABLED(CASE_LIGHT_USE_RGB_LED) +#if EITHER(CASE_LIGHT_USE_RGB_LED, CASE_LIGHT_USE_NEOPIXEL) #include "../../feature/caselight.h" #endif #if ENABLED(LED_COLOR_PRESETS) - const LEDColor LEDLights::defaultLEDColor = MakeLEDColor( - LED_USER_PRESET_RED, LED_USER_PRESET_GREEN, LED_USER_PRESET_BLUE, - LED_USER_PRESET_WHITE, LED_USER_PRESET_BRIGHTNESS + const LEDColor LEDLights::defaultLEDColor = LEDColor( + LED_USER_PRESET_RED, LED_USER_PRESET_GREEN, LED_USER_PRESET_BLUE + OPTARG(HAS_WHITE_LED, LED_USER_PRESET_WHITE) + OPTARG(NEOPIXEL_LED, LED_USER_PRESET_BRIGHTNESS) ); #endif -#if EITHER(LED_CONTROL_MENU, PRINTER_EVENT_LEDS) +#if ANY(LED_CONTROL_MENU, PRINTER_EVENT_LEDS, CASE_LIGHT_IS_COLOR_LED) LEDColor LEDLights::color; bool LEDLights::lights_on; #endif @@ -75,37 +76,44 @@ void LEDLights::setup() { } void LEDLights::set_color(const LEDColor &incol - #if ENABLED(NEOPIXEL_LED) - , bool isSequence/*=false*/ - #endif + OPTARG(NEOPIXEL_IS_SEQUENTIAL, bool isSequence/*=false*/) ) { #if ENABLED(NEOPIXEL_LED) const uint32_t neocolor = LEDColorWhite() == incol ? neo.Color(NEO_WHITE) - : neo.Color(incol.r, incol.g, incol.b, incol.w); - static uint16_t nextLed = 0; - - #ifdef NEOPIXEL_BKGD_LED_INDEX - if (NEOPIXEL_BKGD_LED_INDEX == nextLed) { - neo.set_color_background(); - if (++nextLed >= neo.pixels()) { - nextLed = 0; - return; + : neo.Color(incol.r, incol.g, incol.b OPTARG(HAS_WHITE_LED, incol.w)); + + #if ENABLED(NEOPIXEL_IS_SEQUENTIAL) + static uint16_t nextLed = 0; + #ifdef NEOPIXEL_BKGD_INDEX_FIRST + while (WITHIN(nextLed, NEOPIXEL_BKGD_INDEX_FIRST, NEOPIXEL_BKGD_INDEX_LAST)) { + neo.reset_background_color(); + if (++nextLed >= neo.pixels()) { nextLed = 0; return; } } - } + #endif #endif + #if BOTH(CASE_LIGHT_MENU, CASE_LIGHT_USE_NEOPIXEL) + // Update brightness only if caselight is ON or switching leds off + if (caselight.on || incol.is_off()) + #endif neo.set_brightness(incol.i); - if (isSequence) { - neo.set_pixel_color(nextLed, neocolor); - neo.show(); - if (++nextLed >= neo.pixels()) nextLed = 0; - return; - } + #if ENABLED(NEOPIXEL_IS_SEQUENTIAL) + if (isSequence) { + neo.set_pixel_color(nextLed, neocolor); + neo.show(); + if (++nextLed >= neo.pixels()) nextLed = 0; + return; + } + #endif + #if BOTH(CASE_LIGHT_MENU, CASE_LIGHT_USE_NEOPIXEL) + // Update color only if caselight is ON or switching leds off + if (caselight.on || incol.is_off()) + #endif neo.set_color(neocolor); #endif @@ -121,11 +129,11 @@ void LEDLights::set_color(const LEDColor &incol // This variant uses 3-4 separate pins for the RGB(W) components. // If the pins can do PWM then their intensity will be set. - #define _UPDATE_RGBW(C,c) do { \ - if (PWM_PIN(RGB_LED_##C##_PIN)) \ - analogWrite(pin_t(RGB_LED_##C##_PIN), c); \ - else \ - WRITE(RGB_LED_##C##_PIN, c ? HIGH : LOW); \ + #define _UPDATE_RGBW(C,c) do { \ + if (PWM_PIN(RGB_LED_##C##_PIN)) \ + hal.set_pwm_duty(pin_t(RGB_LED_##C##_PIN), c); \ + else \ + WRITE(RGB_LED_##C##_PIN, c ? HIGH : LOW); \ }while(0) #define UPDATE_RGBW(C,c) _UPDATE_RGBW(C, TERN1(CASE_LIGHT_USE_RGB_LED, caselight.on) ? incol.c : 0) UPDATE_RGBW(R,r); UPDATE_RGBW(G,g); UPDATE_RGBW(B,b); @@ -150,7 +158,7 @@ void LEDLights::set_color(const LEDColor &incol void LEDLights::toggle() { if (lights_on) set_off(); else update(); } #endif -#ifdef LED_BACKLIGHT_TIMEOUT +#if LED_POWEROFF_TIMEOUT > 0 millis_t LEDLights::led_off_time; // = 0 @@ -169,9 +177,10 @@ void LEDLights::set_color(const LEDColor &incol #if ENABLED(NEOPIXEL2_SEPARATE) #if ENABLED(NEO2_COLOR_PRESETS) - const LEDColor LEDLights2::defaultLEDColor = MakeLEDColor( - NEO2_USER_PRESET_RED, NEO2_USER_PRESET_GREEN, NEO2_USER_PRESET_BLUE, - NEO2_USER_PRESET_WHITE, NEO2_USER_PRESET_BRIGHTNESS + const LEDColor LEDLights2::defaultLEDColor = LEDColor( + NEO2_USER_PRESET_RED, NEO2_USER_PRESET_GREEN, NEO2_USER_PRESET_BLUE + OPTARG(HAS_WHITE_LED2, NEO2_USER_PRESET_WHITE) + OPTARG(NEOPIXEL_LED, NEO2_USER_PRESET_BRIGHTNESS) ); #endif @@ -190,7 +199,7 @@ void LEDLights::set_color(const LEDColor &incol void LEDLights2::set_color(const LEDColor &incol) { const uint32_t neocolor = LEDColorWhite() == incol ? neo2.Color(NEO2_WHITE) - : neo2.Color(incol.r, incol.g, incol.b, incol.w); + : neo2.Color(incol.r, incol.g, incol.b OPTARG(HAS_WHITE_LED2, incol.w)); neo2.set_brightness(incol.i); neo2.set_color(neocolor); diff --git a/Marlin/src/feature/leds/leds.h b/Marlin/src/feature/leds/leds.h index c34eb57f4493..bce9052424a8 100644 --- a/Marlin/src/feature/leds/leds.h +++ b/Marlin/src/feature/leds/leds.h @@ -29,60 +29,37 @@ #include -#if ENABLED(NEOPIXEL_LED) - #include "neopixel.h" -#endif - // A white component can be passed -#if ANY(RGBW_LED, NEOPIXEL_LED, PCA9632_RGBW) +#if EITHER(RGBW_LED, PCA9632_RGBW) #define HAS_WHITE_LED 1 #endif +#if ENABLED(NEOPIXEL_LED) + #define _NEOPIXEL_INCLUDE_ + #include "neopixel.h" + #undef _NEOPIXEL_INCLUDE_ +#endif + /** * LEDcolor type for use with leds.set_color */ typedef struct LEDColor { uint8_t r, g, b - #if HAS_WHITE_LED - , w - #if ENABLED(NEOPIXEL_LED) - , i - #endif - #endif + OPTARG(HAS_WHITE_LED, w) + OPTARG(NEOPIXEL_LED, i) ; LEDColor() : r(255), g(255), b(255) - #if HAS_WHITE_LED - , w(255) - #if ENABLED(NEOPIXEL_LED) - , i(NEOPIXEL_BRIGHTNESS) - #endif - #endif + OPTARG(HAS_WHITE_LED, w(255)) + OPTARG(NEOPIXEL_LED, i(NEOPIXEL_BRIGHTNESS)) {} - LEDColor(uint8_t r, uint8_t g, uint8_t b - #if HAS_WHITE_LED - , uint8_t w=0 - #if ENABLED(NEOPIXEL_LED) - , uint8_t i=NEOPIXEL_BRIGHTNESS - #endif - #endif - ) : r(r), g(g), b(b) - #if HAS_WHITE_LED - , w(w) - #if ENABLED(NEOPIXEL_LED) - , i(i) - #endif - #endif - {} + LEDColor(uint8_t r, uint8_t g, uint8_t b OPTARG(HAS_WHITE_LED, uint8_t w=0) OPTARG(NEOPIXEL_LED, uint8_t i=NEOPIXEL_BRIGHTNESS)) + : r(r), g(g), b(b) OPTARG(HAS_WHITE_LED, w(w)) OPTARG(NEOPIXEL_LED, i(i)) {} LEDColor(const uint8_t (&rgbw)[4]) : r(rgbw[0]), g(rgbw[1]), b(rgbw[2]) - #if HAS_WHITE_LED - , w(rgbw[3]) - #if ENABLED(NEOPIXEL_LED) - , i(NEOPIXEL_BRIGHTNESS) - #endif - #endif + OPTARG(HAS_WHITE_LED, w(rgbw[3])) + OPTARG(NEOPIXEL_LED, i(NEOPIXEL_BRIGHTNESS)) {} LEDColor& operator=(const uint8_t (&rgbw)[4]) { @@ -109,17 +86,8 @@ typedef struct LEDColor { } LEDColor; /** - * Color helpers and presets + * Color presets */ -#if HAS_WHITE_LED - #if ENABLED(NEOPIXEL_LED) - #define MakeLEDColor(R,G,B,W,I) LEDColor(R, G, B, W, I) - #else - #define MakeLEDColor(R,G,B,W,I) LEDColor(R, G, B, W) - #endif -#else - #define MakeLEDColor(R,G,B,W,I) LEDColor(R, G, B) -#endif #define LEDColorOff() LEDColor( 0, 0, 0) #define LEDColorRed() LEDColor(255, 0, 0) @@ -147,47 +115,37 @@ class LEDLights { static void setup(); // init() static void set_color(const LEDColor &color - #if ENABLED(NEOPIXEL_LED) - , bool isSequence=false - #endif + OPTARG(NEOPIXEL_IS_SEQUENTIAL, bool isSequence=false) ); - static inline void set_color(uint8_t r, uint8_t g, uint8_t b - #if HAS_WHITE_LED - , uint8_t w=0 - #endif - #if ENABLED(NEOPIXEL_LED) - , uint8_t i=NEOPIXEL_BRIGHTNESS - , bool isSequence=false - #endif + static void set_color(uint8_t r, uint8_t g, uint8_t b + OPTARG(HAS_WHITE_LED, uint8_t w=0) + OPTARG(NEOPIXEL_LED, uint8_t i=NEOPIXEL_BRIGHTNESS) + OPTARG(NEOPIXEL_IS_SEQUENTIAL, bool isSequence=false) ) { - set_color(MakeLEDColor(r, g, b, w, i) - #if ENABLED(NEOPIXEL_LED) - , isSequence - #endif - ); + set_color(LEDColor(r, g, b OPTARG(HAS_WHITE_LED, w) OPTARG(NEOPIXEL_LED, i)) OPTARG(NEOPIXEL_IS_SEQUENTIAL, isSequence)); } - static inline void set_off() { set_color(LEDColorOff()); } - static inline void set_green() { set_color(LEDColorGreen()); } - static inline void set_white() { set_color(LEDColorWhite()); } + static void set_off() { set_color(LEDColorOff()); } + static void set_green() { set_color(LEDColorGreen()); } + static void set_white() { set_color(LEDColorWhite()); } #if ENABLED(LED_COLOR_PRESETS) static const LEDColor defaultLEDColor; - static inline void set_default() { set_color(defaultLEDColor); } - static inline void set_red() { set_color(LEDColorRed()); } - static inline void set_orange() { set_color(LEDColorOrange()); } - static inline void set_yellow() { set_color(LEDColorYellow()); } - static inline void set_blue() { set_color(LEDColorBlue()); } - static inline void set_indigo() { set_color(LEDColorIndigo()); } - static inline void set_violet() { set_color(LEDColorViolet()); } + static void set_default() { set_color(defaultLEDColor); } + static void set_red() { set_color(LEDColorRed()); } + static void set_orange() { set_color(LEDColorOrange()); } + static void set_yellow() { set_color(LEDColorYellow()); } + static void set_blue() { set_color(LEDColorBlue()); } + static void set_indigo() { set_color(LEDColorIndigo()); } + static void set_violet() { set_color(LEDColorViolet()); } #endif #if ENABLED(PRINTER_EVENT_LEDS) - static inline LEDColor get_color() { return lights_on ? color : LEDColorOff(); } + static LEDColor get_color() { return lights_on ? color : LEDColorOff(); } #endif - #if EITHER(LED_CONTROL_MENU, PRINTER_EVENT_LEDS) + #if ANY(LED_CONTROL_MENU, PRINTER_EVENT_LEDS, CASE_LIGHT_IS_COLOR_LED) static LEDColor color; // last non-off color static bool lights_on; // the last set color was "on" #endif @@ -196,15 +154,15 @@ class LEDLights { static void toggle(); // swap "off" with color #endif #if EITHER(LED_CONTROL_MENU, CASE_LIGHT_USE_RGB_LED) - static inline void update() { set_color(color); } + static void update() { set_color(color); } #endif - #ifdef LED_BACKLIGHT_TIMEOUT + #if LED_POWEROFF_TIMEOUT > 0 private: static millis_t led_off_time; public: - static inline void reset_timeout(const millis_t &ms) { - led_off_time = ms + LED_BACKLIGHT_TIMEOUT; + static void reset_timeout(const millis_t &ms) { + led_off_time = ms + LED_POWEROFF_TIMEOUT; if (!lights_on) update(); } static void update_timeout(const bool power_on); @@ -223,30 +181,36 @@ extern LEDLights leds; static void set_color(const LEDColor &color); - inline void set_color(uint8_t r, uint8_t g, uint8_t b, uint8_t w=0, uint8_t i=NEOPIXEL2_BRIGHTNESS) { - set_color(MakeLEDColor(r, g, b, w, i)); + static void set_color(uint8_t r, uint8_t g, uint8_t b + OPTARG(HAS_WHITE_LED, uint8_t w=0) + OPTARG(NEOPIXEL_LED, uint8_t i=NEOPIXEL_BRIGHTNESS) + ) { + set_color(LEDColor(r, g, b + OPTARG(HAS_WHITE_LED, w) + OPTARG(NEOPIXEL_LED, i) + )); } - static inline void set_off() { set_color(LEDColorOff()); } - static inline void set_green() { set_color(LEDColorGreen()); } - static inline void set_white() { set_color(LEDColorWhite()); } + static void set_off() { set_color(LEDColorOff()); } + static void set_green() { set_color(LEDColorGreen()); } + static void set_white() { set_color(LEDColorWhite()); } #if ENABLED(NEO2_COLOR_PRESETS) static const LEDColor defaultLEDColor; - static inline void set_default() { set_color(defaultLEDColor); } - static inline void set_red() { set_color(LEDColorRed()); } - static inline void set_orange() { set_color(LEDColorOrange()); } - static inline void set_yellow() { set_color(LEDColorYellow()); } - static inline void set_blue() { set_color(LEDColorBlue()); } - static inline void set_indigo() { set_color(LEDColorIndigo()); } - static inline void set_violet() { set_color(LEDColorViolet()); } + static void set_default() { set_color(defaultLEDColor); } + static void set_red() { set_color(LEDColorRed()); } + static void set_orange() { set_color(LEDColorOrange()); } + static void set_yellow() { set_color(LEDColorYellow()); } + static void set_blue() { set_color(LEDColorBlue()); } + static void set_indigo() { set_color(LEDColorIndigo()); } + static void set_violet() { set_color(LEDColorViolet()); } #endif #if ENABLED(NEOPIXEL2_SEPARATE) static LEDColor color; // last non-off color static bool lights_on; // the last set color was "on" static void toggle(); // swap "off" with color - static inline void update() { set_color(color); } + static void update() { set_color(color); } #endif }; diff --git a/Marlin/src/feature/leds/neopixel.cpp b/Marlin/src/feature/leds/neopixel.cpp index bdd22837ada5..3569cb180d53 100644 --- a/Marlin/src/feature/leds/neopixel.cpp +++ b/Marlin/src/feature/leds/neopixel.cpp @@ -28,26 +28,30 @@ #if ENABLED(NEOPIXEL_LED) -#include "neopixel.h" +#include "leds.h" #if EITHER(NEOPIXEL_STARTUP_TEST, NEOPIXEL2_STARTUP_TEST) #include "../../core/utility.h" #endif Marlin_NeoPixel neo; -int8_t Marlin_NeoPixel::neoindex; +pixel_index_t Marlin_NeoPixel::neoindex; -Adafruit_NeoPixel Marlin_NeoPixel::adaneo1(NEOPIXEL_PIXELS, NEOPIXEL_PIN, NEOPIXEL_TYPE + NEO_KHZ800) - #if CONJOINED_NEOPIXEL - , Marlin_NeoPixel::adaneo2(NEOPIXEL_PIXELS, NEOPIXEL2_PIN, NEOPIXEL2_TYPE + NEO_KHZ800) - #endif -; +Adafruit_NeoPixel Marlin_NeoPixel::adaneo1(NEOPIXEL_PIXELS, NEOPIXEL_PIN, NEOPIXEL_TYPE + NEO_KHZ800); +#if CONJOINED_NEOPIXEL + Adafruit_NeoPixel Marlin_NeoPixel::adaneo2(NEOPIXEL_PIXELS, NEOPIXEL2_PIN, NEOPIXEL2_TYPE + NEO_KHZ800); +#endif -#ifdef NEOPIXEL_BKGD_LED_INDEX +#ifdef NEOPIXEL_BKGD_INDEX_FIRST + + void Marlin_NeoPixel::set_background_color(uint8_t r, uint8_t g, uint8_t b, uint8_t w) { + for (int background_led = NEOPIXEL_BKGD_INDEX_FIRST; background_led <= NEOPIXEL_BKGD_INDEX_LAST; background_led++) + set_pixel_color(background_led, adaneo1.Color(r, g, b, w)); + } - void Marlin_NeoPixel::set_color_background() { - uint8_t background_color[4] = NEOPIXEL_BKGD_COLOR; - set_pixel_color(NEOPIXEL_BKGD_LED_INDEX, adaneo1.Color(background_color[0], background_color[1], background_color[2], background_color[3])); + void Marlin_NeoPixel::reset_background_color() { + constexpr uint8_t background_color[4] = NEOPIXEL_BKGD_COLOR; + set_background_color(background_color[0], background_color[1], background_color[2], background_color[3]); } #endif @@ -59,9 +63,10 @@ void Marlin_NeoPixel::set_color(const uint32_t color) { } else { for (uint16_t i = 0; i < pixels(); ++i) { - #ifdef NEOPIXEL_BKGD_LED_INDEX - if (i == NEOPIXEL_BKGD_LED_INDEX && TERN(NEOPIXEL_BKGD_ALWAYS_ON, true, color != 0x000000)) { - set_color_background(); + #ifdef NEOPIXEL_BKGD_INDEX_FIRST + if (i == NEOPIXEL_BKGD_INDEX_FIRST && TERN(NEOPIXEL_BKGD_ALWAYS_ON, true, color != 0x000000)) { + reset_background_color(); + i += NEOPIXEL_BKGD_INDEX_LAST - (NEOPIXEL_BKGD_INDEX_FIRST); continue; } #endif @@ -90,41 +95,28 @@ void Marlin_NeoPixel::init() { safe_delay(500); set_color_startup(adaneo1.Color(0, 0, 255, 0)); // blue safe_delay(500); + #if HAS_WHITE_LED + set_color_startup(adaneo1.Color(0, 0, 0, 255)); // white + safe_delay(500); + #endif #endif - #ifdef NEOPIXEL_BKGD_LED_INDEX - set_color_background(); - #endif - - #if ENABLED(LED_USER_PRESET_STARTUP) - set_color(adaneo1.Color(LED_USER_PRESET_RED, LED_USER_PRESET_GREEN, LED_USER_PRESET_BLUE, LED_USER_PRESET_WHITE)); - #else - set_color(adaneo1.Color(0, 0, 0, 0)); + #ifdef NEOPIXEL_BKGD_INDEX_FIRST + reset_background_color(); #endif -} -#if 0 -bool Marlin_NeoPixel::set_led_color(const uint8_t r, const uint8_t g, const uint8_t b, const uint8_t w, const uint8_t p) { - const uint32_t color = adaneo1.Color(r, g, b, w); - set_brightness(p); - #if DISABLED(NEOPIXEL_IS_SEQUENTIAL) - set_color(color); - return false; - #else - static uint16_t nextLed = 0; - set_pixel_color(nextLed, color); - show(); - if (++nextLed >= pixels()) nextLed = 0; - return true; - #endif + set_color(adaneo1.Color + TERN(LED_USER_PRESET_STARTUP, + (LED_USER_PRESET_RED, LED_USER_PRESET_GREEN, LED_USER_PRESET_BLUE, LED_USER_PRESET_WHITE), + (0, 0, 0, 0)) + ); } -#endif #if ENABLED(NEOPIXEL2_SEPARATE) Marlin_NeoPixel2 neo2; - int8_t Marlin_NeoPixel2::neoindex; + pixel_index_t Marlin_NeoPixel2::neoindex; Adafruit_NeoPixel Marlin_NeoPixel2::adaneo(NEOPIXEL2_PIXELS, NEOPIXEL2_PIN, NEOPIXEL2_TYPE); void Marlin_NeoPixel2::set_color(const uint32_t color) { @@ -158,13 +150,17 @@ bool Marlin_NeoPixel::set_led_color(const uint8_t r, const uint8_t g, const uint safe_delay(500); set_color_startup(adaneo.Color(0, 0, 255, 0)); // blue safe_delay(500); + #if HAS_WHITE_LED2 + set_color_startup(adaneo.Color(0, 0, 0, 255)); // white + safe_delay(500); + #endif #endif - #if ENABLED(NEO2_USER_PRESET_STARTUP) - set_color(adaneo.Color(NEO2_USER_PRESET_RED, NEO2_USER_PRESET_GREEN, NEO2_USER_PRESET_BLUE, NEO2_USER_PRESET_WHITE)); - #else - set_color(adaneo.Color(0, 0, 0, 0)); - #endif + set_color(adaneo.Color + TERN(NEO2_USER_PRESET_STARTUP, + (NEO2_USER_PRESET_RED, NEO2_USER_PRESET_GREEN, NEO2_USER_PRESET_BLUE, NEO2_USER_PRESET_WHITE), + (0, 0, 0, 0)) + ); } #endif // NEOPIXEL2_SEPARATE diff --git a/Marlin/src/feature/leds/neopixel.h b/Marlin/src/feature/leds/neopixel.h index acf2e7f54d3b..1a38ed1a196d 100644 --- a/Marlin/src/feature/leds/neopixel.h +++ b/Marlin/src/feature/leds/neopixel.h @@ -25,6 +25,10 @@ * NeoPixel support */ +#ifndef _NEOPIXEL_INCLUDE_ + #error "Always include 'leds.h' and not 'neopixel.h' directly." +#endif + // ------------------------ // Includes // ------------------------ @@ -38,6 +42,18 @@ // Defines // ------------------------ +#define _NEO_IS_RGB(N) (N == NEO_RGB || N == NEO_RBG || N == NEO_GRB || N == NEO_GBR || N == NEO_BRG || N == NEO_BGR) + +#if !_NEO_IS_RGB(NEOPIXEL_TYPE) + #define HAS_WHITE_LED 1 +#endif + +#if HAS_WHITE_LED + #define NEO_WHITE 0, 0, 0, 255 +#else + #define NEO_WHITE 255, 255, 255 +#endif + #if defined(NEOPIXEL2_TYPE) && NEOPIXEL2_TYPE != NEOPIXEL_TYPE && DISABLED(NEOPIXEL2_SEPARATE) #define MULTIPLE_NEOPIXEL_TYPES 1 #endif @@ -46,65 +62,57 @@ #define CONJOINED_NEOPIXEL 1 #endif -#if NEOPIXEL_TYPE == NEO_RGB || NEOPIXEL_TYPE == NEO_RBG || NEOPIXEL_TYPE == NEO_GRB || NEOPIXEL_TYPE == NEO_GBR || NEOPIXEL_TYPE == NEO_BRG || NEOPIXEL_TYPE == NEO_BGR - #define NEOPIXEL_IS_RGB 1 -#else - #define NEOPIXEL_IS_RGBW 1 -#endif +// ------------------------ +// Types +// ------------------------ -#if NEOPIXEL_IS_RGB - #define NEO_WHITE 255, 255, 255, 0 -#else - #define NEO_WHITE 0, 0, 0, 255 -#endif +typedef IF<(TERN0(NEOPIXEL_LED, NEOPIXEL_PIXELS > 127)), int16_t, int8_t>::type pixel_index_t; // ------------------------ -// Function prototypes +// Classes // ------------------------ class Marlin_NeoPixel { private: - static Adafruit_NeoPixel adaneo1 - #if CONJOINED_NEOPIXEL - , adaneo2 - #endif - ; + static Adafruit_NeoPixel adaneo1; + #if CONJOINED_NEOPIXEL + static Adafruit_NeoPixel adaneo2; + #endif public: - static int8_t neoindex; + static pixel_index_t neoindex; static void init(); static void set_color_startup(const uint32_t c); static void set_color(const uint32_t c); - #ifdef NEOPIXEL_BKGD_LED_INDEX - static void set_color_background(); + #ifdef NEOPIXEL_BKGD_INDEX_FIRST + static void set_background_color(uint8_t r, uint8_t g, uint8_t b, uint8_t w); + static void reset_background_color(); #endif - static inline void begin() { + static void begin() { adaneo1.begin(); TERN_(CONJOINED_NEOPIXEL, adaneo2.begin()); } - static inline void set_pixel_color(const uint16_t n, const uint32_t c) { + static void set_pixel_color(const uint16_t n, const uint32_t c) { #if ENABLED(NEOPIXEL2_INSERIES) if (n >= NEOPIXEL_PIXELS) adaneo2.setPixelColor(n - (NEOPIXEL_PIXELS), c); else adaneo1.setPixelColor(n, c); #else adaneo1.setPixelColor(n, c); - #if MULTIPLE_NEOPIXEL_TYPES - adaneo2.setPixelColor(n, c); - #endif + TERN_(MULTIPLE_NEOPIXEL_TYPES, adaneo2.setPixelColor(n, c)); #endif } - static inline void set_brightness(const uint8_t b) { + static void set_brightness(const uint8_t b) { adaneo1.setBrightness(b); TERN_(CONJOINED_NEOPIXEL, adaneo2.setBrightness(b)); } - static inline void show() { + static void show() { // Some platforms cannot maintain PWM output when NeoPixel disables interrupts for long durations. TERN_(HAS_PAUSE_SERVO_OUTPUT, PAUSE_SERVO_OUTPUT()); adaneo1.show(); @@ -112,7 +120,6 @@ class Marlin_NeoPixel { #if CONJOINED_NEOPIXEL adaneo2.show(); #else - IF_DISABLED(NEOPIXEL2_SEPARATE, adaneo1.setPin(NEOPIXEL2_PIN)); adaneo1.show(); adaneo1.setPin(NEOPIXEL_PIN); #endif @@ -120,15 +127,13 @@ class Marlin_NeoPixel { TERN_(HAS_PAUSE_SERVO_OUTPUT, RESUME_SERVO_OUTPUT()); } - #if 0 - bool set_led_color(const uint8_t r, const uint8_t g, const uint8_t b, const uint8_t w, const uint8_t p); - #endif - // Accessors - static inline uint16_t pixels() { TERN(NEOPIXEL2_INSERIES, return adaneo1.numPixels() * 2, return adaneo1.numPixels()); } - static inline uint8_t brightness() { return adaneo1.getBrightness(); } - static inline uint32_t Color(uint8_t r, uint8_t g, uint8_t b, uint8_t w) { - return adaneo1.Color(r, g, b, w); + static uint16_t pixels() { return adaneo1.numPixels() * TERN1(NEOPIXEL2_INSERIES, 2); } + + static uint8_t brightness() { return adaneo1.getBrightness(); } + + static uint32_t Color(uint8_t r, uint8_t g, uint8_t b OPTARG(HAS_WHITE_LED, uint8_t w)) { + return adaneo1.Color(r, g, b OPTARG(HAS_WHITE_LED, w)); } }; @@ -137,15 +142,12 @@ extern Marlin_NeoPixel neo; // Neo pixel channel 2 #if ENABLED(NEOPIXEL2_SEPARATE) - #if NEOPIXEL2_TYPE == NEO_RGB || NEOPIXEL2_TYPE == NEO_RBG || NEOPIXEL2_TYPE == NEO_GRB || NEOPIXEL2_TYPE == NEO_GBR || NEOPIXEL2_TYPE == NEO_BRG || NEOPIXEL2_TYPE == NEO_BGR + #if _NEO_IS_RGB(NEOPIXEL2_TYPE) #define NEOPIXEL2_IS_RGB 1 + #define NEO2_WHITE 255, 255, 255 #else #define NEOPIXEL2_IS_RGBW 1 - #endif - - #if NEOPIXEL2_IS_RGB - #define NEO2_WHITE 255, 255, 255, 0 - #else + #define HAS_WHITE_LED2 1 // A white component can be passed for NEOPIXEL2 #define NEO2_WHITE 0, 0, 0, 255 #endif @@ -154,29 +156,31 @@ extern Marlin_NeoPixel neo; static Adafruit_NeoPixel adaneo; public: - static int8_t neoindex; + static pixel_index_t neoindex; static void init(); static void set_color_startup(const uint32_t c); static void set_color(const uint32_t c); - static inline void begin() { adaneo.begin(); } - static inline void set_pixel_color(const uint16_t n, const uint32_t c) { adaneo.setPixelColor(n, c); } - static inline void set_brightness(const uint8_t b) { adaneo.setBrightness(b); } - static inline void show() { + static void begin() { adaneo.begin(); } + static void set_pixel_color(const uint16_t n, const uint32_t c) { adaneo.setPixelColor(n, c); } + static void set_brightness(const uint8_t b) { adaneo.setBrightness(b); } + static void show() { adaneo.show(); adaneo.setPin(NEOPIXEL2_PIN); } // Accessors - static inline uint16_t pixels() { return adaneo.numPixels();} - static inline uint8_t brightness() { return adaneo.getBrightness(); } - static inline uint32_t Color(uint8_t r, uint8_t g, uint8_t b, uint8_t w) { - return adaneo.Color(r, g, b, w); + static uint16_t pixels() { return adaneo.numPixels();} + static uint8_t brightness() { return adaneo.getBrightness(); } + static uint32_t Color(uint8_t r, uint8_t g, uint8_t b OPTARG(HAS_WHITE_LED2, uint8_t w)) { + return adaneo.Color(r, g, b OPTARG(HAS_WHITE_LED2, w)); } }; extern Marlin_NeoPixel2 neo2; #endif // NEOPIXEL2_SEPARATE + +#undef _NEO_IS_RGB diff --git a/Marlin/src/feature/leds/pca9533.cpp b/Marlin/src/feature/leds/pca9533.cpp index 0fd4d6675771..914db21ba31c 100644 --- a/Marlin/src/feature/leds/pca9533.cpp +++ b/Marlin/src/feature/leds/pca9533.cpp @@ -36,7 +36,7 @@ void PCA9533_init() { PCA9533_reset(); } -static void PCA9533_writeAllRegisters(uint8_t psc0, uint8_t pwm0, uint8_t psc1, uint8_t pwm1, uint8_t ls0){ +static void PCA9533_writeAllRegisters(uint8_t psc0, uint8_t pwm0, uint8_t psc1, uint8_t pwm1, uint8_t ls0) { uint8_t data[6] = { PCA9533_REG_PSC0 | PCA9533_REGM_AI, psc0, pwm0, psc1, pwm1, ls0 }; Wire.beginTransmission(PCA9533_Addr >> 1); Wire.write(data, 6); @@ -44,7 +44,7 @@ static void PCA9533_writeAllRegisters(uint8_t psc0, uint8_t pwm0, uint8_t psc1, delayMicroseconds(1); } -static void PCA9533_writeRegister(uint8_t reg, uint8_t val){ +static void PCA9533_writeRegister(uint8_t reg, uint8_t val) { uint8_t data[2] = { reg, val }; Wire.beginTransmission(PCA9533_Addr >> 1); Wire.write(data, 2); diff --git a/Marlin/src/feature/leds/pca9632.cpp b/Marlin/src/feature/leds/pca9632.cpp index bb30e0b48b0e..abea98800451 100644 --- a/Marlin/src/feature/leds/pca9632.cpp +++ b/Marlin/src/feature/leds/pca9632.cpp @@ -93,9 +93,7 @@ static void PCA9632_WriteRegister(const byte addr, const byte regadd, const byte } static void PCA9632_WriteAllRegisters(const byte addr, const byte regadd, const byte vr, const byte vg, const byte vb - #if ENABLED(PCA9632_RGBW) - , const byte vw - #endif + OPTARG(PCA9632_RGBW, const byte vw) ) { #if DISABLED(PCA9632_NO_AUTO_INC) uint8_t data[4]; @@ -143,9 +141,7 @@ void PCA9632_set_led_color(const LEDColor &color) { ; PCA9632_WriteAllRegisters(PCA9632_ADDRESS,PCA9632_PWM0, color.r, color.g, color.b - #if ENABLED(PCA9632_RGBW) - , color.w - #endif + OPTARG(PCA9632_RGBW, color.w) ); PCA9632_WriteRegister(PCA9632_ADDRESS,PCA9632_LEDOUT, LEDOUT); } diff --git a/Marlin/src/feature/leds/printer_event_leds.cpp b/Marlin/src/feature/leds/printer_event_leds.cpp index 32c68627045c..e6407a6320a5 100644 --- a/Marlin/src/feature/leds/printer_event_leds.cpp +++ b/Marlin/src/feature/leds/printer_event_leds.cpp @@ -40,17 +40,15 @@ PrinterEventLEDs printerEventLEDs; uint8_t PrinterEventLEDs::old_intensity = 0; - inline uint8_t pel_intensity(const float &start, const float ¤t, const float &target) { - if (uint16_t(start) == uint16_t(target)) return 255; - return (uint8_t)map(constrain(current, start, target), start, target, 0.f, 255.f); + inline uint8_t pel_intensity(const celsius_t start, const celsius_t current, const celsius_t target) { + if (start == target) return 255; + return (uint8_t)map(constrain(current, start, target), start, target, 0, 255); } - inline void pel_set_rgb(const uint8_t r, const uint8_t g, const uint8_t b) { + inline void pel_set_rgb(const uint8_t r, const uint8_t g, const uint8_t b OPTARG(HAS_WHITE_LED, const uint8_t w=0)) { leds.set_color( - MakeLEDColor(r, g, b, 0, neo.brightness()) - #if ENABLED(NEOPIXEL_IS_SEQUENTIAL) - , true - #endif + LEDColor(r, g, b OPTARG(HAS_WHITE_LED, w) OPTARG(NEOPIXEL_LED, neo.brightness())) + OPTARG(NEOPIXEL_IS_SEQUENTIAL, true) ); } @@ -58,7 +56,7 @@ PrinterEventLEDs printerEventLEDs; #if HAS_TEMP_HOTEND - void PrinterEventLEDs::onHotendHeating(const float &start, const float ¤t, const float &target) { + void PrinterEventLEDs::onHotendHeating(const celsius_t start, const celsius_t current, const celsius_t target) { const uint8_t blue = pel_intensity(start, current, target); if (blue != old_intensity) { old_intensity = blue; @@ -70,7 +68,7 @@ PrinterEventLEDs printerEventLEDs; #if HAS_HEATED_BED - void PrinterEventLEDs::onBedHeating(const float &start, const float ¤t, const float &target) { + void PrinterEventLEDs::onBedHeating(const celsius_t start, const celsius_t current, const celsius_t target) { const uint8_t red = pel_intensity(start, current, target); if (red != old_intensity) { old_intensity = red; @@ -82,7 +80,7 @@ PrinterEventLEDs printerEventLEDs; #if HAS_HEATED_CHAMBER - void PrinterEventLEDs::onChamberHeating(const float &start, const float ¤t, const float &target) { + void PrinterEventLEDs::onChamberHeating(const celsius_t start, const celsius_t current, const celsius_t target) { const uint8_t green = pel_intensity(start, current, target); if (green != old_intensity) { old_intensity = green; diff --git a/Marlin/src/feature/leds/printer_event_leds.h b/Marlin/src/feature/leds/printer_event_leds.h index 668c9c969be0..2a4342e8f55c 100644 --- a/Marlin/src/feature/leds/printer_event_leds.h +++ b/Marlin/src/feature/leds/printer_event_leds.h @@ -36,38 +36,32 @@ class PrinterEventLEDs { static bool leds_off_after_print; #endif - static inline void set_done() { - #if ENABLED(LED_COLOR_PRESETS) - leds.set_default(); - #else - leds.set_off(); - #endif - } + static void set_done() { TERN(LED_COLOR_PRESETS, leds.set_default(), leds.set_off()); } public: #if HAS_TEMP_HOTEND - static inline LEDColor onHotendHeatingStart() { old_intensity = 0; return leds.get_color(); } - static void onHotendHeating(const float &start, const float ¤t, const float &target); + static LEDColor onHotendHeatingStart() { old_intensity = 0; return leds.get_color(); } + static void onHotendHeating(const celsius_t start, const celsius_t current, const celsius_t target); #endif #if HAS_HEATED_BED - static inline LEDColor onBedHeatingStart() { old_intensity = 127; return leds.get_color(); } - static void onBedHeating(const float &start, const float ¤t, const float &target); + static LEDColor onBedHeatingStart() { old_intensity = 127; return leds.get_color(); } + static void onBedHeating(const celsius_t start, const celsius_t current, const celsius_t target); #endif #if HAS_HEATED_CHAMBER - static inline LEDColor onChamberHeatingStart() { old_intensity = 127; return leds.get_color(); } - static void onChamberHeating(const float &start, const float ¤t, const float &target); + static LEDColor onChamberHeatingStart() { old_intensity = 127; return leds.get_color(); } + static void onChamberHeating(const celsius_t start, const celsius_t current, const celsius_t target); #endif #if HAS_TEMP_HOTEND || HAS_HEATED_BED || HAS_HEATED_CHAMBER - static inline void onHeatingDone() { leds.set_white(); } - static inline void onPidTuningDone(LEDColor c) { leds.set_color(c); } + static void onHeatingDone() { leds.set_white(); } + static void onPidTuningDone(LEDColor c) { leds.set_color(c); } #endif #if ENABLED(SDSUPPORT) - static inline void onPrintCompleted() { + static void onPrintCompleted() { leds.set_green(); #if HAS_LEDS_OFF_FLAG leds_off_after_print = true; @@ -77,7 +71,7 @@ class PrinterEventLEDs { #endif } - static inline void onResumeAfterWait() { + static void onResumeAfterWait() { #if HAS_LEDS_OFF_FLAG if (leds_off_after_print) { set_done(); diff --git a/Marlin/src/feature/leds/tempstat.cpp b/Marlin/src/feature/leds/tempstat.cpp index 880258f85245..967b9f4d815d 100644 --- a/Marlin/src/feature/leds/tempstat.cpp +++ b/Marlin/src/feature/leds/tempstat.cpp @@ -36,10 +36,10 @@ void handle_status_leds() { static millis_t next_status_led_update_ms = 0; if (ELAPSED(millis(), next_status_led_update_ms)) { next_status_led_update_ms += 500; // Update every 0.5s - float max_temp = TERN0(HAS_HEATED_BED, _MAX(thermalManager.degTargetBed(), thermalManager.degBed())); + celsius_t max_temp = TERN0(HAS_HEATED_BED, _MAX(thermalManager.degTargetBed(), thermalManager.wholeDegBed())); HOTEND_LOOP() - max_temp = _MAX(max_temp, thermalManager.degHotend(e), thermalManager.degTargetHotend(e)); - const int8_t new_red = (max_temp > 55.0) ? HIGH : (max_temp < 54.0 || old_red < 0) ? LOW : old_red; + max_temp = _MAX(max_temp, thermalManager.wholeDegHotend(e), thermalManager.degTargetHotend(e)); + const int8_t new_red = (max_temp > 55) ? HIGH : (max_temp < 54 || old_red < 0) ? LOW : old_red; if (new_red != old_red) { old_red = new_red; #if PIN_EXISTS(STAT_LED_RED) diff --git a/Marlin/src/feature/max7219.cpp b/Marlin/src/feature/max7219.cpp index d7433cb7d93d..474933aa1946 100644 --- a/Marlin/src/feature/max7219.cpp +++ b/Marlin/src/feature/max7219.cpp @@ -124,13 +124,12 @@ uint8_t Max7219::suspended; // = 0; #define SIG_DELAY() DELAY_NS(250) #endif -void Max7219::error(const char * const func, const int32_t v1, const int32_t v2/*=-1*/) { +void Max7219::error(FSTR_P const func, const int32_t v1, const int32_t v2/*=-1*/) { #if ENABLED(MAX7219_ERRORS) SERIAL_ECHOPGM("??? Max7219::"); - SERIAL_ECHOPGM_P(func); - SERIAL_CHAR('('); + SERIAL_ECHOF(func, AS_CHAR('(')); SERIAL_ECHO(v1); - if (v2 > 0) SERIAL_ECHOPAIR(", ", v2); + if (v2 > 0) SERIAL_ECHOPGM(", ", v2); SERIAL_CHAR(')'); SERIAL_EOL(); #else @@ -256,7 +255,7 @@ void Max7219::set(const uint8_t line, const uint8_t bits) { } // Draw a float with a decimal point and optional digits - void Max7219::print(const uint8_t start, const float value, const uint8_t pre_size, const uint8_t post_size, const bool leadzero=false) { + void Max7219::print(const uint8_t start, const_float_t value, const uint8_t pre_size, const uint8_t post_size, const bool leadzero=false) { if (pre_size) print(start, value, pre_size, leadzero, !!post_size); if (post_size) { const int16_t after = ABS(value) * (10 ^ post_size); @@ -268,24 +267,24 @@ void Max7219::set(const uint8_t line, const uint8_t bits) { // Modify a single LED bit and send the changed line void Max7219::led_set(const uint8_t x, const uint8_t y, const bool on) { - if (x >= MAX7219_X_LEDS || y >= MAX7219_Y_LEDS) return error(PSTR("led_set"), x, y); + if (x >= MAX7219_X_LEDS || y >= MAX7219_Y_LEDS) return error(F("led_set"), x, y); if (BIT_7219(x, y) == on) return; XOR_7219(x, y); refresh_unit_line(LED_IND(x, y)); } void Max7219::led_on(const uint8_t x, const uint8_t y) { - if (x >= MAX7219_X_LEDS || y >= MAX7219_Y_LEDS) return error(PSTR("led_on"), x, y); + if (x >= MAX7219_X_LEDS || y >= MAX7219_Y_LEDS) return error(F("led_on"), x, y); led_set(x, y, true); } void Max7219::led_off(const uint8_t x, const uint8_t y) { - if (x >= MAX7219_X_LEDS || y >= MAX7219_Y_LEDS) return error(PSTR("led_off"), x, y); + if (x >= MAX7219_X_LEDS || y >= MAX7219_Y_LEDS) return error(F("led_off"), x, y); led_set(x, y, false); } void Max7219::led_toggle(const uint8_t x, const uint8_t y) { - if (x >= MAX7219_X_LEDS || y >= MAX7219_Y_LEDS) return error(PSTR("led_toggle"), x, y); + if (x >= MAX7219_X_LEDS || y >= MAX7219_Y_LEDS) return error(F("led_toggle"), x, y); led_set(x, y, !BIT_7219(x, y)); } @@ -328,13 +327,13 @@ void Max7219::fill() { } void Max7219::clear_row(const uint8_t row) { - if (row >= MAX7219_Y_LEDS) return error(PSTR("clear_row"), row); + if (row >= MAX7219_Y_LEDS) return error(F("clear_row"), row); LOOP_L_N(x, MAX7219_X_LEDS) CLR_7219(x, row); send_row(row); } void Max7219::clear_column(const uint8_t col) { - if (col >= MAX7219_X_LEDS) return error(PSTR("set_column"), col); + if (col >= MAX7219_X_LEDS) return error(F("set_column"), col); LOOP_L_N(y, MAX7219_Y_LEDS) CLR_7219(col, y); send_column(col); } @@ -345,7 +344,7 @@ void Max7219::clear_column(const uint8_t col) { * once with a single call to the function (if rotated 90° or 270°). */ void Max7219::set_row(const uint8_t row, const uint32_t val) { - if (row >= MAX7219_Y_LEDS) return error(PSTR("set_row"), row); + if (row >= MAX7219_Y_LEDS) return error(F("set_row"), row); uint32_t mask = _BV32(MAX7219_X_LEDS - 1); LOOP_L_N(x, MAX7219_X_LEDS) { if (val & mask) SET_7219(x, row); else CLR_7219(x, row); @@ -360,7 +359,7 @@ void Max7219::set_row(const uint8_t row, const uint32_t val) { * once with a single call to the function (if rotated 0° or 180°). */ void Max7219::set_column(const uint8_t col, const uint32_t val) { - if (col >= MAX7219_X_LEDS) return error(PSTR("set_column"), col); + if (col >= MAX7219_X_LEDS) return error(F("set_column"), col); uint32_t mask = _BV32(MAX7219_Y_LEDS - 1); LOOP_L_N(y, MAX7219_Y_LEDS) { if (val & mask) SET_7219(col, y); else CLR_7219(col, y); @@ -371,56 +370,56 @@ void Max7219::set_column(const uint8_t col, const uint32_t val) { void Max7219::set_rows_16bits(const uint8_t y, uint32_t val) { #if MAX7219_X_LEDS == 8 - if (y > MAX7219_Y_LEDS - 2) return error(PSTR("set_rows_16bits"), y, val); + if (y > MAX7219_Y_LEDS - 2) return error(F("set_rows_16bits"), y, val); set_row(y + 1, val); val >>= 8; set_row(y + 0, val); #else // at least 16 bits on each row - if (y > MAX7219_Y_LEDS - 1) return error(PSTR("set_rows_16bits"), y, val); + if (y > MAX7219_Y_LEDS - 1) return error(F("set_rows_16bits"), y, val); set_row(y, val); #endif } void Max7219::set_rows_32bits(const uint8_t y, uint32_t val) { #if MAX7219_X_LEDS == 8 - if (y > MAX7219_Y_LEDS - 4) return error(PSTR("set_rows_32bits"), y, val); + if (y > MAX7219_Y_LEDS - 4) return error(F("set_rows_32bits"), y, val); set_row(y + 3, val); val >>= 8; set_row(y + 2, val); val >>= 8; set_row(y + 1, val); val >>= 8; set_row(y + 0, val); #elif MAX7219_X_LEDS == 16 - if (y > MAX7219_Y_LEDS - 2) return error(PSTR("set_rows_32bits"), y, val); + if (y > MAX7219_Y_LEDS - 2) return error(F("set_rows_32bits"), y, val); set_row(y + 1, val); val >>= 16; set_row(y + 0, val); #else // at least 24 bits on each row. In the 3 matrix case, just display the low 24 bits - if (y > MAX7219_Y_LEDS - 1) return error(PSTR("set_rows_32bits"), y, val); + if (y > MAX7219_Y_LEDS - 1) return error(F("set_rows_32bits"), y, val); set_row(y, val); #endif } void Max7219::set_columns_16bits(const uint8_t x, uint32_t val) { #if MAX7219_Y_LEDS == 8 - if (x > MAX7219_X_LEDS - 2) return error(PSTR("set_columns_16bits"), x, val); + if (x > MAX7219_X_LEDS - 2) return error(F("set_columns_16bits"), x, val); set_column(x + 0, val); val >>= 8; set_column(x + 1, val); #else // at least 16 bits in each column - if (x > MAX7219_X_LEDS - 1) return error(PSTR("set_columns_16bits"), x, val); + if (x > MAX7219_X_LEDS - 1) return error(F("set_columns_16bits"), x, val); set_column(x, val); #endif } void Max7219::set_columns_32bits(const uint8_t x, uint32_t val) { #if MAX7219_Y_LEDS == 8 - if (x > MAX7219_X_LEDS - 4) return error(PSTR("set_rows_32bits"), x, val); + if (x > MAX7219_X_LEDS - 4) return error(F("set_rows_32bits"), x, val); set_column(x + 3, val); val >>= 8; set_column(x + 2, val); val >>= 8; set_column(x + 1, val); val >>= 8; set_column(x + 0, val); #elif MAX7219_Y_LEDS == 16 - if (x > MAX7219_X_LEDS - 2) return error(PSTR("set_rows_32bits"), x, val); + if (x > MAX7219_X_LEDS - 2) return error(F("set_rows_32bits"), x, val); set_column(x + 1, val); val >>= 16; set_column(x + 0, val); #else // at least 24 bits on each row. In the 3 matrix case, just display the low 24 bits - if (x > MAX7219_X_LEDS - 1) return error(PSTR("set_rows_32bits"), x, val); + if (x > MAX7219_X_LEDS - 1) return error(F("set_rows_32bits"), x, val); set_column(x, val); #endif } diff --git a/Marlin/src/feature/max7219.h b/Marlin/src/feature/max7219.h index 8e98c9456c3f..809bda6d4b35 100644 --- a/Marlin/src/feature/max7219.h +++ b/Marlin/src/feature/max7219.h @@ -42,6 +42,8 @@ * a Max7219_Set_Row(). The opposite is true for rotations of 0 or 180 degrees. */ +#include "../inc/MarlinConfig.h" + #ifndef MAX7219_ROTATE #define MAX7219_ROTATE 0 #endif @@ -86,13 +88,13 @@ class Max7219 { static void send(const uint8_t reg, const uint8_t data); // Refresh all units - static inline void refresh() { for (uint8_t i = 0; i < 8; i++) refresh_line(i); } + static void refresh() { for (uint8_t i = 0; i < 8; i++) refresh_line(i); } // Suspend / resume updates to the LED unit // Use these methods to speed up multiple changes // or to apply updates from interrupt context. - static inline void suspend() { suspended++; } - static inline void resume() { suspended--; suspended |= 0x80; } + static void suspend() { suspended++; } + static void resume() { suspended--; suspended |= 0x80; } // Update a single native line on all units static void refresh_line(const uint8_t line); @@ -100,6 +102,13 @@ class Max7219 { // Update a single native line on just one unit static void refresh_unit_line(const uint8_t line); + #if ENABLED(MAX7219_NUMERIC) + // Draw an integer with optional leading zeros and optional decimal point + void print(const uint8_t start, int16_t value, uint8_t size, const bool leadzero=false, bool dec=false); + // Draw a float with a decimal point and optional digits + void print(const uint8_t start, const_float_t value, const uint8_t pre_size, const uint8_t post_size, const bool leadzero=false); + #endif + // Set a single LED by XY coordinate static void led_set(const uint8_t x, const uint8_t y, const bool on); static void led_on(const uint8_t x, const uint8_t y); @@ -133,7 +142,7 @@ class Max7219 { private: static uint8_t suspended; - static void error(const char * const func, const int32_t v1, const int32_t v2=-1); + static void error(FSTR_P const func, const int32_t v1, const int32_t v2=-1); static void noop(); static void set(const uint8_t line, const uint8_t bits); static void send_row(const uint8_t row); diff --git a/Marlin/src/feature/meatpack.cpp b/Marlin/src/feature/meatpack.cpp index 7e81dbed79df..b2899243b2fb 100644 --- a/Marlin/src/feature/meatpack.cpp +++ b/Marlin/src/feature/meatpack.cpp @@ -39,10 +39,9 @@ #include "../inc/MarlinConfig.h" -#if ENABLED(MEATPACK) +#if HAS_MEATPACK #include "meatpack.h" -MeatPack meatpack; #define MeatPack_ProtocolVersion "PV01" //#define MP_DEBUG @@ -50,14 +49,6 @@ MeatPack meatpack; #define DEBUG_OUT ENABLED(MP_DEBUG) #include "../core/debug_out.h" -bool MeatPack::cmd_is_next = false; // A command is pending -uint8_t MeatPack::state = 0; // Configuration state OFF -uint8_t MeatPack::second_char = 0; // The unpacked 2nd character from an out-of-sequence packed pair -uint8_t MeatPack::cmd_count = 0, // Counts how many command bytes are received (need 2) - MeatPack::full_char_count = 0, // Counts how many full-width characters are to be received - MeatPack::char_out_count = 0; // Stores number of characters to be read out. -uint8_t MeatPack::char_out_buf[2]; // Output buffer for caching up to 2 characters - // The 15 most-common characters used in G-code, ~90-95% of all G-code uses these characters // Stored in SRAM for performance. uint8_t meatPackLookupTable[16] = { @@ -66,7 +57,9 @@ uint8_t meatPackLookupTable[16] = { '\0' // Unused. 0b1111 indicates a literal character }; -TERN_(MP_DEBUG, uint8_t chars_decoded = 0); // Log the first 64 bytes after each reset +#if ENABLED(MP_DEBUG) + uint8_t chars_decoded = 0; // Log the first 64 bytes after each reset +#endif void MeatPack::reset_state() { state = 0; @@ -147,7 +140,7 @@ void MeatPack::handle_output_char(const uint8_t c) { #if ENABLED(MP_DEBUG) if (chars_decoded < 1024) { ++chars_decoded; - DEBUG_ECHOLNPAIR("RB: ", AS_CHAR(c)); + DEBUG_ECHOLNPGM("RB: ", AS_CHAR(c)); } #endif } @@ -176,10 +169,9 @@ void MeatPack::handle_command(const MeatPack_Command c) { void MeatPack::report_state() { // NOTE: if any configuration vars are added below, the outgoing sync text for host plugin // should not contain the "PV' substring, as this is used to indicate protocol version - SERIAL_ECHOPGM("[MP] "); - SERIAL_ECHOPGM(MeatPack_ProtocolVersion " "); + SERIAL_ECHOPGM("[MP] " MeatPack_ProtocolVersion " "); serialprint_onoff(TEST(state, MPConfig_Bit_Active)); - SERIAL_ECHOPGM_P(TEST(state, MPConfig_Bit_NoSpaces) ? PSTR(" NSP\n") : PSTR(" ESP\n")); + SERIAL_ECHOF(TEST(state, MPConfig_Bit_NoSpaces) ? F(" NSP\n") : F(" ESP\n")); } /** @@ -212,7 +204,7 @@ void MeatPack::handle_rx_char(const uint8_t c, const serial_index_t serial_ind) handle_rx_char_inner(c); // Other characters are passed on for MeatPack decoding } -uint8_t MeatPack::get_result_char(char* const __restrict out) { +uint8_t MeatPack::get_result_char(char * const __restrict out) { uint8_t res = 0; if (char_out_count) { res = char_out_count; @@ -223,4 +215,4 @@ uint8_t MeatPack::get_result_char(char* const __restrict out) { return res; } -#endif // MEATPACK +#endif // HAS_MEATPACK diff --git a/Marlin/src/feature/meatpack.h b/Marlin/src/feature/meatpack.h index e30a5ac979c1..a56e65b6cc30 100644 --- a/Marlin/src/feature/meatpack.h +++ b/Marlin/src/feature/meatpack.h @@ -90,18 +90,18 @@ class MeatPack { static const uint8_t kSpaceCharIdx = 11; static const char kSpaceCharReplace = 'E'; - static bool cmd_is_next; // A command is pending - static uint8_t state; // Configuration state - static uint8_t second_char; // Buffers a character if dealing with out-of-sequence pairs - static uint8_t cmd_count, // Counter of command bytes received (need 2) - full_char_count, // Counter for full-width characters to be received - char_out_count; // Stores number of characters to be read out. - static uint8_t char_out_buf[2]; // Output buffer for caching up to 2 characters + bool cmd_is_next; // A command is pending + uint8_t state; // Configuration state + uint8_t second_char; // Buffers a character if dealing with out-of-sequence pairs + uint8_t cmd_count, // Counter of command bytes received (need 2) + full_char_count, // Counter for full-width characters to be received + char_out_count; // Stores number of characters to be read out. + uint8_t char_out_buf[2]; // Output buffer for caching up to 2 characters public: // Pass in a character rx'd by SD card or serial. Automatically parses command/ctrl sequences, // and will control state internally. - static void handle_rx_char(const uint8_t c, const serial_index_t serial_ind); + void handle_rx_char(const uint8_t c, const serial_index_t serial_ind); /** * After passing in rx'd char using above method, call this to get characters out. @@ -109,17 +109,17 @@ class MeatPack { * @param out [in] Output pointer for unpacked/processed data. * @return Number of characters returned. Range from 0 to 2. */ - static uint8_t get_result_char(char* const __restrict out); - - static void reset_state(); - static void report_state(); - static uint8_t unpack_chars(const uint8_t pk, uint8_t* __restrict const chars_out); - static void handle_command(const MeatPack_Command c); - static void handle_output_char(const uint8_t c); - static void handle_rx_char_inner(const uint8_t c); -}; + uint8_t get_result_char(char * const __restrict out); + + void reset_state(); + void report_state(); + uint8_t unpack_chars(const uint8_t pk, uint8_t* __restrict const chars_out); + void handle_command(const MeatPack_Command c); + void handle_output_char(const uint8_t c); + void handle_rx_char_inner(const uint8_t c); -extern MeatPack meatpack; + MeatPack() : cmd_is_next(false), state(0), second_char(0), cmd_count(0), full_char_count(0), char_out_count(0) {} +}; // Implement the MeatPack serial class so it's transparent to rest of the code template @@ -127,26 +127,25 @@ struct MeatpackSerial : public SerialBase > { typedef SerialBase< MeatpackSerial > BaseClassT; SerialT & out; + MeatPack meatpack; char serialBuffer[2]; uint8_t charCount; uint8_t readIndex; - NO_INLINE size_t write(uint8_t c) { return out.write(c); } - void flush() { out.flush(); } - void begin(long br) { out.begin(br); readIndex = 0; } - void end() { out.end(); } + NO_INLINE void write(uint8_t c) { out.write(c); } + void flush() { out.flush(); } + void begin(long br) { out.begin(br); readIndex = 0; } + void end() { out.end(); } - void msgDone() { out.msgDone(); } + void msgDone() { out.msgDone(); } // Existing instances implement Arduino's operator bool, so use that if it's available - bool connected() { return Private::HasMember_connected::value ? CALL_IF_EXISTS(bool, &out, connected) : (bool)out; } - void flushTX() { CALL_IF_EXISTS(void, &out, flushTX); } + bool connected() { return Private::HasMember_connected::value ? CALL_IF_EXISTS(bool, &out, connected) : (bool)out; } + void flushTX() { CALL_IF_EXISTS(void, &out, flushTX); } + SerialFeature features(serial_index_t index) const { return SerialFeature::MeatPack | CALL_IF_EXISTS(SerialFeature, &out, features, index); } - int available(uint8_t index) { - // There is a potential issue here with multiserial, since it'll return its decoded buffer whatever the serial index here. - // So, instead of doing MeatpackSerial> we should do MultiSerial, MeatpackSerial<...>> - // TODO, let's fix this later on + int available(serial_index_t index) { if (charCount) return charCount; // The buffer still has data if (out.available(index) <= 0) return 0; // No data to read @@ -160,7 +159,7 @@ struct MeatpackSerial : public SerialBase > { return charCount; } - int readImpl(const uint8_t index) { + int readImpl(const serial_index_t index) { // Not enough char to make progress? if (charCount == 0 && available(index) == 0) return -1; @@ -168,9 +167,9 @@ struct MeatpackSerial : public SerialBase > { return serialBuffer[readIndex++]; } - int read(uint8_t index) { return readImpl(index); } - int available() { return available(0); } - int read() { return readImpl(0); } + int read(serial_index_t index) { return readImpl(index); } + int available() { return available(0); } + int read() { return readImpl(0); } MeatpackSerial(const bool e, SerialT & out) : BaseClassT(e), out(out) {} }; diff --git a/Marlin/src/feature/mixing.cpp b/Marlin/src/feature/mixing.cpp index 722020ba8ac9..9ebc90127f43 100644 --- a/Marlin/src/feature/mixing.cpp +++ b/Marlin/src/feature/mixing.cpp @@ -106,11 +106,32 @@ void Mixer::reset_vtools() { MIXER_STEPPER_LOOP(i) color[t][i] = (i == 0) ? COLOR_A_MASK : 0; #endif + + // MIXING_PRESETS: Set a variety of obvious mixes as presets + #if ENABLED(MIXING_PRESETS) && WITHIN(MIXING_STEPPERS, 2, 3) + #if MIXING_STEPPERS == 2 + if (MIXING_VIRTUAL_TOOLS > 2) { collector[0] = 1; collector[1] = 1; mixer.normalize(2); } // 1:1 + if (MIXING_VIRTUAL_TOOLS > 3) { collector[0] = 3; mixer.normalize(3); } // 3:1 + if (MIXING_VIRTUAL_TOOLS > 4) { collector[0] = 1; collector[1] = 3; mixer.normalize(4); } // 1:3 + if (MIXING_VIRTUAL_TOOLS > 5) { collector[1] = 2; mixer.normalize(5); } // 1:2 + if (MIXING_VIRTUAL_TOOLS > 6) { collector[0] = 2; collector[1] = 1; mixer.normalize(6); } // 2:1 + if (MIXING_VIRTUAL_TOOLS > 7) { collector[0] = 3; collector[1] = 2; mixer.normalize(7); } // 3:2 + #else + if (MIXING_VIRTUAL_TOOLS > 3) { collector[0] = 1; collector[1] = 1; collector[2] = 1; mixer.normalize(3); } // 1:1:1 + if (MIXING_VIRTUAL_TOOLS > 4) { collector[1] = 3; collector[2] = 0; mixer.normalize(4); } // 1:3:0 + if (MIXING_VIRTUAL_TOOLS > 5) { collector[0] = 0; collector[2] = 1; mixer.normalize(5); } // 0:3:1 + if (MIXING_VIRTUAL_TOOLS > 6) { collector[1] = 1; mixer.normalize(6); } // 0:1:1 + if (MIXING_VIRTUAL_TOOLS > 7) { collector[0] = 1; collector[2] = 0; mixer.normalize(7); } // 1:1:0 + #endif + ZERO(collector); + #endif } // called at boot void Mixer::init() { + ZERO(collector); + reset_vtools(); #if HAS_MIXER_SYNC_CHANNEL @@ -119,8 +140,6 @@ void Mixer::init() { color[MIXER_AUTORETRACT_TOOL][i] = COLOR_A_MASK; #endif - ZERO(collector); - #if EITHER(HAS_DUAL_MIXING, GRADIENT_MIX) update_mix_from_vtool(); #endif @@ -135,11 +154,11 @@ void Mixer::refresh_collector(const float proportion/*=1.0*/, const uint8_t t/*= cmax = _MAX(cmax, v); csum += v; } - //SERIAL_ECHOPAIR("Mixer::refresh_collector(", proportion, ", ", t, ") cmax=", cmax, " csum=", csum, " color"); + //SERIAL_ECHOPGM("Mixer::refresh_collector(", proportion, ", ", t, ") cmax=", cmax, " csum=", csum, " color"); const float inv_prop = proportion / csum; MIXER_STEPPER_LOOP(i) { c[i] = color[t][i] * inv_prop; - //SERIAL_ECHOPAIR(" [", t, "][", i, "] = ", color[t][i], " (", c[i], ") "); + //SERIAL_ECHOPGM(" [", t, "][", i, "] = ", color[t][i], " (", c[i], ") "); } //SERIAL_EOL(); } @@ -150,19 +169,17 @@ void Mixer::refresh_collector(const float proportion/*=1.0*/, const uint8_t t/*= #include "../module/planner.h" gradient_t Mixer::gradient = { - false, // enabled - {0}, // color (array) - 0, 0, // start_z, end_z - 0, 1, // start_vtool, end_vtool - {0}, {0} // start_mix[], end_mix[] - #if ENABLED(GRADIENT_VTOOL) - , -1 // vtool_index - #endif + false, // enabled + {0}, // color (array) + 0, 0, // start_z, end_z + 0, 1, // start_vtool, end_vtool + {0}, {0} // start_mix[], end_mix[] + OPTARG(GRADIENT_VTOOL, -1) // vtool_index }; float Mixer::prev_z; // = 0 - void Mixer::update_gradient_for_z(const float z) { + void Mixer::update_gradient_for_z(const_float_t z) { if (z == prev_z) return; prev_z = z; diff --git a/Marlin/src/feature/mixing.h b/Marlin/src/feature/mixing.h index 65d1f1bf9559..85d52d69c8f3 100644 --- a/Marlin/src/feature/mixing.h +++ b/Marlin/src/feature/mixing.h @@ -61,9 +61,6 @@ enum MixTool { #define MAX_VTOOLS TERN(HAS_MIXER_SYNC_CHANNEL, 254, 255) static_assert(NR_MIXING_VIRTUAL_TOOLS <= MAX_VTOOLS, "MIXING_VIRTUAL_TOOLS must be <= " STRINGIFY(MAX_VTOOLS) "!"); -#define MIXER_BLOCK_FIELD mixer_comp_t b_color[MIXING_STEPPERS] -#define MIXER_POPULATE_BLOCK() mixer.populate_block(block->b_color) -#define MIXER_STEPPER_SETUP() mixer.stepper_setup(current_block->b_color) #define MIXER_STEPPER_LOOP(VAR) for (uint_fast8_t VAR = 0; VAR < MIXING_STEPPERS; VAR++) #if ENABLED(GRADIENT_MIX) @@ -73,9 +70,11 @@ static_assert(NR_MIXING_VIRTUAL_TOOLS <= MAX_VTOOLS, "MIXING_VIRTUAL_TOOLS must mixer_comp_t color[MIXING_STEPPERS]; // The current gradient color float start_z, end_z; // Region for gradient int8_t start_vtool, end_vtool; // Start and end virtual tools - mixer_perc_t start_mix[MIXING_STEPPERS], // Start and end mixes from those tools + mixer_perc_t start_mix[MIXING_STEPPERS], // Start and end mixes from those tools end_mix[MIXING_STEPPERS]; - TERN_(GRADIENT_VTOOL, int8_t vtool_index); // Use this virtual tool number as index + #if ENABLED(GRADIENT_VTOOL) + int8_t vtool_index; // Use this virtual tool number as index + #endif } gradient_t; #endif @@ -127,7 +126,7 @@ class Mixer { static mixer_perc_t mix[MIXING_STEPPERS]; // Scratch array for the Mix in proportion to 100 - static inline void copy_mix_to_color(mixer_comp_t (&tcolor)[MIXING_STEPPERS]) { + static void copy_mix_to_color(mixer_comp_t (&tcolor)[MIXING_STEPPERS]) { // Scale each component to the largest one in terms of COLOR_A_MASK // So the largest component will be COLOR_A_MASK and the other will be in proportion to it const float scale = (COLOR_A_MASK) * RECIPROCAL(_MAX( @@ -146,14 +145,14 @@ class Mixer { #endif } - static inline void update_mix_from_vtool(const uint8_t j=selected_vtool) { + static void update_mix_from_vtool(const uint8_t j=selected_vtool) { float ctot = 0; MIXER_STEPPER_LOOP(i) ctot += color[j][i]; //MIXER_STEPPER_LOOP(i) mix[i] = 100.0f * color[j][i] / ctot; MIXER_STEPPER_LOOP(i) mix[i] = mixer_perc_t(100.0f * color[j][i] / ctot); #ifdef MIXER_NORMALIZER_DEBUG - SERIAL_ECHOPAIR("V-tool ", j, " [ "); + SERIAL_ECHOPGM("V-tool ", j, " [ "); SERIAL_ECHOLIST_N(MIXING_STEPPERS, color[j][0], color[j][1], color[j][2], color[j][3], color[j][4], color[j][5]); SERIAL_ECHOPGM(" ] to Mix [ "); SERIAL_ECHOLIST_N(MIXING_STEPPERS, mix[0], mix[1], mix[2], mix[3], mix[4], mix[5]); @@ -166,7 +165,7 @@ class Mixer { #if HAS_DUAL_MIXING // Update the virtual tool from an edited mix - static inline void update_vtool_from_mix() { + static void update_vtool_from_mix() { copy_mix_to_color(color[selected_vtool]); TERN_(GRADIENT_MIX, refresh_gradient()); // MIXER_STEPPER_LOOP(i) collector[i] = mix[i]; @@ -181,9 +180,9 @@ class Mixer { static float prev_z; // Update the current mix from the gradient for a given Z - static void update_gradient_for_z(const float z); + static void update_gradient_for_z(const_float_t z); static void update_gradient_for_planner_z(); - static inline void gradient_control(const float z) { + static void gradient_control(const_float_t z) { if (gradient.enabled) { if (z >= gradient.end_z) T(gradient.end_vtool); @@ -192,7 +191,7 @@ class Mixer { } } - static inline void update_mix_from_gradient() { + static void update_mix_from_gradient() { float ctot = 0; MIXER_STEPPER_LOOP(i) ctot += gradient.color[i]; MIXER_STEPPER_LOOP(i) mix[i] = (mixer_perc_t)CEIL(100.0f * gradient.color[i] / ctot); diff --git a/Marlin/src/feature/mmu/mmu.cpp b/Marlin/src/feature/mmu/mmu.cpp index 9a448296bbe2..58c49ed22454 100644 --- a/Marlin/src/feature/mmu/mmu.cpp +++ b/Marlin/src/feature/mmu/mmu.cpp @@ -24,11 +24,19 @@ #if HAS_PRUSA_MMU1 -#include "../module/stepper.h" +#include "../../MarlinCore.h" +#include "../../module/planner.h" +#include "../../module/stepper.h" + +void mmu_init() { + SET_OUTPUT(E_MUX0_PIN); + SET_OUTPUT(E_MUX1_PIN); + SET_OUTPUT(E_MUX2_PIN); +} void select_multiplexed_stepper(const uint8_t e) { planner.synchronize(); - disable_e_steppers(); + stepper.disable_e_steppers(); WRITE(E_MUX0_PIN, TEST(e, 0) ? HIGH : LOW); WRITE(E_MUX1_PIN, TEST(e, 1) ? HIGH : LOW); WRITE(E_MUX2_PIN, TEST(e, 2) ? HIGH : LOW); diff --git a/Marlin/src/feature/mmu/mmu.h b/Marlin/src/feature/mmu/mmu.h index 10805c8e26b7..23742d00c61e 100644 --- a/Marlin/src/feature/mmu/mmu.h +++ b/Marlin/src/feature/mmu/mmu.h @@ -21,4 +21,5 @@ */ #pragma once +void mmu_init(); void select_multiplexed_stepper(const uint8_t e); diff --git a/Marlin/src/feature/mmu/mmu2.cpp b/Marlin/src/feature/mmu/mmu2.cpp index a1bec36e4578..2813337c635b 100644 --- a/Marlin/src/feature/mmu/mmu2.cpp +++ b/Marlin/src/feature/mmu/mmu2.cpp @@ -35,7 +35,7 @@ MMU2 mmu2; #include "../../libs/nozzle.h" #include "../../module/temperature.h" #include "../../module/planner.h" -#include "../../module/stepper/indirection.h" +#include "../../module/stepper.h" #include "../../MarlinCore.h" #if ENABLED(HOST_PROMPT_SUPPORT) @@ -54,7 +54,7 @@ MMU2 mmu2; #define MMU_CMD_TIMEOUT 45000UL // 45s timeout for mmu commands (except P0) #define MMU_P0_TIMEOUT 3000UL // Timeout for P0 command: 3seconds -#define MMU2_COMMAND(S) tx_str_P(PSTR(S "\n")) +#define MMU2_COMMAND(S) tx_str(F(S "\n")) #if ENABLED(MMU_EXTRUDER_SENSOR) uint8_t mmu_idl_sens = 0; @@ -75,7 +75,7 @@ MMU2 mmu2; #define MMU2_NO_TOOL 99 #define MMU_BAUD 115200 -bool MMU2::enabled, MMU2::ready, MMU2::mmu_print_saved; +bool MMU2::_enabled, MMU2::ready, MMU2::mmu_print_saved; #if HAS_PRUSA_MMU2S bool MMU2::mmu2s_triggered; #endif @@ -159,7 +159,7 @@ void MMU2::mmu_loop() { MMU2_COMMAND("S1"); // Read Version state = -2; } - else if (millis() > 3000000) { + else if (millis() > 30000) { // 30sec after reset disable MMU SERIAL_ECHOLNPGM("MMU not responding - DISABLED"); state = 0; } @@ -169,7 +169,7 @@ void MMU2::mmu_loop() { if (rx_ok()) { sscanf(rx_buffer, "%huok\n", &version); - DEBUG_ECHOLNPAIR("MMU => ", version, "\nMMU <= 'S2'"); + DEBUG_ECHOLNPGM("MMU => ", version, "\nMMU <= 'S2'"); MMU2_COMMAND("S2"); // Read Build Number state = -3; @@ -180,7 +180,7 @@ void MMU2::mmu_loop() { if (rx_ok()) { sscanf(rx_buffer, "%huok\n", &buildnr); - DEBUG_ECHOLNPAIR("MMU => ", buildnr); + DEBUG_ECHOLNPGM("MMU => ", buildnr); check_version(); @@ -217,9 +217,9 @@ void MMU2::mmu_loop() { if (rx_ok()) { sscanf(rx_buffer, "%hhuok\n", &finda); - DEBUG_ECHOLNPAIR("MMU => ", finda, "\nMMU - ENABLED"); + DEBUG_ECHOLNPGM("MMU => ", finda, "\nMMU - ENABLED"); - enabled = true; + _enabled = true; state = 1; TERN_(HAS_PRUSA_MMU2S, mmu2s_triggered = false); } @@ -229,17 +229,17 @@ void MMU2::mmu_loop() { if (cmd) { if (WITHIN(cmd, MMU_CMD_T0, MMU_CMD_T0 + EXTRUDERS - 1)) { // tool change - int filament = cmd - MMU_CMD_T0; - DEBUG_ECHOLNPAIR("MMU <= T", filament); - tx_printf_P(PSTR("T%d\n"), filament); + const int filament = cmd - MMU_CMD_T0; + DEBUG_ECHOLNPGM("MMU <= T", filament); + tx_printf(F("T%d\n"), filament); TERN_(MMU_EXTRUDER_SENSOR, mmu_idl_sens = 1); // enable idler sensor, if any state = 3; // wait for response } else if (WITHIN(cmd, MMU_CMD_L0, MMU_CMD_L0 + EXTRUDERS - 1)) { // load - int filament = cmd - MMU_CMD_L0; - DEBUG_ECHOLNPAIR("MMU <= L", filament); - tx_printf_P(PSTR("L%d\n"), filament); + const int filament = cmd - MMU_CMD_L0; + DEBUG_ECHOLNPGM("MMU <= L", filament); + tx_printf(F("L%d\n"), filament); state = 3; // wait for response } else if (cmd == MMU_CMD_C0) { @@ -257,9 +257,9 @@ void MMU2::mmu_loop() { } else if (WITHIN(cmd, MMU_CMD_E0, MMU_CMD_E0 + EXTRUDERS - 1)) { // eject filament - int filament = cmd - MMU_CMD_E0; - DEBUG_ECHOLNPAIR("MMU <= E", filament); - tx_printf_P(PSTR("E%d\n"), filament); + const int filament = cmd - MMU_CMD_E0; + DEBUG_ECHOLNPGM("MMU <= E", filament); + tx_printf(F("E%d\n"), filament); state = 3; // wait for response } else if (cmd == MMU_CMD_R0) { @@ -270,9 +270,9 @@ void MMU2::mmu_loop() { } else if (WITHIN(cmd, MMU_CMD_F0, MMU_CMD_F0 + EXTRUDERS - 1)) { // filament type - int filament = cmd - MMU_CMD_F0; - DEBUG_ECHOLNPAIR("MMU <= F", filament, " ", cmd_arg); - tx_printf_P(PSTR("F%d %d\n"), filament, cmd_arg); + const int filament = cmd - MMU_CMD_F0; + DEBUG_ECHOLNPGM("MMU <= F", filament, " ", cmd_arg); + tx_printf(F("F%d %d\n"), filament, cmd_arg); state = 3; // wait for response } @@ -356,13 +356,15 @@ void MMU2::mmu_loop() { */ bool MMU2::rx_start() { // check for start message - return rx_str_P(PSTR("start\n")); + return rx_str(F("start\n")); } /** * Check if the data received ends with the given string. */ -bool MMU2::rx_str_P(const char* str) { +bool MMU2::rx_str(FSTR_P fstr) { + PGM_P pstr = FTOP(fstr); + uint8_t i = strlen(rx_buffer); while (MMU2_SERIAL.available()) { @@ -375,14 +377,14 @@ bool MMU2::rx_str_P(const char* str) { } rx_buffer[i] = '\0'; - uint8_t len = strlen_P(str); + uint8_t len = strlen_P(pstr); if (i < len) return false; - str += len; + pstr += len; while (len--) { - char c0 = pgm_read_byte(str--), c1 = rx_buffer[i--]; + char c0 = pgm_read_byte(pstr--), c1 = rx_buffer[i--]; if (c0 == c1) continue; if (c0 == '\r' && c1 == '\n') continue; // match cr as lf if (c0 == '\n' && c1 == '\r') continue; // match lf as cr @@ -394,19 +396,19 @@ bool MMU2::rx_str_P(const char* str) { /** * Transfer data to MMU, no argument */ -void MMU2::tx_str_P(const char* str) { +void MMU2::tx_str(FSTR_P fstr) { clear_rx_buffer(); - uint8_t len = strlen_P(str); - LOOP_L_N(i, len) MMU2_SERIAL.write(pgm_read_byte(str++)); + PGM_P pstr = FTOP(fstr); + while (const char c = pgm_read_byte(pstr)) { MMU2_SERIAL.write(c); pstr++; } prev_request = millis(); } /** * Transfer data to MMU, single argument */ -void MMU2::tx_printf_P(const char* format, int argument = -1) { +void MMU2::tx_printf(FSTR_P format, int argument = -1) { clear_rx_buffer(); - uint8_t len = sprintf_P(tx_buffer, format, argument); + const uint8_t len = sprintf_P(tx_buffer, FTOP(format), argument); LOOP_L_N(i, len) MMU2_SERIAL.write(tx_buffer[i]); prev_request = millis(); } @@ -414,9 +416,9 @@ void MMU2::tx_printf_P(const char* format, int argument = -1) { /** * Transfer data to MMU, two arguments */ -void MMU2::tx_printf_P(const char* format, int argument1, int argument2) { +void MMU2::tx_printf(FSTR_P format, int argument1, int argument2) { clear_rx_buffer(); - uint8_t len = sprintf_P(tx_buffer, format, argument1, argument2); + const uint8_t len = sprintf_P(tx_buffer, FTOP(format), argument1, argument2); LOOP_L_N(i, len) MMU2_SERIAL.write(tx_buffer[i]); prev_request = millis(); } @@ -433,7 +435,7 @@ void MMU2::clear_rx_buffer() { * Check if we received 'ok' from MMU */ bool MMU2::rx_ok() { - if (rx_str_P(PSTR("ok\n"))) { + if (rx_str(F("ok\n"))) { prev_P0_request = millis(); return true; } @@ -446,12 +448,12 @@ bool MMU2::rx_ok() { void MMU2::check_version() { if (buildnr < MMU_REQUIRED_FW_BUILDNR) { SERIAL_ERROR_MSG("Invalid MMU2 firmware. Version >= " STRINGIFY(MMU_REQUIRED_FW_BUILDNR) " required."); - kill(GET_TEXT(MSG_KILL_MMU2_FIRMWARE)); + kill(GET_TEXT_F(MSG_KILL_MMU2_FIRMWARE)); } } static void mmu2_not_responding() { - LCD_MESSAGEPGM(MSG_MMU2_NOT_RESPONDING); + LCD_MESSAGE(MSG_MMU2_NOT_RESPONDING); BUZZ(100, 659); BUZZ(200, 698); BUZZ(100, 659); @@ -480,14 +482,14 @@ static void mmu2_not_responding() { */ void MMU2::tool_change(const uint8_t index) { - if (!enabled) return; + if (!_enabled) return; set_runout_valid(false); if (index != extruder) { - DISABLE_AXIS_E0(); - ui.status_printf_P(0, GET_TEXT(MSG_MMU2_LOADING_FILAMENT), int(index + 1)); + stepper.disable_extruder(); + ui.status_printf(0, GET_TEXT_F(MSG_MMU2_LOADING_FILAMENT), int(index + 1)); command(MMU_CMD_T0 + index); manage_response(true, true); @@ -495,7 +497,7 @@ static void mmu2_not_responding() { if (load_to_gears()) { extruder = index; // filament change is finished active_extruder = 0; - ENABLE_AXIS_E0(); + stepper.enable_extruder(); SERIAL_ECHO_MSG(STR_ACTIVE_EXTRUDER, extruder); } ui.reset_status(); @@ -511,8 +513,8 @@ static void mmu2_not_responding() { * Tx Same as T?, except nozzle doesn't have to be preheated. Tc must be placed after extruder nozzle is preheated to finish filament load. * Tc Load to nozzle after filament was prepared by Tx and extruder nozzle is already heated. */ - void MMU2::tool_change(const char* special) { - if (!enabled) return; + void MMU2::tool_change(const char *special) { + if (!_enabled) return; set_runout_valid(false); @@ -531,13 +533,13 @@ static void mmu2_not_responding() { #if ENABLED(MMU2_MENUS) planner.synchronize(); const uint8_t index = mmu2_choose_filament(); - DISABLE_AXIS_E0(); + stepper.disable_extruder(); command(MMU_CMD_T0 + index); manage_response(true, true); if (load_to_gears()) { mmu_loop(); - ENABLE_AXIS_E0(); + stepper.enable_extruder(); extruder = index; active_extruder = 0; } @@ -561,19 +563,19 @@ static void mmu2_not_responding() { * Handle tool change */ void MMU2::tool_change(const uint8_t index) { - if (!enabled) return; + if (!_enabled) return; set_runout_valid(false); if (index != extruder) { - DISABLE_AXIS_E0(); + stepper.disable_extruder(); if (FILAMENT_PRESENT()) { DEBUG_ECHOLNPGM("Unloading\n"); mmu_loading_flag = false; command(MMU_CMD_U0); manage_response(true, true); } - ui.status_printf_P(0, GET_TEXT(MSG_MMU2_LOADING_FILAMENT), int(index + 1)); + ui.status_printf(0, GET_TEXT_F(MSG_MMU2_LOADING_FILAMENT), int(index + 1)); mmu_loading_flag = true; command(MMU_CMD_T0 + index); manage_response(true, true); @@ -582,7 +584,7 @@ static void mmu2_not_responding() { extruder = index; active_extruder = 0; - ENABLE_AXIS_E0(); + stepper.enable_extruder(); SERIAL_ECHO_MSG(STR_ACTIVE_EXTRUDER, extruder); ui.reset_status(); @@ -598,8 +600,8 @@ static void mmu2_not_responding() { * Tx Same as T?, except nozzle doesn't have to be preheated. Tc must be placed after extruder nozzle is preheated to finish filament load. * Tc Load to nozzle after filament was prepared by Tx and extruder nozzle is already heated. */ - void MMU2::tool_change(const char* special) { - if (!enabled) return; + void MMU2::tool_change(const char *special) { + if (!_enabled) return; set_runout_valid(false); @@ -620,14 +622,14 @@ static void mmu2_not_responding() { #if ENABLED(MMU2_MENUS) planner.synchronize(); uint8_t index = mmu2_choose_filament(); - DISABLE_AXIS_E0(); + stepper.disable_extruder(); command(MMU_CMD_T0 + index); manage_response(true, true); mmu_continue_loading(); command(MMU_CMD_C0); mmu_loop(); - ENABLE_AXIS_E0(); + stepper.enable_extruder(); extruder = index; active_extruder = 0; #else @@ -647,7 +649,7 @@ static void mmu2_not_responding() { void MMU2::mmu_continue_loading() { for (uint8_t i = 0; i < MMU_LOADING_ATTEMPTS_NR; i++) { - DEBUG_ECHOLNPAIR("Additional load attempt #", i); + DEBUG_ECHOLNPGM("Additional load attempt #", i); if (FILAMENT_PRESENT()) break; command(MMU_CMD_C0); manage_response(true, true); @@ -665,19 +667,19 @@ static void mmu2_not_responding() { * Handle tool change */ void MMU2::tool_change(const uint8_t index) { - if (!enabled) return; + if (!_enabled) return; set_runout_valid(false); if (index != extruder) { - DISABLE_AXIS_E0(); - ui.status_printf_P(0, GET_TEXT(MSG_MMU2_LOADING_FILAMENT), int(index + 1)); + stepper.disable_extruder(); + ui.status_printf(0, GET_TEXT_F(MSG_MMU2_LOADING_FILAMENT), int(index + 1)); command(MMU_CMD_T0 + index); manage_response(true, true); command(MMU_CMD_C0); extruder = index; //filament change is finished active_extruder = 0; - ENABLE_AXIS_E0(); + stepper.enable_extruder(); SERIAL_ECHO_MSG(STR_ACTIVE_EXTRUDER, extruder); ui.reset_status(); } @@ -692,8 +694,8 @@ static void mmu2_not_responding() { * Tx Same as T?, except nozzle doesn't have to be preheated. Tc must be placed after extruder nozzle is preheated to finish filament load. * Tc Load to nozzle after filament was prepared by Tx and extruder nozzle is already heated. */ - void MMU2::tool_change(const char* special) { - if (!enabled) return; + void MMU2::tool_change(const char *special) { + if (!_enabled) return; set_runout_valid(false); @@ -714,13 +716,13 @@ static void mmu2_not_responding() { #if ENABLED(MMU2_MENUS) planner.synchronize(); uint8_t index = mmu2_choose_filament(); - DISABLE_AXIS_E0(); + stepper.disable_extruder(); command(MMU_CMD_T0 + index); manage_response(true, true); command(MMU_CMD_C0); mmu_loop(); - ENABLE_AXIS_E0(); + stepper.enable_extruder(); extruder = index; active_extruder = 0; #else @@ -744,7 +746,7 @@ static void mmu2_not_responding() { * Set next command */ void MMU2::command(const uint8_t mmu_cmd) { - if (!enabled) return; + if (!_enabled) return; cmd = mmu_cmd; ready = false; } @@ -775,7 +777,7 @@ void MMU2::manage_response(const bool move_axes, const bool turn_off_nozzle) { bool response = false; mmu_print_saved = false; xyz_pos_t resume_position; - int16_t resume_hotend_temp = thermalManager.degTargetHotend(active_extruder); + celsius_t resume_hotend_temp = thermalManager.degTargetHotend(active_extruder); KEEPALIVE_STATE(PAUSED_FOR_USER); @@ -808,14 +810,14 @@ void MMU2::manage_response(const bool move_axes, const bool turn_off_nozzle) { if (turn_off_nozzle && resume_hotend_temp) { thermalManager.setTargetHotend(resume_hotend_temp, active_extruder); - LCD_MESSAGEPGM(MSG_HEATING); + LCD_MESSAGE(MSG_HEATING); BUZZ(200, 40); while (!thermalManager.wait_for_hotend(active_extruder, false)) safe_delay(1000); } if (move_axes && all_axes_homed()) { - LCD_MESSAGEPGM(MSG_MMU2_RESUMING); + LCD_MESSAGE(MSG_MMU2_RESUMING); BUZZ(198, 404); BUZZ(4, 0); BUZZ(198, 404); // Move XY to starting position, then Z @@ -826,14 +828,14 @@ void MMU2::manage_response(const bool move_axes, const bool turn_off_nozzle) { } else { BUZZ(198, 404); BUZZ(4, 0); BUZZ(198, 404); - LCD_MESSAGEPGM(MSG_MMU2_RESUMING); + LCD_MESSAGE(MSG_MMU2_RESUMING); } } } } void MMU2::set_filament_type(const uint8_t index, const uint8_t filamentType) { - if (!enabled) return; + if (!_enabled) return; cmd_arg = filamentType; command(MMU_CMD_F0 + index); @@ -842,7 +844,7 @@ void MMU2::set_filament_type(const uint8_t index, const uint8_t filamentType) { } void MMU2::filament_runout() { - queue.inject_P(PSTR(MMU2_FILAMENT_RUNOUT_SCRIPT)); + queue.inject(F(MMU2_FILAMENT_RUNOUT_SCRIPT)); planner.synchronize(); } @@ -853,7 +855,7 @@ void MMU2::filament_runout() { if (cmd == MMU_CMD_NONE && last_cmd == MMU_CMD_C0) { if (present && !mmu2s_triggered) { DEBUG_ECHOLNPGM("MMU <= 'A'"); - tx_str_P(PSTR("A\n")); + tx_str(F("A\n")); } // Slowly spin the extruder during C0 else { @@ -892,7 +894,7 @@ void MMU2::filament_runout() { // Load filament into MMU2 void MMU2::load_filament(const uint8_t index) { - if (!enabled) return; + if (!_enabled) return; command(MMU_CMD_L0 + index); manage_response(false, false); @@ -904,15 +906,15 @@ void MMU2::load_filament(const uint8_t index) { */ bool MMU2::load_filament_to_nozzle(const uint8_t index) { - if (!enabled) return false; + if (!_enabled) return false; if (thermalManager.tooColdToExtrude(active_extruder)) { BUZZ(200, 404); - LCD_ALERTMESSAGEPGM(MSG_HOTEND_TOO_COLD); + LCD_ALERTMESSAGE(MSG_HOTEND_TOO_COLD); return false; } - DISABLE_AXIS_E0(); + stepper.disable_extruder(); command(MMU_CMD_T0 + index); manage_response(true, true); @@ -940,17 +942,17 @@ void MMU2::load_to_nozzle() { bool MMU2::eject_filament(const uint8_t index, const bool recover) { - if (!enabled) return false; + if (!_enabled) return false; if (thermalManager.tooColdToExtrude(active_extruder)) { BUZZ(200, 404); - LCD_ALERTMESSAGEPGM(MSG_HOTEND_TOO_COLD); + LCD_ALERTMESSAGE(MSG_HOTEND_TOO_COLD); return false; } - LCD_MESSAGEPGM(MSG_MMU2_EJECTING_FILAMENT); + LCD_MESSAGE(MSG_MMU2_EJECTING_FILAMENT); - ENABLE_AXIS_E0(); + stepper.enable_extruder(); current_position.e -= MMU2_FILAMENTCHANGE_EJECT_FEED; line_to_current_position(MMM_TO_MMS(2500)); planner.synchronize(); @@ -958,11 +960,11 @@ bool MMU2::eject_filament(const uint8_t index, const bool recover) { manage_response(false, false); if (recover) { - LCD_MESSAGEPGM(MSG_MMU2_EJECT_RECOVER); + LCD_MESSAGE(MSG_MMU2_EJECT_RECOVER); BUZZ(200, 404); - TERN_(HOST_PROMPT_SUPPORT, host_prompt_do(PROMPT_USER_CONTINUE, PSTR("MMU2 Eject Recover"), CONTINUE_STR)); - TERN_(EXTENSIBLE_UI, ExtUI::onUserConfirmRequired_P(PSTR("MMU2 Eject Recover"))); - wait_for_user_response(); + TERN_(HOST_PROMPT_SUPPORT, hostui.prompt_do(PROMPT_USER_CONTINUE, F("MMU2 Eject Recover"), FPSTR(CONTINUE_STR))); + TERN_(EXTENSIBLE_UI, ExtUI::onUserConfirmRequired(F("MMU2 Eject Recover"))); + TERN_(HAS_RESUME_CONTINUE, wait_for_user_response()); BUZZ(200, 404); BUZZ(200, 404); @@ -979,7 +981,7 @@ bool MMU2::eject_filament(const uint8_t index, const bool recover) { BUZZ(200, 404); - DISABLE_AXIS_E0(); + stepper.disable_extruder(); return true; } @@ -989,11 +991,11 @@ bool MMU2::eject_filament(const uint8_t index, const bool recover) { */ bool MMU2::unload() { - if (!enabled) return false; + if (!_enabled) return false; if (thermalManager.tooColdToExtrude(active_extruder)) { BUZZ(200, 404); - LCD_ALERTMESSAGEPGM(MSG_HOTEND_TOO_COLD); + LCD_ALERTMESSAGE(MSG_HOTEND_TOO_COLD); return false; } @@ -1016,7 +1018,7 @@ bool MMU2::unload() { void MMU2::execute_extruder_sequence(const E_Step * sequence, int steps) { planner.synchronize(); - ENABLE_AXIS_E0(); + stepper.enable_extruder(); const E_Step* step = sequence; @@ -1025,7 +1027,7 @@ void MMU2::execute_extruder_sequence(const E_Step * sequence, int steps) { const feedRate_t fr_mm_m = pgm_read_float(&(step->feedRate)); DEBUG_ECHO_START(); - DEBUG_ECHOLNPAIR("E step ", es, "/", fr_mm_m); + DEBUG_ECHOLNPGM("E step ", es, "/", fr_mm_m); current_position.e += es; line_to_current_position(MMM_TO_MMS(fr_mm_m)); @@ -1034,7 +1036,7 @@ void MMU2::execute_extruder_sequence(const E_Step * sequence, int steps) { step++; } - DISABLE_AXIS_E0(); + stepper.disable_extruder(); } #endif // HAS_PRUSA_MMU2 diff --git a/Marlin/src/feature/mmu/mmu2.h b/Marlin/src/feature/mmu/mmu2.h index 4326989a74b0..7d3d9ec4df38 100644 --- a/Marlin/src/feature/mmu/mmu2.h +++ b/Marlin/src/feature/mmu/mmu2.h @@ -43,9 +43,10 @@ class MMU2 { static void init(); static void reset(); + static bool enabled() { return _enabled; } static void mmu_loop(); static void tool_change(const uint8_t index); - static void tool_change(const char* special); + static void tool_change(const char *special); static uint8_t get_current_tool(); static void set_filament_type(const uint8_t index, const uint8_t type); @@ -56,10 +57,10 @@ class MMU2 { static bool eject_filament(const uint8_t index, const bool recover); private: - static bool rx_str_P(const char* str); - static void tx_str_P(const char* str); - static void tx_printf_P(const char* format, const int argument); - static void tx_printf_P(const char* format, const int argument1, const int argument2); + static bool rx_str(FSTR_P fstr); + static void tx_str(FSTR_P fstr); + static void tx_printf(FSTR_P ffmt, const int argument); + static void tx_printf(FSTR_P ffmt, const int argument1, const int argument2); static void clear_rx_buffer(); static bool rx_ok(); @@ -88,7 +89,7 @@ class MMU2 { static void mmu_continue_loading(); #endif - static bool enabled, ready, mmu_print_saved; + static bool _enabled, ready, mmu_print_saved; static uint8_t cmd, cmd_arg, last_cmd, extruder; static int8_t state; @@ -98,7 +99,7 @@ class MMU2 { static millis_t prev_request, prev_P0_request; static char rx_buffer[MMU_RX_SIZE], tx_buffer[MMU_TX_SIZE]; - static inline void set_runout_valid(const bool valid) { + static void set_runout_valid(const bool valid) { finda_runout_valid = valid; #if HAS_FILAMENT_SENSOR if (valid) runout.reset(); diff --git a/Marlin/src/feature/password/password.cpp b/Marlin/src/feature/password/password.cpp index 90bb647118b5..1d376cc586cd 100644 --- a/Marlin/src/feature/password/password.cpp +++ b/Marlin/src/feature/password/password.cpp @@ -31,7 +31,7 @@ Password password; // public: -bool Password::is_set, Password::is_locked; +bool Password::is_set, Password::is_locked, Password::did_first_run; // = false uint32_t Password::value, Password::value_entry; // @@ -40,19 +40,22 @@ uint32_t Password::value, Password::value_entry; // void Password::lock_machine() { is_locked = true; - TERN_(HAS_LCD_MENU, authenticate_user(ui.status_screen, screen_password_entry)); + TERN_(HAS_MARLINUI_MENU, authenticate_user(ui.status_screen, screen_password_entry)); } // // Authentication check // void Password::authentication_check() { - if (value_entry == value) + if (value_entry == value) { is_locked = false; - else + did_first_run = true; + } + else { + is_locked = true; SERIAL_ECHOLNPGM(STR_WRONG_PASSWORD); - - TERN_(HAS_LCD_MENU, authentication_done()); + } + TERN_(HAS_MARLINUI_MENU, authentication_done()); } #endif // PASSWORD_FEATURE diff --git a/Marlin/src/feature/password/password.h b/Marlin/src/feature/password/password.h index 1382d6df405f..208765b21225 100644 --- a/Marlin/src/feature/password/password.h +++ b/Marlin/src/feature/password/password.h @@ -25,15 +25,15 @@ class Password { public: - static bool is_set, is_locked; + static bool is_set, is_locked, did_first_run; static uint32_t value, value_entry; - Password() { is_locked = false; } + Password() {} static void lock_machine(); static void authentication_check(); - #if HAS_LCD_MENU + #if HAS_MARLINUI_MENU static void access_menu_password(); static void authentication_done(); static void media_gatekeeper(); diff --git a/Marlin/src/feature/pause.cpp b/Marlin/src/feature/pause.cpp index a6a28925edaf..69c25cbe3319 100644 --- a/Marlin/src/feature/pause.cpp +++ b/Marlin/src/feature/pause.cpp @@ -39,6 +39,10 @@ #include "../module/printcounter.h" #include "../module/temperature.h" +#if ENABLED(AUTO_BED_LEVELING_UBL) + #include "bedlevel/bedlevel.h" +#endif + #if ENABLED(FWRETRACT) #include "fwretract.h" #endif @@ -53,6 +57,8 @@ #if ENABLED(EXTENSIBLE_UI) #include "../lcd/extui/ui_api.h" +#elif ENABLED(DWIN_CREALITY_LCD_ENHANCED) + #include "../lcd/e3v2/proui/dwin.h" #endif #include "../lcd/marlinui.h" @@ -75,7 +81,7 @@ static xyze_pos_t resume_position; -#if HAS_LCD_MENU +#if M600_PURGE_MORE_RESUMABLE PauseMenuResponse pause_menu_response; PauseMode pause_mode = PAUSE_MODE_PAUSE_PRINT; #endif @@ -95,7 +101,7 @@ fil_change_settings_t fc_settings[EXTRUDERS]; #if HAS_BUZZER static void impatient_beep(const int8_t max_beep_count, const bool restart=false) { - if (TERN0(HAS_LCD_MENU, pause_mode == PAUSE_MODE_PAUSE_PRINT)) return; + if (TERN0(HAS_MARLINUI_MENU, pause_mode == PAUSE_MODE_PAUSE_PRINT)) return; static millis_t next_buzz = 0; static int8_t runout_beep = 0; @@ -130,7 +136,7 @@ fil_change_settings_t fc_settings[EXTRUDERS]; */ static bool ensure_safe_temperature(const bool wait=true, const PauseMode mode=PAUSE_MODE_SAME) { DEBUG_SECTION(est, "ensure_safe_temperature", true); - DEBUG_ECHOLNPAIR("... wait:", wait, " mode:", mode); + DEBUG_ECHOLNPGM("... wait:", wait, " mode:", mode); #if ENABLED(PREVENT_COLD_EXTRUSION) if (!DEBUGGING(DRYRUN) && thermalManager.targetTooColdToExtrude(active_extruder)) @@ -143,7 +149,7 @@ static bool ensure_safe_temperature(const bool wait=true, const PauseMode mode=P // Allow interruption by Emergency Parser M108 wait_for_heatup = TERN1(PREVENT_COLD_EXTRUSION, !thermalManager.allow_cold_extrude); - while (wait_for_heatup && ABS(thermalManager.degHotend(active_extruder) - thermalManager.degTargetHotend(active_extruder)) > TEMP_WINDOW) + while (wait_for_heatup && ABS(thermalManager.wholeDegHotend(active_extruder) - thermalManager.degTargetHotend(active_extruder)) > (TEMP_WINDOW)) idle(); wait_for_heatup = false; @@ -170,13 +176,13 @@ static bool ensure_safe_temperature(const bool wait=true, const PauseMode mode=P * * Returns 'true' if load was completed, 'false' for abort */ -bool load_filament(const float &slow_load_length/*=0*/, const float &fast_load_length/*=0*/, const float &purge_length/*=0*/, const int8_t max_beep_count/*=0*/, +bool load_filament(const_float_t slow_load_length/*=0*/, const_float_t fast_load_length/*=0*/, const_float_t purge_length/*=0*/, const int8_t max_beep_count/*=0*/, const bool show_lcd/*=false*/, const bool pause_for_user/*=false*/, const PauseMode mode/*=PAUSE_MODE_PAUSE_PRINT*/ DXC_ARGS ) { DEBUG_SECTION(lf, "load_filament", true); - DEBUG_ECHOLNPAIR("... slowlen:", slow_load_length, " fastlen:", fast_load_length, " purgelen:", purge_length, " maxbeep:", max_beep_count, " showlcd:", show_lcd, " pauseforuser:", pause_for_user, " pausemode:", mode DXC_SAY); + DEBUG_ECHOLNPGM("... slowlen:", slow_load_length, " fastlen:", fast_load_length, " purgelen:", purge_length, " maxbeep:", max_beep_count, " showlcd:", show_lcd, " pauseforuser:", pause_for_user, " pausemode:", mode DXC_SAY); if (!ensure_safe_temperature(false, mode)) { if (show_lcd) ui.pause_show_message(PAUSE_MESSAGE_STATUS, mode); @@ -191,15 +197,26 @@ bool load_filament(const float &slow_load_length/*=0*/, const float &fast_load_l KEEPALIVE_STATE(PAUSED_FOR_USER); wait_for_user = true; // LCD click or M108 will clear this + + TERN_(EXTENSIBLE_UI, ExtUI::onUserConfirmRequired(F("Load Filament"))); + #if ENABLED(HOST_PROMPT_SUPPORT) const char tool = '0' + TERN0(MULTI_FILAMENT_SENSOR, active_extruder); - host_prompt_do(PROMPT_USER_CONTINUE, PSTR("Load Filament T"), tool, CONTINUE_STR); + hostui.prompt_do(PROMPT_USER_CONTINUE, F("Load Filament T"), tool, FPSTR(CONTINUE_STR)); #endif - TERN_(EXTENSIBLE_UI, ExtUI::onUserConfirmRequired_P(PSTR("Load Filament"))); - while (wait_for_user) { impatient_beep(max_beep_count); + #if BOTH(FILAMENT_CHANGE_RESUME_ON_INSERT, FILAMENT_RUNOUT_SENSOR) + #if ENABLED(MULTI_FILAMENT_SENSOR) + #define _CASE_INSERTED(N) case N-1: if (READ(FIL_RUNOUT##N##_PIN) != FIL_RUNOUT##N##_STATE) wait_for_user = false; break; + switch (active_extruder) { + REPEAT_1(NUM_RUNOUT_SENSORS, _CASE_INSERTED) + } + #else + if (READ(FIL_RUNOUT_PIN) != FIL_RUNOUT_STATE) wait_for_user = false; + #endif + #endif idle_no_sleep(); } } @@ -212,6 +229,8 @@ bool load_filament(const float &slow_load_length/*=0*/, const float &fast_load_l set_duplication_enabled(false, DXC_ext); #endif + TERN_(BELTPRINTER, do_blocking_move_to_xy(0.00, 50.00)); + // Slow Load filament if (slow_load_length) unscaled_e_move(slow_load_length, FILAMENT_CHANGE_SLOW_LOAD_FEEDRATE); @@ -237,8 +256,8 @@ bool load_filament(const float &slow_load_length/*=0*/, const float &fast_load_l if (show_lcd) ui.pause_show_message(PAUSE_MESSAGE_PURGE); - TERN_(HOST_PROMPT_SUPPORT, host_prompt_do(PROMPT_USER_CONTINUE, PSTR("Filament Purging..."), CONTINUE_STR)); - TERN_(EXTENSIBLE_UI, ExtUI::onUserConfirmRequired_P(PSTR("Filament Purging..."))); + TERN_(EXTENSIBLE_UI, ExtUI::onUserConfirmRequired(GET_TEXT_F(MSG_FILAMENT_CHANGE_PURGE))); + TERN_(HOST_PROMPT_SUPPORT, hostui.prompt_do(PROMPT_USER_CONTINUE, GET_TEXT_F(MSG_FILAMENT_CHANGE_PURGE), FPSTR(CONTINUE_STR))); wait_for_user = true; // A click or M108 breaks the purge_length loop for (float purge_count = purge_length; purge_count > 0 && wait_for_user; --purge_count) unscaled_e_move(1, ADVANCED_PAUSE_PURGE_FEEDRATE); @@ -255,23 +274,27 @@ bool load_filament(const float &slow_load_length/*=0*/, const float &fast_load_l unscaled_e_move(purge_length, ADVANCED_PAUSE_PURGE_FEEDRATE); } - TERN_(HOST_PROMPT_SUPPORT, filament_load_host_prompt()); // Initiate another host prompt. + TERN_(HOST_PROMPT_SUPPORT, hostui.filament_load_prompt()); // Initiate another host prompt. - #if HAS_LCD_MENU + #if M600_PURGE_MORE_RESUMABLE if (show_lcd) { // Show "Purge More" / "Resume" menu and wait for reply KEEPALIVE_STATE(PAUSED_FOR_USER); wait_for_user = false; - ui.pause_show_message(PAUSE_MESSAGE_OPTION); + #if EITHER(HAS_MARLINUI_MENU, DWIN_CREALITY_LCD_ENHANCED) + ui.pause_show_message(PAUSE_MESSAGE_OPTION); // Also sets PAUSE_RESPONSE_WAIT_FOR + #else + pause_menu_response = PAUSE_RESPONSE_WAIT_FOR; + #endif while (pause_menu_response == PAUSE_RESPONSE_WAIT_FOR) idle_no_sleep(); } #endif // Keep looping if "Purge More" was selected - } while (TERN0(HAS_LCD_MENU, show_lcd && pause_menu_response == PAUSE_RESPONSE_EXTRUDE_MORE)); + } while (TERN0(M600_PURGE_MORE_RESUMABLE, pause_menu_response == PAUSE_RESPONSE_EXTRUDE_MORE)); #endif - TERN_(HOST_PROMPT_SUPPORT, host_action_prompt_end()); + TERN_(HOST_PROMPT_SUPPORT, hostui.prompt_end()); return true; } @@ -282,8 +305,8 @@ bool load_filament(const float &slow_load_length/*=0*/, const float &fast_load_l * send current back to their board, potentially frying it. */ inline void disable_active_extruder() { - #if HAS_E_STEPPER_ENABLE - disable_e_stepper(active_extruder); + #if HAS_EXTRUDERS + stepper.DISABLE_EXTRUDER(active_extruder); safe_delay(100); #endif } @@ -298,21 +321,21 @@ inline void disable_active_extruder() { * * Returns 'true' if unload was completed, 'false' for abort */ -bool unload_filament(const float &unload_length, const bool show_lcd/*=false*/, +bool unload_filament(const_float_t unload_length, const bool show_lcd/*=false*/, const PauseMode mode/*=PAUSE_MODE_PAUSE_PRINT*/ #if BOTH(FILAMENT_UNLOAD_ALL_EXTRUDERS, MIXING_EXTRUDER) - , const float &mix_multiplier/*=1.0*/ + , const_float_t mix_multiplier/*=1.0*/ #endif ) { DEBUG_SECTION(uf, "unload_filament", true); - DEBUG_ECHOLNPAIR("... unloadlen:", unload_length, " showlcd:", show_lcd, " mode:", mode + DEBUG_ECHOLNPGM("... unloadlen:", unload_length, " showlcd:", show_lcd, " mode:", mode #if BOTH(FILAMENT_UNLOAD_ALL_EXTRUDERS, MIXING_EXTRUDER) , " mixmult:", mix_multiplier #endif ); #if !BOTH(FILAMENT_UNLOAD_ALL_EXTRUDERS, MIXING_EXTRUDER) - constexpr float mix_multiplier = 1.0; + constexpr float mix_multiplier = 1.0f; #endif if (!ensure_safe_temperature(false, mode)) { @@ -367,9 +390,9 @@ bool unload_filament(const float &unload_length, const bool show_lcd/*=false*/, */ uint8_t did_pause_print = 0; -bool pause_print(const float &retract, const xyz_pos_t &park_point, const float &unload_length/*=0*/, const bool show_lcd/*=false*/ DXC_ARGS) { +bool pause_print(const_float_t retract, const xyz_pos_t &park_point, const bool show_lcd/*=false*/, const_float_t unload_length/*=0*/ DXC_ARGS) { DEBUG_SECTION(pp, "pause_print", true); - DEBUG_ECHOLNPAIR("... park.x:", park_point.x, " y:", park_point.y, " z:", park_point.z, " unloadlen:", unload_length, " showlcd:", show_lcd DXC_SAY); + DEBUG_ECHOLNPGM("... park.x:", park_point.x, " y:", park_point.y, " z:", park_point.z, " unloadlen:", unload_length, " showlcd:", show_lcd DXC_SAY); UNUSED(show_lcd); @@ -377,20 +400,21 @@ bool pause_print(const float &retract, const xyz_pos_t &park_point, const float #if ENABLED(HOST_ACTION_COMMANDS) #ifdef ACTION_ON_PAUSED - host_action_paused(); + hostui.paused(); #elif defined(ACTION_ON_PAUSE) - host_action_pause(); + hostui.pause(); #endif #endif - TERN_(HOST_PROMPT_SUPPORT, host_prompt_open(PROMPT_INFO, PSTR("Pause"), DISMISS_STR)); + TERN_(HOST_PROMPT_SUPPORT, hostui.prompt_open(PROMPT_INFO, F("Pause"), FPSTR(DISMISS_STR))); // Indicate that the printer is paused ++did_pause_print; // Pause the print job and timer #if ENABLED(SDSUPPORT) - if (IS_SD_PRINTING()) { + const bool was_sd_printing = IS_SD_PRINTING(); + if (was_sd_printing) { card.pauseSDPrint(); ++did_pause_print; // Indicate SD pause also } @@ -401,6 +425,15 @@ bool pause_print(const float &retract, const xyz_pos_t &park_point, const float // Save current position resume_position = current_position; + // Will the nozzle be parking? + const bool do_park = !axes_should_home(); + + #if ENABLED(POWER_LOSS_RECOVERY) + // Save PLR info in case the power goes out while parked + const float park_raise = do_park ? nozzle.park_mode_0_height(park_point.z) - current_position.z : POWER_LOSS_ZRAISE; + if (was_sd_printing && recovery.enabled) recovery.save(true, park_raise, do_park); + #endif + // Wait for buffered blocks to complete planner.synchronize(); @@ -410,13 +443,20 @@ bool pause_print(const float &retract, const xyz_pos_t &park_point, const float // Initial retract before move to filament change position if (retract && thermalManager.hotEnoughToExtrude(active_extruder)) { - DEBUG_ECHOLNPAIR("... retract:", retract); + DEBUG_ECHOLNPGM("... retract:", retract); + + #if ENABLED(AUTO_BED_LEVELING_UBL) + const bool leveling_was_enabled = planner.leveling_active; // save leveling state + set_bed_leveling_enabled(false); // turn off leveling + #endif + unscaled_e_move(retract, PAUSE_PARK_RETRACT_FEEDRATE); + + TERN_(AUTO_BED_LEVELING_UBL, set_bed_leveling_enabled(leveling_was_enabled)); // restore leveling } - // Park the nozzle by moving up by z_lift and then moving to (x_pos, y_pos) - if (!axes_should_home()) - nozzle.park(0, park_point); + // If axes don't need to home then the nozzle can park + if (do_park) nozzle.park(0, park_point); // Park the nozzle by doing a Minimum Z Raise followed by an XY Move #if ENABLED(DUAL_X_CARRIAGE) const int8_t saved_ext = active_extruder; @@ -424,7 +464,8 @@ bool pause_print(const float &retract, const xyz_pos_t &park_point, const float set_duplication_enabled(false, DXC_ext); #endif - if (unload_length) // Unload the filament + // Unload the filament, if specified + if (unload_length) unload_filament(unload_length, show_lcd, PAUSE_MODE_CHANGE_FILAMENT); #if ENABLED(DUAL_X_CARRIAGE) @@ -452,16 +493,16 @@ bool pause_print(const float &retract, const xyz_pos_t &park_point, const float void show_continue_prompt(const bool is_reload) { DEBUG_SECTION(scp, "pause_print", true); - DEBUG_ECHOLNPAIR("... is_reload:", is_reload); + DEBUG_ECHOLNPGM("... is_reload:", is_reload); ui.pause_show_message(is_reload ? PAUSE_MESSAGE_INSERT : PAUSE_MESSAGE_WAITING); SERIAL_ECHO_START(); - SERIAL_ECHOPGM_P(is_reload ? PSTR(_PMSG(STR_FILAMENT_CHANGE_INSERT) "\n") : PSTR(_PMSG(STR_FILAMENT_CHANGE_WAIT) "\n")); + SERIAL_ECHOF(is_reload ? F(_PMSG(STR_FILAMENT_CHANGE_INSERT) "\n") : F(_PMSG(STR_FILAMENT_CHANGE_WAIT) "\n")); } void wait_for_confirmation(const bool is_reload/*=false*/, const int8_t max_beep_count/*=0*/ DXC_ARGS) { DEBUG_SECTION(wfc, "wait_for_confirmation", true); - DEBUG_ECHOLNPAIR("... is_reload:", is_reload, " maxbeep:", max_beep_count DXC_SAY); + DEBUG_ECHOLNPGM("... is_reload:", is_reload, " maxbeep:", max_beep_count DXC_SAY); bool nozzle_timed_out = false; @@ -482,8 +523,8 @@ void wait_for_confirmation(const bool is_reload/*=false*/, const int8_t max_beep // Wait for filament insert by user and press button KEEPALIVE_STATE(PAUSED_FOR_USER); - TERN_(HOST_PROMPT_SUPPORT, host_prompt_do(PROMPT_USER_CONTINUE, GET_TEXT(MSG_NOZZLE_PARKED), CONTINUE_STR)); - TERN_(EXTENSIBLE_UI, ExtUI::onUserConfirmRequired_P(GET_TEXT(MSG_NOZZLE_PARKED))); + TERN_(HOST_PROMPT_SUPPORT, hostui.prompt_do(PROMPT_USER_CONTINUE, GET_TEXT_F(MSG_NOZZLE_PARKED), FPSTR(CONTINUE_STR))); + TERN_(EXTENSIBLE_UI, ExtUI::onUserConfirmRequired(GET_TEXT_F(MSG_NOZZLE_PARKED))); wait_for_user = true; // LCD click or M108 will clear this while (wait_for_user) { impatient_beep(max_beep_count); @@ -498,15 +539,17 @@ void wait_for_confirmation(const bool is_reload/*=false*/, const int8_t max_beep ui.pause_show_message(PAUSE_MESSAGE_HEAT); SERIAL_ECHO_MSG(_PMSG(STR_FILAMENT_CHANGE_HEAT)); - TERN_(HOST_PROMPT_SUPPORT, host_prompt_do(PROMPT_USER_CONTINUE, GET_TEXT(MSG_HEATER_TIMEOUT), GET_TEXT(MSG_REHEAT))); + TERN_(HOST_PROMPT_SUPPORT, hostui.prompt_do(PROMPT_USER_CONTINUE, GET_TEXT_F(MSG_HEATER_TIMEOUT), GET_TEXT_F(MSG_REHEAT))); - TERN_(EXTENSIBLE_UI, ExtUI::onUserConfirmRequired_P(GET_TEXT(MSG_HEATER_TIMEOUT))); + TERN_(EXTENSIBLE_UI, ExtUI::onUserConfirmRequired(GET_TEXT_F(MSG_HEATER_TIMEOUT))); - wait_for_user_response(0, true); // Wait for LCD click or M108 + TERN_(HAS_RESUME_CONTINUE, wait_for_user_response(0, true)); // Wait for LCD click or M108 - TERN_(HOST_PROMPT_SUPPORT, host_prompt_do(PROMPT_INFO, GET_TEXT(MSG_REHEATING))); + TERN_(HOST_PROMPT_SUPPORT, hostui.prompt_do(PROMPT_INFO, GET_TEXT_F(MSG_REHEATING))); - TERN_(EXTENSIBLE_UI, ExtUI::onStatusChanged_P(GET_TEXT(MSG_REHEATING))); + TERN_(EXTENSIBLE_UI, ExtUI::onStatusChanged(GET_TEXT_F(MSG_REHEATING))); + + TERN_(DWIN_CREALITY_LCD_ENHANCED, LCD_MESSAGE(MSG_REHEATING)); // Re-enable the heaters if they timed out HOTEND_LOOP() thermalManager.reset_hotend_idle_timer(e); @@ -521,11 +564,14 @@ void wait_for_confirmation(const bool is_reload/*=false*/, const int8_t max_beep const millis_t nozzle_timeout = SEC_TO_MS(PAUSE_PARK_NOZZLE_TIMEOUT); HOTEND_LOOP() thermalManager.heater_idle[e].start(nozzle_timeout); - TERN_(HOST_PROMPT_SUPPORT, host_prompt_do(PROMPT_USER_CONTINUE, PSTR("Reheat Done"), CONTINUE_STR)); - TERN_(EXTENSIBLE_UI, ExtUI::onUserConfirmRequired_P(PSTR("Reheat finished."))); - wait_for_user = true; - nozzle_timed_out = false; + TERN_(HOST_PROMPT_SUPPORT, hostui.prompt_do(PROMPT_USER_CONTINUE, GET_TEXT_F(MSG_REHEATDONE), FPSTR(CONTINUE_STR))); + TERN_(EXTENSIBLE_UI, ExtUI::onUserConfirmRequired(GET_TEXT_F(MSG_REHEATDONE))); + TERN_(DWIN_CREALITY_LCD_ENHANCED, LCD_MESSAGE(MSG_REHEATDONE)); + + IF_DISABLED(PAUSE_REHEAT_FAST_RESUME, wait_for_user = true); + + nozzle_timed_out = false; first_impatient_beep(max_beep_count); } idle_no_sleep(); @@ -555,12 +601,12 @@ void wait_for_confirmation(const bool is_reload/*=false*/, const int8_t max_beep * - Send host action for resume, if configured * - Resume the current SD print job, if any */ -void resume_print(const float &slow_load_length/*=0*/, const float &fast_load_length/*=0*/, const float &purge_length/*=ADVANCED_PAUSE_PURGE_LENGTH*/, const int8_t max_beep_count/*=0*/, int16_t targetTemp/*=0*/ DXC_ARGS) { +void resume_print(const_float_t slow_load_length/*=0*/, const_float_t fast_load_length/*=0*/, const_float_t purge_length/*=ADVANCED_PAUSE_PURGE_LENGTH*/, const int8_t max_beep_count/*=0*/, const celsius_t targetTemp/*=0*/ DXC_ARGS) { DEBUG_SECTION(rp, "resume_print", true); - DEBUG_ECHOLNPAIR("... slowlen:", slow_load_length, " fastlen:", fast_load_length, " purgelen:", purge_length, " maxbeep:", max_beep_count, " targetTemp:", targetTemp DXC_SAY); + DEBUG_ECHOLNPGM("... slowlen:", slow_load_length, " fastlen:", fast_load_length, " purgelen:", purge_length, " maxbeep:", max_beep_count, " targetTemp:", targetTemp DXC_SAY); /* - SERIAL_ECHOLNPAIR( + SERIAL_ECHOLNPGM( "start of resume_print()\ndual_x_carriage_mode:", dual_x_carriage_mode, "\nextruder_duplication_enabled:", extruder_duplication_enabled, "\nactive_extruder:", active_extruder, @@ -577,9 +623,8 @@ void resume_print(const float &slow_load_length/*=0*/, const float &fast_load_le thermalManager.reset_hotend_idle_timer(e); } - if (targetTemp > thermalManager.degTargetHotend(active_extruder)) { + if (targetTemp > thermalManager.degTargetHotend(active_extruder)) thermalManager.setTargetHotend(targetTemp, active_extruder); - } // Load the new filament load_filament(slow_load_length, fast_load_length, purge_length, max_beep_count, true, nozzle_timed_out, PAUSE_MODE_SAME DXC_PASS); @@ -592,22 +637,31 @@ void resume_print(const float &slow_load_length/*=0*/, const float &fast_load_le ui.pause_show_message(PAUSE_MESSAGE_RESUME); // Check Temperature before moving hotend - ensure_safe_temperature(); + ensure_safe_temperature(DISABLED(BELTPRINTER)); // Retract to prevent oozing unscaled_e_move(-(PAUSE_PARK_RETRACT_LENGTH), feedRate_t(PAUSE_PARK_RETRACT_FEEDRATE)); if (!axes_should_home()) { - // Move XY to starting position, then Z - do_blocking_move_to_xy(resume_position, feedRate_t(NOZZLE_PARK_XY_FEEDRATE)); + // Move XY back to saved position + destination.set(resume_position.x, resume_position.y, current_position.z, current_position.e); + prepare_internal_move_to_destination(NOZZLE_PARK_XY_FEEDRATE); - // Move Z_AXIS to saved position - do_blocking_move_to_z(resume_position.z, feedRate_t(NOZZLE_PARK_Z_FEEDRATE)); + // Move Z back to saved position + destination.z = resume_position.z; + prepare_internal_move_to_destination(NOZZLE_PARK_Z_FEEDRATE); } + #if ENABLED(AUTO_BED_LEVELING_UBL) + const bool leveling_was_enabled = planner.leveling_active; // save leveling state + set_bed_leveling_enabled(false); // turn off leveling + #endif + // Unretract unscaled_e_move(PAUSE_PARK_RETRACT_LENGTH, feedRate_t(PAUSE_PARK_RETRACT_FEEDRATE)); + TERN_(AUTO_BED_LEVELING_UBL, set_bed_leveling_enabled(leveling_was_enabled)); // restore leveling + // Intelligent resuming #if ENABLED(FWRETRACT) // If retracted before goto pause @@ -625,23 +679,28 @@ void resume_print(const float &slow_load_length/*=0*/, const float &fast_load_le // Set extruder to saved position planner.set_e_position_mm((destination.e = current_position.e = resume_position.e)); - // Write PLR now to update the z axis value - TERN_(POWER_LOSS_RECOVERY, if (recovery.enabled) recovery.save(true)); - ui.pause_show_message(PAUSE_MESSAGE_STATUS); #ifdef ACTION_ON_RESUMED - host_action_resumed(); + hostui.resumed(); #elif defined(ACTION_ON_RESUME) - host_action_resume(); + hostui.resume(); #endif --did_pause_print; - TERN_(HOST_PROMPT_SUPPORT, host_prompt_open(PROMPT_INFO, PSTR("Resuming"), DISMISS_STR)); + TERN_(HOST_PROMPT_SUPPORT, hostui.prompt_open(PROMPT_INFO, F("Resuming"), FPSTR(DISMISS_STR))); + + // Resume the print job timer if it was running + if (print_job_timer.isPaused()) print_job_timer.start(); #if ENABLED(SDSUPPORT) - if (did_pause_print) { card.startFileprint(); --did_pause_print; } + if (did_pause_print) { + --did_pause_print; + card.startOrResumeFilePrinting(); + // Write PLR now to update the z axis value + TERN_(POWER_LOSS_RECOVERY, if (recovery.enabled) recovery.save(true)); + } #endif #if ENABLED(ADVANCED_PAUSE_FANS_PAUSE) && HAS_FAN @@ -650,11 +709,9 @@ void resume_print(const float &slow_load_length/*=0*/, const float &fast_load_le TERN_(HAS_FILAMENT_SENSOR, runout.reset()); - // Resume the print job timer if it was running - if (print_job_timer.isPaused()) print_job_timer.start(); - - TERN_(HAS_DISPLAY, ui.reset_status()); - TERN_(HAS_LCD_MENU, ui.return_to_status()); + TERN_(HAS_STATUS_MESSAGE, ui.reset_status()); + TERN_(HAS_MARLINUI_MENU, ui.return_to_status()); + TERN_(DWIN_CREALITY_LCD_ENHANCED, HMI_ReturnScreen()); } #endif // ADVANCED_PAUSE_FEATURE diff --git a/Marlin/src/feature/pause.h b/Marlin/src/feature/pause.h index 7e58d4564ebb..134b1d1b3294 100644 --- a/Marlin/src/feature/pause.h +++ b/Marlin/src/feature/pause.h @@ -59,7 +59,7 @@ enum PauseMessage : char { PAUSE_MESSAGE_HEATING }; -#if HAS_LCD_MENU +#if M600_PURGE_MORE_RESUMABLE enum PauseMenuResponse : char { PAUSE_RESPONSE_WAIT_FOR, PAUSE_RESPONSE_EXTRUDE_MORE, @@ -73,31 +73,52 @@ extern fil_change_settings_t fc_settings[EXTRUDERS]; extern uint8_t did_pause_print; -#if ENABLED(DUAL_X_CARRIAGE) - #define DXC_PARAMS , const int8_t DXC_ext=-1 - #define DXC_ARGS , const int8_t DXC_ext - #define DXC_PASS , DXC_ext - #define DXC_SAY , " dxc:", int(DXC_ext) -#else - #define DXC_PARAMS - #define DXC_ARGS - #define DXC_PASS - #define DXC_SAY -#endif - -bool pause_print(const float &retract, const xyz_pos_t &park_point, const float &unload_length=0, const bool show_lcd=false DXC_PARAMS); +#define DXC_PARAMS OPTARG(DUAL_X_CARRIAGE, const int8_t DXC_ext=-1) +#define DXC_ARGS OPTARG(DUAL_X_CARRIAGE, const int8_t DXC_ext) +#define DXC_PASS OPTARG(DUAL_X_CARRIAGE, DXC_ext) +#define DXC_SAY OPTARG(DUAL_X_CARRIAGE, " dxc:", int(DXC_ext)) + +// Pause the print. If unload_length is set, do a Filament Unload +bool pause_print( + const_float_t retract, // (mm) Retraction length + const xyz_pos_t &park_point, // Parking XY Position and Z Raise + const bool show_lcd=false, // Set LCD status messages? + const_float_t unload_length=0 // (mm) Filament Change Unload Length - 0 to skip + DXC_PARAMS // Dual-X-Carriage extruder index +); -void wait_for_confirmation(const bool is_reload=false, const int8_t max_beep_count=0 DXC_PARAMS); +void wait_for_confirmation( + const bool is_reload=false, // Reload Filament? (otherwise Resume Print) + const int8_t max_beep_count=0 // Beep alert for attention + DXC_PARAMS // Dual-X-Carriage extruder index +); -void resume_print(const float &slow_load_length=0, const float &fast_load_length=0, const float &extrude_length=ADVANCED_PAUSE_PURGE_LENGTH, - const int8_t max_beep_count=0, int16_t targetTemp=0 DXC_PARAMS); +void resume_print( + const_float_t slow_load_length=0, // (mm) Slow Load Length for finishing move + const_float_t fast_load_length=0, // (mm) Fast Load Length for initial move + const_float_t extrude_length=ADVANCED_PAUSE_PURGE_LENGTH, // (mm) Purge length + const int8_t max_beep_count=0, // Beep alert for attention + const celsius_t targetTemp=0 // (°C) A target temperature for the hotend + DXC_PARAMS // Dual-X-Carriage extruder index +); -bool load_filament(const float &slow_load_length=0, const float &fast_load_length=0, const float &extrude_length=0, const int8_t max_beep_count=0, - const bool show_lcd=false, const bool pause_for_user=false, const PauseMode mode=PAUSE_MODE_PAUSE_PRINT DXC_PARAMS); +bool load_filament( + const_float_t slow_load_length=0, // (mm) Slow Load Length for finishing move + const_float_t fast_load_length=0, // (mm) Fast Load Length for initial move + const_float_t extrude_length=0, // (mm) Purge length + const int8_t max_beep_count=0, // Beep alert for attention + const bool show_lcd=false, // Set LCD status messages? + const bool pause_for_user=false, // Pause for user before returning? + const PauseMode mode=PAUSE_MODE_PAUSE_PRINT // Pause Mode to apply + DXC_PARAMS // Dual-X-Carriage extruder index +); -bool unload_filament(const float &unload_length, const bool show_lcd=false, const PauseMode mode=PAUSE_MODE_PAUSE_PRINT +bool unload_filament( + const_float_t unload_length, // (mm) Filament Unload Length - 0 to skip + const bool show_lcd=false, // Set LCD status messages? + const PauseMode mode=PAUSE_MODE_PAUSE_PRINT // Pause Mode to apply #if BOTH(FILAMENT_UNLOAD_ALL_EXTRUDERS, MIXING_EXTRUDER) - , const float &mix_multiplier=1.0 + , const_float_t mix_multiplier=1.0f // Extrusion multiplier (for a Mixing Extruder) #endif ); diff --git a/Marlin/src/feature/power.cpp b/Marlin/src/feature/power.cpp index d22247b46df4..c2ed169aa809 100644 --- a/Marlin/src/feature/power.cpp +++ b/Marlin/src/feature/power.cpp @@ -24,114 +24,228 @@ * power.cpp - power control */ -#include "../inc/MarlinConfig.h" +#include "../inc/MarlinConfigPre.h" -#if ENABLED(AUTO_POWER_CONTROL) +#if EITHER(PSU_CONTROL, AUTO_POWER_CONTROL) #include "power.h" +#include "../module/planner.h" +#include "../module/stepper.h" #include "../module/temperature.h" -#include "../module/stepper/indirection.h" #include "../MarlinCore.h" +#if ENABLED(PS_OFF_SOUND) + #include "../libs/buzzer.h" +#endif + #if defined(PSU_POWERUP_GCODE) || defined(PSU_POWEROFF_GCODE) #include "../gcode/gcode.h" #endif -#if BOTH(USE_CONTROLLER_FAN, AUTO_POWER_CONTROLLERFAN) - #include "controllerfan.h" +Power powerManager; +bool Power::psu_on; + +#if ENABLED(AUTO_POWER_CONTROL) + #include "../module/temperature.h" + + #if BOTH(USE_CONTROLLER_FAN, AUTO_POWER_CONTROLLERFAN) + #include "controllerfan.h" + #endif + + millis_t Power::lastPowerOn; #endif -Power powerManager; +/** + * Initialize pins & state for the power manager. + * + */ +void Power::init() { + psu_on = ENABLED(PSU_DEFAULT_OFF); // Set opposite state to get full power_off/on + TERN(PSU_DEFAULT_OFF, power_off(), power_on()); +} -millis_t Power::lastPowerOn; +/** + * Power on if the power is currently off. + * Restores stepper drivers and processes any PSU_POWERUP_GCODE. + * + */ +void Power::power_on() { + #if ENABLED(AUTO_POWER_CONTROL) + const millis_t now = millis(); + lastPowerOn = now + !now; + #endif + + if (psu_on) return; -bool Power::is_power_needed() { - #if ENABLED(AUTO_POWER_FANS) - FANS_LOOP(i) if (thermalManager.fan_speed[i]) return true; + #if EITHER(POWER_OFF_TIMER, POWER_OFF_WAIT_FOR_COOLDOWN) + cancelAutoPowerOff(); #endif - #if ENABLED(AUTO_POWER_E_FANS) - HOTEND_LOOP() if (thermalManager.autofan_speed[e]) return true; + OUT_WRITE(PS_ON_PIN, PSU_ACTIVE_STATE); + psu_on = true; + safe_delay(PSU_POWERUP_DELAY); + restore_stepper_drivers(); + TERN_(HAS_TRINAMIC_CONFIG, safe_delay(PSU_POWERUP_DELAY)); + + #ifdef PSU_POWERUP_GCODE + gcode.process_subcommands_now(F(PSU_POWERUP_GCODE)); #endif +} - #if BOTH(USE_CONTROLLER_FAN, AUTO_POWER_CONTROLLERFAN) - if (controllerFan.state()) return true; +/** + * Power off if the power is currently on. + * Processes any PSU_POWEROFF_GCODE and makes a PS_OFF_SOUND if enabled. + */ +void Power::power_off() { + SERIAL_ECHOLNPGM(STR_POWEROFF); + + TERN_(HAS_SUICIDE, suicide()); + + if (!psu_on) return; + + #ifdef PSU_POWEROFF_GCODE + gcode.process_subcommands_now(F(PSU_POWEROFF_GCODE)); #endif - if (TERN0(AUTO_POWER_CHAMBER_FAN, thermalManager.chamberfan_speed)) - return true; + #if ENABLED(PS_OFF_SOUND) + BUZZ(1000, 659); + #endif - // If any of the drivers or the bed are enabled... - if (X_ENABLE_READ() == X_ENABLE_ON || Y_ENABLE_READ() == Y_ENABLE_ON || Z_ENABLE_READ() == Z_ENABLE_ON - #if HAS_X2_ENABLE - || X2_ENABLE_READ() == X_ENABLE_ON - #endif - #if HAS_Y2_ENABLE - || Y2_ENABLE_READ() == Y_ENABLE_ON + OUT_WRITE(PS_ON_PIN, !PSU_ACTIVE_STATE); + psu_on = false; + + #if EITHER(POWER_OFF_TIMER, POWER_OFF_WAIT_FOR_COOLDOWN) + cancelAutoPowerOff(); + #endif +} + +#if EITHER(AUTO_POWER_CONTROL, POWER_OFF_WAIT_FOR_COOLDOWN) + + bool Power::is_cooling_needed() { + #if HAS_HOTEND && AUTO_POWER_E_TEMP + HOTEND_LOOP() if (thermalManager.degHotend(e) >= (AUTO_POWER_E_TEMP)) return true; #endif - #if HAS_Z2_ENABLE - || Z2_ENABLE_READ() == Z_ENABLE_ON + + #if HAS_HEATED_CHAMBER && AUTO_POWER_CHAMBER_TEMP + if (thermalManager.degChamber() >= (AUTO_POWER_CHAMBER_TEMP)) return true; #endif - #if E_STEPPERS - #define _OR_ENABLED_E(N) || E##N##_ENABLE_READ() == E_ENABLE_ON - REPEAT(E_STEPPERS, _OR_ENABLED_E) + + #if HAS_COOLER && AUTO_POWER_COOLER_TEMP + if (thermalManager.degCooler() >= (AUTO_POWER_COOLER_TEMP)) return true; #endif - ) return true; - HOTEND_LOOP() if (thermalManager.degTargetHotend(e) > 0 || thermalManager.temp_hotend[e].soft_pwm_amount > 0) return true; - if (TERN0(HAS_HEATED_BED, thermalManager.degTargetBed() > 0 || thermalManager.temp_bed.soft_pwm_amount > 0)) return true; + return false; + } + +#endif + +#if EITHER(POWER_OFF_TIMER, POWER_OFF_WAIT_FOR_COOLDOWN) - #if HAS_HOTEND && AUTO_POWER_E_TEMP - HOTEND_LOOP() if (thermalManager.degHotend(e) >= AUTO_POWER_E_TEMP) return true; + #if ENABLED(POWER_OFF_TIMER) + millis_t Power::power_off_time = 0; + void Power::setPowerOffTimer(const millis_t delay_ms) { power_off_time = millis() + delay_ms; } #endif - #if HAS_HEATED_CHAMBER && AUTO_POWER_CHAMBER_TEMP - if (thermalManager.degChamber() >= AUTO_POWER_CHAMBER_TEMP) return true; + #if ENABLED(POWER_OFF_WAIT_FOR_COOLDOWN) + bool Power::power_off_on_cooldown = false; + void Power::setPowerOffOnCooldown(const bool ena) { power_off_on_cooldown = ena; } #endif - return false; -} + void Power::cancelAutoPowerOff() { + TERN_(POWER_OFF_TIMER, power_off_time = 0); + TERN_(POWER_OFF_WAIT_FOR_COOLDOWN, power_off_on_cooldown = false); + } -void Power::check() { - static millis_t nextPowerCheck = 0; - millis_t ms = millis(); - if (ELAPSED(ms, nextPowerCheck)) { - nextPowerCheck = ms + 2500UL; - if (is_power_needed()) - power_on(); - else if (!lastPowerOn || ELAPSED(ms, lastPowerOn + SEC_TO_MS(POWER_TIMEOUT))) - power_off(); + void Power::checkAutoPowerOff() { + if (TERN1(POWER_OFF_TIMER, !power_off_time) && TERN1(POWER_OFF_WAIT_FOR_COOLDOWN, !power_off_on_cooldown)) return; + if (TERN0(POWER_OFF_WAIT_FOR_COOLDOWN, power_off_on_cooldown && is_cooling_needed())) return; + if (TERN0(POWER_OFF_TIMER, power_off_time && PENDING(millis(), power_off_time))) return; + power_off(); } -} -void Power::power_on() { - lastPowerOn = millis(); - if (!powersupply_on) { - PSU_PIN_ON(); - safe_delay(PSU_POWERUP_DELAY); - restore_stepper_drivers(); - TERN_(HAS_TRINAMIC_CONFIG, safe_delay(PSU_POWERUP_DELAY)); - #ifdef PSU_POWERUP_GCODE - GcodeSuite::process_subcommands_now_P(PSTR(PSU_POWERUP_GCODE)); +#endif // POWER_OFF_TIMER || POWER_OFF_WAIT_FOR_COOLDOWN + +#if ENABLED(AUTO_POWER_CONTROL) + + #ifndef POWER_TIMEOUT + #define POWER_TIMEOUT 0 + #endif + + /** + * Check all conditions that would signal power needing to be on. + * + * @returns bool if power is needed + */ + bool Power::is_power_needed() { + + // If any of the stepper drivers are enabled... + if (stepper.axis_enabled.bits) return true; + + if (printJobOngoing() || printingIsPaused()) return true; + + #if ENABLED(AUTO_POWER_FANS) + FANS_LOOP(i) if (thermalManager.fan_speed[i]) return true; #endif + + #if ENABLED(AUTO_POWER_E_FANS) + HOTEND_LOOP() if (thermalManager.autofan_speed[e]) return true; + #endif + + #if BOTH(USE_CONTROLLER_FAN, AUTO_POWER_CONTROLLERFAN) + if (controllerFan.state()) return true; + #endif + + if (TERN0(AUTO_POWER_CHAMBER_FAN, thermalManager.chamberfan_speed)) + return true; + + if (TERN0(AUTO_POWER_COOLER_FAN, thermalManager.coolerfan_speed)) + return true; + + #if HAS_HOTEND + HOTEND_LOOP() if (thermalManager.degTargetHotend(e) > 0 || thermalManager.temp_hotend[e].soft_pwm_amount > 0) return true; + #endif + + if (TERN0(HAS_HEATED_BED, thermalManager.degTargetBed() > 0 || thermalManager.temp_bed.soft_pwm_amount > 0)) return true; + + return is_cooling_needed(); } -} -void Power::power_off() { - if (powersupply_on) { - #ifdef PSU_POWEROFF_GCODE - GcodeSuite::process_subcommands_now_P(PSTR(PSU_POWEROFF_GCODE)); + /** + * Check if we should power off automatically (POWER_TIMEOUT elapsed, !is_power_needed). + * + * @param pause pause the 'timer' + */ + void Power::check(const bool pause) { + static millis_t nextPowerCheck = 0; + const millis_t now = millis(); + #if POWER_TIMEOUT > 0 + static bool _pause = false; + if (pause != _pause) { + lastPowerOn = now + !now; + _pause = pause; + } + if (pause) return; #endif - PSU_PIN_OFF(); + if (ELAPSED(now, nextPowerCheck)) { + nextPowerCheck = now + 2500UL; + if (is_power_needed()) + power_on(); + else if (!lastPowerOn || (POWER_TIMEOUT > 0 && ELAPSED(now, lastPowerOn + SEC_TO_MS(POWER_TIMEOUT)))) + power_off(); + } } -} -void Power::power_off_soon() { - #if POWER_OFF_DELAY - lastPowerOn = millis() - SEC_TO_MS(POWER_TIMEOUT) + SEC_TO_MS(POWER_OFF_DELAY); - #else - power_off(); + #if POWER_OFF_DELAY > 0 + + /** + * Power off with a delay. Power off is triggered by check() after the delay. + */ + void Power::power_off_soon() { + lastPowerOn = millis() - SEC_TO_MS(POWER_TIMEOUT) + SEC_TO_MS(POWER_OFF_DELAY); + } + #endif -} #endif // AUTO_POWER_CONTROL + +#endif // PSU_CONTROL || AUTO_POWER_CONTROL diff --git a/Marlin/src/feature/power.h b/Marlin/src/feature/power.h index 2462b9231b92..839366ca602b 100644 --- a/Marlin/src/feature/power.h +++ b/Marlin/src/feature/power.h @@ -25,17 +25,48 @@ * power.h - power control */ -#include "../core/millis_t.h" +#if EITHER(AUTO_POWER_CONTROL, POWER_OFF_TIMER) + #include "../core/millis_t.h" +#endif class Power { public: - static void check(); + static bool psu_on; + + static void init(); static void power_on(); static void power_off(); - static void power_off_soon(); - private: - static millis_t lastPowerOn; - static bool is_power_needed(); + + #if EITHER(POWER_OFF_TIMER, POWER_OFF_WAIT_FOR_COOLDOWN) + #if ENABLED(POWER_OFF_TIMER) + static millis_t power_off_time; + static void setPowerOffTimer(const millis_t delay_ms); + #endif + #if ENABLED(POWER_OFF_WAIT_FOR_COOLDOWN) + static bool power_off_on_cooldown; + static void setPowerOffOnCooldown(const bool ena); + #endif + static void cancelAutoPowerOff(); + static void checkAutoPowerOff(); + #endif + + #if ENABLED(AUTO_POWER_CONTROL) && POWER_OFF_DELAY > 0 + static void power_off_soon(); + #else + static void power_off_soon() { power_off(); } + #endif + + #if ENABLED(AUTO_POWER_CONTROL) + static void check(const bool pause); + + private: + static millis_t lastPowerOn; + static bool is_power_needed(); + static bool is_cooling_needed(); + #elif ENABLED(POWER_OFF_WAIT_FOR_COOLDOWN) + private: + static bool is_cooling_needed(); + #endif }; extern Power powerManager; diff --git a/Marlin/src/feature/power_monitor.cpp b/Marlin/src/feature/power_monitor.cpp index 97c4a933637e..504f1ea48e3b 100644 --- a/Marlin/src/feature/power_monitor.cpp +++ b/Marlin/src/feature/power_monitor.cpp @@ -26,8 +26,11 @@ #include "power_monitor.h" -#include "../lcd/marlinui.h" -#include "../lcd/lcdprint.h" +#if HAS_MARLINUI_MENU + #include "../lcd/marlinui.h" + #include "../lcd/lcdprint.h" +#endif + #include "../libs/numtostr.h" uint8_t PowerMonitor::flags; // = 0 @@ -54,7 +57,7 @@ PowerMonitor power_monitor; // Single instance - this calls the constructor } #endif - #if HAS_POWER_MONITOR_VREF + #if ENABLED(POWER_MONITOR_VOLTAGE) void PowerMonitor::draw_voltage() { const float volts = getVolts(); lcd_put_u8str(volts < 100 ? ftostr31ns(volts) : ui16tostr4rj((uint16_t)volts)); diff --git a/Marlin/src/feature/power_monitor.h b/Marlin/src/feature/power_monitor.h index f378ee2a107c..f6e0b292e30c 100644 --- a/Marlin/src/feature/power_monitor.h +++ b/Marlin/src/feature/power_monitor.h @@ -23,7 +23,7 @@ #include "../inc/MarlinConfig.h" -#define PM_SAMPLE_RANGE 1024 +#define PM_SAMPLE_RANGE HAL_ADC_RANGE #define PM_K_VALUE 6 #define PM_K_SCALE 6 @@ -35,7 +35,7 @@ struct pm_lpf_t { filter_buf = filter_buf - (filter_buf >> K_VALUE) + (uint32_t(sample) << K_SCALE); } void capture() { - value = filter_buf * (SCALE * (1.0f / (1UL << (PM_K_VALUE + PM_K_SCALE)))) + (POWER_MONITOR_CURRENT_OFFSET); + value = filter_buf * (SCALE * (1.0f / (1UL << (PM_K_VALUE + PM_K_SCALE)))); } void reset(uint16_t reset_value = 0) { filter_buf = uint32_t(reset_value) << (K_VALUE + K_SCALE); @@ -69,19 +69,15 @@ class PowerMonitor { }; #if ENABLED(POWER_MONITOR_CURRENT) - FORCE_INLINE static float getAmps() { return amps.value; } + FORCE_INLINE static float getAmps() { return amps.value + (POWER_MONITOR_CURRENT_OFFSET); } void add_current_sample(const uint16_t value) { amps.add_sample(value); } #endif - #if HAS_POWER_MONITOR_VREF - #if ENABLED(POWER_MONITOR_VOLTAGE) - FORCE_INLINE static float getVolts() { return volts.value; } - #else - FORCE_INLINE static float getVolts() { return POWER_MONITOR_FIXED_VOLTAGE; } // using a specified fixed valtage as the voltage measurement - #endif - #if ENABLED(POWER_MONITOR_VOLTAGE) - void add_voltage_sample(const uint16_t value) { volts.add_sample(value); } - #endif + #if ENABLED(POWER_MONITOR_VOLTAGE) + FORCE_INLINE static float getVolts() { return volts.value + (POWER_MONITOR_VOLTAGE_OFFSET); } + void add_voltage_sample(const uint16_t value) { volts.add_sample(value); } + #else + FORCE_INLINE static float getVolts() { return POWER_MONITOR_FIXED_VOLTAGE; } #endif #if HAS_POWER_MONITOR_WATTS @@ -98,7 +94,7 @@ class PowerMonitor { FORCE_INLINE static void set_current_display(const bool b) { SET_BIT_TO(flags, PM_DISP_BIT_I, b); } FORCE_INLINE static void toggle_current_display() { TBI(flags, PM_DISP_BIT_I); } #endif - #if HAS_POWER_MONITOR_VREF + #if ENABLED(POWER_MONITOR_VOLTAGE) static void draw_voltage(); FORCE_INLINE static bool voltage_display_enabled() { return TEST(flags, PM_DISP_BIT_V); } FORCE_INLINE static void set_voltage_display(const bool b) { SET_BIT_TO(flags, PM_DISP_BIT_V, b); } diff --git a/Marlin/src/feature/powerloss.cpp b/Marlin/src/feature/powerloss.cpp index 1bb3a7e91531..214c248d359c 100644 --- a/Marlin/src/feature/powerloss.cpp +++ b/Marlin/src/feature/powerloss.cpp @@ -40,7 +40,7 @@ uint8_t PrintJobRecovery::queue_index_r; uint32_t PrintJobRecovery::cmd_sdpos, // = 0 PrintJobRecovery::sdpos[BUFSIZE]; -#if ENABLED(DWIN_CREALITY_LCD) +#if HAS_DWIN_E3V2_BASIC bool PrintJobRecovery::dwin_flag; // = false #endif @@ -54,6 +54,10 @@ uint32_t PrintJobRecovery::cmd_sdpos, // = 0 #include "../module/temperature.h" #include "../core/serial.h" +#if HOMING_Z_WITH_PROBE + #include "../module/probe.h" +#endif + #if ENABLED(FWRETRACT) #include "fwretract.h" #endif @@ -66,9 +70,6 @@ PrintJobRecovery recovery; #ifndef POWER_LOSS_PURGE_LEN #define POWER_LOSS_PURGE_LEN 0 #endif -#ifndef POWER_LOSS_ZRAISE - #define POWER_LOSS_ZRAISE 2 // Move on loss with backup power, or on resume without it -#endif #if DISABLED(BACKUP_POWER_SUPPLY) #undef POWER_LOSS_RETRACT_LEN // No retract at outage without backup power @@ -112,7 +113,7 @@ void PrintJobRecovery::check() { if (card.isMounted()) { load(); if (!valid()) return cancel(); - queue.inject_P(PSTR("M1000S")); + queue.inject(F("M1000S")); } } @@ -133,21 +134,23 @@ void PrintJobRecovery::load() { (void)file.read(&info, sizeof(info)); close(); } - debug(PSTR("Load")); + debug(F("Load")); } /** * Set info fields that won't change */ void PrintJobRecovery::prepare() { - card.getAbsFilename(info.sd_filename); // SD filename + card.getAbsFilenameInCWD(info.sd_filename); // SD filename cmd_sdpos = 0; } /** * Save the current machine state to the power-loss recovery file */ -void PrintJobRecovery::save(const bool force/*=false*/, const float zraise/*=0*/) { +void PrintJobRecovery::save(const bool force/*=false*/, const float zraise/*=POWER_LOSS_ZRAISE*/, const bool raised/*=false*/) { + + // We don't check IS_SD_PRINTING here so a save may occur during a pause #if SAVE_INFO_INTERVAL_MS > 0 static millis_t next_save_ms; // = 0 @@ -179,20 +182,19 @@ void PrintJobRecovery::save(const bool force/*=false*/, const float zraise/*=0*/ info.valid_foot = info.valid_head; // Machine state - info.current_position = current_position; + // info.sdpos and info.current_position are pre-filled from the Stepper ISR + info.feedrate = uint16_t(MMS_TO_MMM(feedrate_mm_s)); info.zraise = zraise; + info.flag.raised = raised; // Was Z raised before power-off? TERN_(GCODE_REPEAT_MARKERS, info.stored_repeat = repeat); TERN_(HAS_HOME_OFFSET, info.home_offset = home_offset); TERN_(HAS_POSITION_SHIFT, info.position_shift = position_shift); - - #if HAS_MULTI_EXTRUDER - info.active_extruder = active_extruder; - #endif + E_TERN_(info.active_extruder = active_extruder); #if DISABLED(NO_VOLUMETRICS) - info.volumetric_enabled = parser.volumetric_enabled; + info.flag.volumetric_enabled = parser.volumetric_enabled; #if HAS_MULTI_EXTRUDER for (int8_t e = 0; e < EXTRUDERS; e++) info.filament_size[e] = planner.filament_size[e]; #else @@ -200,11 +202,11 @@ void PrintJobRecovery::save(const bool force/*=false*/, const float zraise/*=0*/ #endif #endif - #if EXTRUDERS - HOTEND_LOOP() info.target_temperature[e] = thermalManager.temp_hotend[e].target; + #if HAS_EXTRUDERS + HOTEND_LOOP() info.target_temperature[e] = thermalManager.degTargetHotend(e); #endif - TERN_(HAS_HEATED_BED, info.target_temperature_bed = thermalManager.temp_bed.target); + TERN_(HAS_HEATED_BED, info.target_temperature_bed = thermalManager.degTargetBed()); #if HAS_FAN COPY(info.fan_speed, thermalManager.fan_speed); @@ -240,21 +242,21 @@ void PrintJobRecovery::save(const bool force/*=false*/, const float zraise/*=0*/ #if ENABLED(BACKUP_POWER_SUPPLY) - void PrintJobRecovery::retract_and_lift(const float &zraise) { + void PrintJobRecovery::retract_and_lift(const_float_t zraise) { #if POWER_LOSS_RETRACT_LEN || POWER_LOSS_ZRAISE gcode.set_relative_mode(true); // Use relative coordinates #if POWER_LOSS_RETRACT_LEN // Retract filament now - gcode.process_subcommands_now_P(PSTR("G1 F3000 E-" STRINGIFY(POWER_LOSS_RETRACT_LEN))); + gcode.process_subcommands_now(F("G1 F3000 E-" STRINGIFY(POWER_LOSS_RETRACT_LEN))); #endif #if POWER_LOSS_ZRAISE // Raise the Z axis now if (zraise) { char cmd[20], str_1[16]; - sprintf_P(cmd, PSTR("G0 Z%s"), dtostrf(zraise, 1, 3, str_1)); + sprintf_P(cmd, PSTR("G0Z%s"), dtostrf(zraise, 1, 3, str_1)); gcode.process_subcommands_now(cmd); } #else @@ -268,6 +270,10 @@ void PrintJobRecovery::save(const bool force/*=false*/, const float zraise/*=0*/ #endif +#endif // POWER_LOSS_PIN + +#if PIN_EXISTS(POWER_LOSS) || ENABLED(DEBUG_POWER_LOSS_RECOVERY) + /** * An outage was detected by a sensor pin. * - If not SD printing, let the machine turn off on its own with no "KILL" screen @@ -276,7 +282,7 @@ void PrintJobRecovery::save(const bool force/*=false*/, const float zraise/*=0*/ * - If backup power is available Retract E and Raise Z * - Go to the KILL screen */ - void PrintJobRecovery::_outage() { + void PrintJobRecovery::_outage(TERN_(DEBUG_POWER_LOSS_RECOVERY, const bool simulated/*=false*/)) { #if ENABLED(BACKUP_POWER_SUPPLY) static bool lock = false; if (lock) return; // No re-entrance from idle() during retract_and_lift() @@ -290,8 +296,9 @@ void PrintJobRecovery::save(const bool force/*=false*/, const float zraise/*=0*/ constexpr float zraise = 0; #endif - // Save, including the limited Z raise - if (IS_SD_PRINTING()) save(true, zraise); + // Save the current position, distance that Z was (or should be) raised, + // and a flag whether the raise was already done here. + if (IS_SD_PRINTING()) save(true, zraise, ENABLED(BACKUP_POWER_SUPPLY)); // Disable all heaters to reduce power loss thermalManager.disable_all_heaters(); @@ -303,17 +310,23 @@ void PrintJobRecovery::save(const bool force/*=false*/, const float zraise/*=0*/ retract_and_lift(zraise); #endif - kill(GET_TEXT(MSG_OUTAGE_RECOVERY)); + if (TERN0(DEBUG_POWER_LOSS_RECOVERY, simulated)) { + card.fileHasFinished(); + current_position.reset(); + sync_plan_position(); + } + else + kill(GET_TEXT_F(MSG_OUTAGE_RECOVERY)); } -#endif +#endif // POWER_LOSS_PIN || DEBUG_POWER_LOSS_RECOVERY /** * Save the recovery info the recovery file */ void PrintJobRecovery::write() { - debug(PSTR("Write")); + debug(F("Write")); open(false); file.seekSet(0); @@ -339,71 +352,109 @@ void PrintJobRecovery::resume() { #if HAS_LEVELING // Make sure leveling is off before any G92 and G28 - gcode.process_subcommands_now_P(PSTR("M420 S0 Z0")); + gcode.process_subcommands_now(F("M420 S0 Z0")); #endif #if HAS_HEATED_BED - const int16_t bt = info.target_temperature_bed; + const celsius_t bt = info.target_temperature_bed; if (bt) { // Restore the bed temperature - sprintf_P(cmd, PSTR("M190 S%i"), bt); + sprintf_P(cmd, PSTR("M190S%i"), bt); gcode.process_subcommands_now(cmd); } #endif - // Restore all hotend temperatures + // Heat hotend enough to soften material #if HAS_HOTEND HOTEND_LOOP() { - const int16_t et = info.target_temperature[e]; + const celsius_t et = _MAX(info.target_temperature[e], 180); if (et) { #if HAS_MULTI_HOTEND - sprintf_P(cmd, PSTR("T%i S"), e); + sprintf_P(cmd, PSTR("T%iS"), e); gcode.process_subcommands_now(cmd); #endif - sprintf_P(cmd, PSTR("M109 S%i"), et); + sprintf_P(cmd, PSTR("M109S%i"), et); gcode.process_subcommands_now(cmd); } } #endif - // Reset E, raise Z, home XY... - #if Z_HOME_DIR > 0 + // Interpret the saved Z according to flags + const float z_print = info.current_position.z, + z_raised = z_print + info.zraise; + + // + // Home the axes that can safely be homed, and + // establish the current position as best we can. + // - // If Z homing goes to max, just reset E and home all - gcode.process_subcommands_now_P(PSTR( - "G92.9 E0\n" - "G28R0" - )); + gcode.process_subcommands_now(F("G92.9E0")); // Reset E to 0 - #else // "G92.9 E0 ..." + #if Z_HOME_TO_MAX - // If a Z raise occurred at outage restore Z, otherwise raise Z now - sprintf_P(cmd, PSTR("G92.9 E0 " TERN(BACKUP_POWER_SUPPLY, "Z%s", "Z0\nG1Z%s")), dtostrf(info.zraise, 1, 3, str_1)); + float z_now = z_raised; + + // If Z homing goes to max then just move back to the "raised" position + sprintf_P(cmd, PSTR( + "G28R0\n" // Home all axes (no raise) + "G1Z%sF1200" // Move Z down to (raised) height + ), dtostrf(z_now, 1, 3, str_1)); gcode.process_subcommands_now(cmd); - // Home safely with no Z raise - gcode.process_subcommands_now_P(PSTR( - "G28R0" // No raise during G28 - #if IS_CARTESIAN && (DISABLED(POWER_LOSS_RECOVER_ZHOME) || defined(POWER_LOSS_ZHOME_POS)) - "XY" // Don't home Z on Cartesian unless overridden - #endif - )); + #elif DISABLED(BELTPRINTER) + + #if ENABLED(POWER_LOSS_RECOVER_ZHOME) && defined(POWER_LOSS_ZHOME_POS) + #define HOMING_Z_DOWN 1 + #endif + + float z_now = info.flag.raised ? z_raised : z_print; + + #if !HOMING_Z_DOWN + // Set Z to the real position + sprintf_P(cmd, PSTR("G92.9Z%s"), dtostrf(z_now, 1, 3, str_1)); + gcode.process_subcommands_now(cmd); + #endif + + // Does Z need to be raised now? It should be raised before homing XY. + if (z_raised > z_now) { + z_now = z_raised; + sprintf_P(cmd, PSTR("G1Z%sF600"), dtostrf(z_now, 1, 3, str_1)); + gcode.process_subcommands_now(cmd); + } + + // Home XY with no Z raise + gcode.process_subcommands_now(F("G28R0XY")); // No raise during G28 #endif - #ifdef POWER_LOSS_ZHOME_POS - // If defined move to a safe Z homing position that avoids the print - constexpr xy_pos_t p = POWER_LOSS_ZHOME_POS; - sprintf_P(cmd, PSTR("G1 X%s Y%s F1000\nG28Z"), dtostrf(p.x, 1, 3, str_1), dtostrf(p.y, 1, 3, str_2)); + #if HOMING_Z_DOWN + // Move to a safe XY position and home Z while avoiding the print. + const xy_pos_t p = xy_pos_t(POWER_LOSS_ZHOME_POS) TERN_(HOMING_Z_WITH_PROBE, - probe.offset_xy); + sprintf_P(cmd, PSTR("G1X%sY%sF1000\nG28HZ"), dtostrf(p.x, 1, 3, str_1), dtostrf(p.y, 1, 3, str_2)); gcode.process_subcommands_now(cmd); #endif - // Ensure that all axes are marked as homed + // Mark all axes as having been homed (no effect on current_position) set_all_homed(); + #if HAS_LEVELING + // Restore Z fade and possibly re-enable bed leveling compensation. + // Leveling may already be enabled due to the ENABLE_LEVELING_AFTER_G28 option. + // TODO: Add a G28 parameter to leave leveling disabled. + sprintf_P(cmd, PSTR("M420S%cZ%s"), '0' + (char)info.flag.leveling, dtostrf(info.fade, 1, 1, str_1)); + gcode.process_subcommands_now(cmd); + + #if !HOMING_Z_DOWN + // The physical Z was adjusted at power-off so undo the M420S1 correction to Z with G92.9. + sprintf_P(cmd, PSTR("G92.9Z%s"), dtostrf(z_now, 1, 1, str_1)); + gcode.process_subcommands_now(cmd); + #endif + #endif + #if ENABLED(POWER_LOSS_RECOVER_ZHOME) - // Now move to ZsavedPos + POWER_LOSS_ZRAISE - sprintf_P(cmd, PSTR("G1 F500 Z%s"), dtostrf(info.current_position.z + POWER_LOSS_ZRAISE, 1, 3, str_1)); + // Z was homed down to the bed, so move up to the raised height. + z_now = z_raised; + sprintf_P(cmd, PSTR("G1Z%sF600"), dtostrf(z_now, 1, 3, str_1)); gcode.process_subcommands_now(cmd); #endif @@ -411,23 +462,38 @@ void PrintJobRecovery::resume() { #if DISABLED(NO_VOLUMETRICS) #if HAS_MULTI_EXTRUDER for (int8_t e = 0; e < EXTRUDERS; e++) { - sprintf_P(cmd, PSTR("M200 T%i D%s"), e, dtostrf(info.filament_size[e], 1, 3, str_1)); + sprintf_P(cmd, PSTR("M200T%iD%s"), e, dtostrf(info.filament_size[e], 1, 3, str_1)); gcode.process_subcommands_now(cmd); } - if (!info.volumetric_enabled) { - sprintf_P(cmd, PSTR("M200 T%i D0"), info.active_extruder); + if (!info.flag.volumetric_enabled) { + sprintf_P(cmd, PSTR("M200T%iD0"), info.active_extruder); gcode.process_subcommands_now(cmd); } #else - if (info.volumetric_enabled) { - sprintf_P(cmd, PSTR("M200 D%s"), dtostrf(info.filament_size[0], 1, 3, str_1)); + if (info.flag.volumetric_enabled) { + sprintf_P(cmd, PSTR("M200D%s"), dtostrf(info.filament_size[0], 1, 3, str_1)); gcode.process_subcommands_now(cmd); } #endif #endif - // Select the previously active tool (with no_move) - #if HAS_MULTI_EXTRUDER + // Restore all hotend temperatures + #if HAS_HOTEND + HOTEND_LOOP() { + const celsius_t et = info.target_temperature[e]; + if (et) { + #if HAS_MULTI_HOTEND + sprintf_P(cmd, PSTR("T%iS"), e); + gcode.process_subcommands_now(cmd); + #endif + sprintf_P(cmd, PSTR("M109S%i"), et); + gcode.process_subcommands_now(cmd); + } + } + #endif + + // Restore the previously active tool (with no_move) + #if HAS_MULTI_EXTRUDER || HAS_MULTI_HOTEND sprintf_P(cmd, PSTR("T%i S"), info.active_extruder); gcode.process_subcommands_now(cmd); #endif @@ -437,13 +503,13 @@ void PrintJobRecovery::resume() { FANS_LOOP(i) { const int f = info.fan_speed[i]; if (f) { - sprintf_P(cmd, PSTR("M106 P%i S%i"), i, f); + sprintf_P(cmd, PSTR("M106P%iS%i"), i, f); gcode.process_subcommands_now(cmd); } } #endif - // Restore retract and hop state + // Restore retract and hop state from an active `G10` command #if ENABLED(FWRETRACT) LOOP_L_N(e, EXTRUDERS) { if (info.retract[e] != 0.0) { @@ -454,64 +520,49 @@ void PrintJobRecovery::resume() { fwretract.current_hop = info.retract_hop; #endif - #if HAS_LEVELING - // Restore leveling state before 'G92 Z' to ensure - // the Z stepper count corresponds to the native Z. - if (info.fade || info.flag.leveling) { - sprintf_P(cmd, PSTR("M420 S%i Z%s"), int(info.flag.leveling), dtostrf(info.fade, 1, 1, str_1)); - gcode.process_subcommands_now(cmd); - } - #endif - #if ENABLED(GRADIENT_MIX) memcpy(&mixer.gradient, &info.gradient, sizeof(info.gradient)); #endif // Un-retract if there was a retract at outage - #if POWER_LOSS_RETRACT_LEN - gcode.process_subcommands_now_P(PSTR("G1 E" STRINGIFY(POWER_LOSS_RETRACT_LEN) " F3000")); + #if ENABLED(BACKUP_POWER_SUPPLY) && POWER_LOSS_RETRACT_LEN > 0 + gcode.process_subcommands_now(F("G1F3000E" STRINGIFY(POWER_LOSS_RETRACT_LEN))); #endif - // Additional purge if configured + // Additional purge on resume if configured #if POWER_LOSS_PURGE_LEN - sprintf_P(cmd, PSTR("G1 E%d F3000"), (POWER_LOSS_PURGE_LEN) + (POWER_LOSS_RETRACT_LEN)); + sprintf_P(cmd, PSTR("G1F3000E%d"), (POWER_LOSS_PURGE_LEN) + (POWER_LOSS_RETRACT_LEN)); gcode.process_subcommands_now(cmd); #endif #if ENABLED(NOZZLE_CLEAN_FEATURE) - gcode.process_subcommands_now_P(PSTR("G12")); + gcode.process_subcommands_now(F("G12")); #endif - // Move back to the saved XY - sprintf_P(cmd, PSTR("G1 X%s Y%s F3000"), + // Move back over to the saved XY + sprintf_P(cmd, PSTR("G1X%sY%sF3000"), dtostrf(info.current_position.x, 1, 3, str_1), dtostrf(info.current_position.y, 1, 3, str_2) ); gcode.process_subcommands_now(cmd); - // Move back to the saved Z - dtostrf(info.current_position.z, 1, 3, str_1); - #if Z_HOME_DIR > 0 || ENABLED(POWER_LOSS_RECOVER_ZHOME) - sprintf_P(cmd, PSTR("G1 Z%s F500"), str_1); - #else - gcode.process_subcommands_now_P(PSTR("G1 Z0 F200")); - sprintf_P(cmd, PSTR("G92.9 Z%s"), str_1); - #endif + // Move back down to the saved Z for printing + sprintf_P(cmd, PSTR("G1Z%sF600"), dtostrf(z_print, 1, 3, str_1)); gcode.process_subcommands_now(cmd); // Restore the feedrate - sprintf_P(cmd, PSTR("G1 F%d"), info.feedrate); + sprintf_P(cmd, PSTR("G1F%d"), info.feedrate); gcode.process_subcommands_now(cmd); // Restore E position with G92.9 - sprintf_P(cmd, PSTR("G92.9 E%s"), dtostrf(info.current_position.e, 1, 3, str_1)); + sprintf_P(cmd, PSTR("G92.9E%s"), dtostrf(info.current_position.e, 1, 3, str_1)); gcode.process_subcommands_now(cmd); TERN_(GCODE_REPEAT_MARKERS, repeat = info.stored_repeat); TERN_(HAS_HOME_OFFSET, home_offset = info.home_offset); TERN_(HAS_POSITION_SHIFT, position_shift = info.position_shift); #if HAS_HOME_OFFSET || HAS_POSITION_SHIFT - LOOP_XYZ(i) update_workspace_offset((AxisEnum)i); + LOOP_LINEAR_AXES(i) update_workspace_offset((AxisEnum)i); #endif // Relative axis modes @@ -529,7 +580,7 @@ void PrintJobRecovery::resume() { char *fn = info.sd_filename; sprintf_P(cmd, M23_STR, fn); gcode.process_subcommands_now(cmd); - sprintf_P(cmd, PSTR("M24 S%ld T%ld"), resume_sdpos, info.print_job_elapsed); + sprintf_P(cmd, PSTR("M24S%ldT%ld"), resume_sdpos, info.print_job_elapsed); gcode.process_subcommands_now(cmd); TERN_(DEBUG_POWER_LOSS_RECOVERY, marlin_debug_flags = old_flags); @@ -537,23 +588,31 @@ void PrintJobRecovery::resume() { #if ENABLED(DEBUG_POWER_LOSS_RECOVERY) - void PrintJobRecovery::debug(PGM_P const prefix) { - DEBUG_ECHOPGM_P(prefix); - DEBUG_ECHOLNPAIR(" Job Recovery Info...\nvalid_head:", info.valid_head, " valid_foot:", info.valid_foot); + void PrintJobRecovery::debug(FSTR_P const prefix) { + DEBUG_ECHOF(prefix); + DEBUG_ECHOLNPGM(" Job Recovery Info...\nvalid_head:", info.valid_head, " valid_foot:", info.valid_foot); if (info.valid_head) { if (info.valid_head == info.valid_foot) { DEBUG_ECHOPGM("current_position: "); - LOOP_XYZE(i) { + LOOP_LOGICAL_AXES(i) { if (i) DEBUG_CHAR(','); DEBUG_DECIMAL(info.current_position[i]); } DEBUG_EOL(); - DEBUG_ECHOLNPAIR("zraise: ", info.zraise); + DEBUG_ECHOLNPGM("feedrate: ", info.feedrate); + + DEBUG_ECHOLNPGM("zraise: ", info.zraise, " ", info.flag.raised ? "(before)" : ""); + + #if ENABLED(GCODE_REPEAT_MARKERS) + DEBUG_ECHOLNPGM("repeat index: ", info.stored_repeat.index); + LOOP_L_N(i, info.stored_repeat.index) + DEBUG_ECHOLNPGM("..... sdpos: ", info.stored_repeat.marker.sdpos, " count: ", info.stored_repeat.marker.counter); + #endif #if HAS_HOME_OFFSET DEBUG_ECHOPGM("home_offset: "); - LOOP_XYZ(i) { + LOOP_LINEAR_AXES(i) { if (i) DEBUG_CHAR(','); DEBUG_DECIMAL(info.home_offset[i]); } @@ -562,17 +621,21 @@ void PrintJobRecovery::resume() { #if HAS_POSITION_SHIFT DEBUG_ECHOPGM("position_shift: "); - LOOP_XYZ(i) { + LOOP_LINEAR_AXES(i) { if (i) DEBUG_CHAR(','); DEBUG_DECIMAL(info.position_shift[i]); } DEBUG_EOL(); #endif - DEBUG_ECHOLNPAIR("feedrate: ", info.feedrate); - #if HAS_MULTI_EXTRUDER - DEBUG_ECHOLNPAIR("active_extruder: ", info.active_extruder); + DEBUG_ECHOLNPGM("active_extruder: ", info.active_extruder); + #endif + + #if DISABLED(NO_VOLUMETRICS) + DEBUG_ECHOPGM("filament_size:"); + LOOP_L_N(i, EXTRUDERS) DEBUG_ECHOLNPGM(" ", info.filament_size[i]); + DEBUG_EOL(); #endif #if HAS_HOTEND @@ -585,7 +648,7 @@ void PrintJobRecovery::resume() { #endif #if HAS_HEATED_BED - DEBUG_ECHOLNPAIR("target_temperature_bed: ", info.target_temperature_bed); + DEBUG_ECHOLNPGM("target_temperature_bed: ", info.target_temperature_bed); #endif #if HAS_FAN @@ -598,8 +661,9 @@ void PrintJobRecovery::resume() { #endif #if HAS_LEVELING - DEBUG_ECHOLNPAIR("leveling: ", info.flag.leveling, " fade: ", info.fade); + DEBUG_ECHOLNPGM("leveling: ", info.flag.leveling ? "ON" : "OFF", " fade: ", info.fade); #endif + #if ENABLED(FWRETRACT) DEBUG_ECHOPGM("retract: "); for (int8_t e = 0; e < EXTRUDERS; e++) { @@ -607,13 +671,30 @@ void PrintJobRecovery::resume() { if (e < EXTRUDERS - 1) DEBUG_CHAR(','); } DEBUG_EOL(); - DEBUG_ECHOLNPAIR("retract_hop: ", info.retract_hop); + DEBUG_ECHOLNPGM("retract_hop: ", info.retract_hop); + #endif + + // Mixing extruder and gradient + #if BOTH(MIXING_EXTRUDER, GRADIENT_MIX) + DEBUG_ECHOLNPGM("gradient: ", info.gradient.enabled ? "ON" : "OFF"); #endif - DEBUG_ECHOLNPAIR("sd_filename: ", info.sd_filename); - DEBUG_ECHOLNPAIR("sdpos: ", info.sdpos); - DEBUG_ECHOLNPAIR("print_job_elapsed: ", info.print_job_elapsed); - DEBUG_ECHOLNPAIR("dryrun: ", info.flag.dryrun); - DEBUG_ECHOLNPAIR("allow_cold_extrusion: ", info.flag.allow_cold_extrusion); + + DEBUG_ECHOLNPGM("sd_filename: ", info.sd_filename); + DEBUG_ECHOLNPGM("sdpos: ", info.sdpos); + DEBUG_ECHOLNPGM("print_job_elapsed: ", info.print_job_elapsed); + + DEBUG_ECHOPGM("axis_relative:"); + if (TEST(info.axis_relative, REL_X)) DEBUG_ECHOPGM(" REL_X"); + if (TEST(info.axis_relative, REL_Y)) DEBUG_ECHOPGM(" REL_Y"); + if (TEST(info.axis_relative, REL_Z)) DEBUG_ECHOPGM(" REL_Z"); + if (TEST(info.axis_relative, REL_E)) DEBUG_ECHOPGM(" REL_E"); + if (TEST(info.axis_relative, E_MODE_ABS)) DEBUG_ECHOPGM(" E_MODE_ABS"); + if (TEST(info.axis_relative, E_MODE_REL)) DEBUG_ECHOPGM(" E_MODE_REL"); + DEBUG_EOL(); + + DEBUG_ECHOLNPGM("flag.dryrun: ", AS_DIGIT(info.flag.dryrun)); + DEBUG_ECHOLNPGM("flag.allow_cold_extrusion: ", AS_DIGIT(info.flag.allow_cold_extrusion)); + DEBUG_ECHOLNPGM("flag.volumetric_enabled: ", AS_DIGIT(info.flag.volumetric_enabled)); } else DEBUG_ECHOLNPGM("INVALID DATA"); diff --git a/Marlin/src/feature/powerloss.h b/Marlin/src/feature/powerloss.h index 25581e17230d..4e97109bb7b9 100644 --- a/Marlin/src/feature/powerloss.h +++ b/Marlin/src/feature/powerloss.h @@ -42,6 +42,10 @@ #define POWER_LOSS_STATE HIGH #endif +#ifndef POWER_LOSS_ZRAISE + #define POWER_LOSS_ZRAISE 2 +#endif + //#define DEBUG_POWER_LOSS_RECOVERY //#define SAVE_EACH_CMD_MODE //#define SAVE_INFO_INTERVAL_MS 0 @@ -52,25 +56,41 @@ typedef struct { // Machine state xyze_pos_t current_position; uint16_t feedrate; + float zraise; // Repeat information - TERN_(GCODE_REPEAT_MARKERS, Repeat stored_repeat); + #if ENABLED(GCODE_REPEAT_MARKERS) + Repeat stored_repeat; + #endif - TERN_(HAS_HOME_OFFSET, xyz_pos_t home_offset); - TERN_(HAS_POSITION_SHIFT, xyz_pos_t position_shift); - TERN_(HAS_MULTI_EXTRUDER, uint8_t active_extruder); + #if HAS_HOME_OFFSET + xyz_pos_t home_offset; + #endif + #if HAS_POSITION_SHIFT + xyz_pos_t position_shift; + #endif + #if HAS_MULTI_EXTRUDER + uint8_t active_extruder; + #endif #if DISABLED(NO_VOLUMETRICS) - bool volumetric_enabled; float filament_size[EXTRUDERS]; #endif - TERN_(HAS_HOTEND, int16_t target_temperature[HOTENDS]); - TERN_(HAS_HEATED_BED, int16_t target_temperature_bed); - TERN_(HAS_FAN, uint8_t fan_speed[FAN_COUNT]); + #if HAS_HOTEND + celsius_t target_temperature[HOTENDS]; + #endif + #if HAS_HEATED_BED + celsius_t target_temperature_bed; + #endif + #if HAS_FAN + uint8_t fan_speed[FAN_COUNT]; + #endif - TERN_(HAS_LEVELING, float fade); + #if HAS_LEVELING + float fade; + #endif #if ENABLED(FWRETRACT) float retract[EXTRUDERS], retract_hop; @@ -80,7 +100,9 @@ typedef struct { #if ENABLED(MIXING_EXTRUDER) //uint_fast8_t selected_vtool; //mixer_comp_t color[NR_MIXING_VIRTUAL_TOOLS][MIXING_STEPPERS]; - TERN_(GRADIENT_MIX, gradient_t gradient); + #if ENABLED(GRADIENT_MIX) + gradient_t gradient; + #endif #endif // SD Filename and position @@ -95,9 +117,15 @@ typedef struct { // Misc. Marlin flags struct { + bool raised:1; // Raised before saved bool dryrun:1; // M111 S8 bool allow_cold_extrusion:1; // M302 P1 - TERN_(HAS_LEVELING, bool leveling:1); + #if HAS_LEVELING + bool leveling:1; // M420 S + #endif + #if DISABLED(NO_VOLUMETRICS) + bool volumetric_enabled:1; // M200 S D + #endif } flag; uint8_t valid_foot; @@ -117,14 +145,14 @@ class PrintJobRecovery { static uint32_t cmd_sdpos, //!< SD position of the next command sdpos[BUFSIZE]; //!< SD positions of queued commands - #if ENABLED(DWIN_CREALITY_LCD) + #if HAS_DWIN_E3V2_BASIC static bool dwin_flag; #endif static void init(); static void prepare(); - static inline void setup() { + static void setup() { #if PIN_EXISTS(POWER_LOSS) #if ENABLED(POWER_LOSS_PULLUP) SET_INPUT_PULLUP(POWER_LOSS_PIN); @@ -137,54 +165,60 @@ class PrintJobRecovery { } // Track each command's file offsets - static inline uint32_t command_sdpos() { return sdpos[queue_index_r]; } - static inline void commit_sdpos(const uint8_t index_w) { sdpos[index_w] = cmd_sdpos; } + static uint32_t command_sdpos() { return sdpos[queue_index_r]; } + static void commit_sdpos(const uint8_t index_w) { sdpos[index_w] = cmd_sdpos; } static bool enabled; static void enable(const bool onoff); static void changed(); - static inline bool exists() { return card.jobRecoverFileExists(); } - static inline void open(const bool read) { card.openJobRecoveryFile(read); } - static inline void close() { file.close(); } + static bool exists() { return card.jobRecoverFileExists(); } + static void open(const bool read) { card.openJobRecoveryFile(read); } + static void close() { file.close(); } static void check(); static void resume(); static void purge(); - static inline void cancel() { purge(); IF_DISABLED(NO_SD_AUTOSTART, card.autofile_begin()); } + static void cancel() { purge(); IF_DISABLED(NO_SD_AUTOSTART, card.autofile_begin()); } static void load(); - static void save(const bool force=ENABLED(SAVE_EACH_CMD_MODE), const float zraise=0); + static void save(const bool force=ENABLED(SAVE_EACH_CMD_MODE), const float zraise=POWER_LOSS_ZRAISE, const bool raised=false); #if PIN_EXISTS(POWER_LOSS) - static inline void outage() { - if (enabled && READ(POWER_LOSS_PIN) == POWER_LOSS_STATE) - _outage(); + static void outage() { + static constexpr uint8_t OUTAGE_THRESHOLD = 3; + static uint8_t outage_counter = 0; + if (enabled && READ(POWER_LOSS_PIN) == POWER_LOSS_STATE) { + outage_counter++; + if (outage_counter >= OUTAGE_THRESHOLD) _outage(); + } + else + outage_counter = 0; } #endif // The referenced file exists - static inline bool interrupted_file_exists() { return card.fileExists(info.sd_filename); } + static bool interrupted_file_exists() { return card.fileExists(info.sd_filename); } - static inline bool valid() { return info.valid() && interrupted_file_exists(); } + static bool valid() { return info.valid() && interrupted_file_exists(); } #if ENABLED(DEBUG_POWER_LOSS_RECOVERY) - static void debug(PGM_P const prefix); + static void debug(FSTR_P const prefix); #else - static inline void debug(PGM_P const) {} + static void debug(FSTR_P const) {} #endif private: static void write(); #if ENABLED(BACKUP_POWER_SUPPLY) - static void retract_and_lift(const float &zraise); + static void retract_and_lift(const_float_t zraise); #endif - #if PIN_EXISTS(POWER_LOSS) + #if PIN_EXISTS(POWER_LOSS) || ENABLED(DEBUG_POWER_LOSS_RECOVERY) friend class GcodeSuite; - static void _outage(); + static void _outage(TERN_(DEBUG_POWER_LOSS_RECOVERY, const bool simulated=false)); #endif }; diff --git a/Marlin/src/feature/probe_temp_comp.cpp b/Marlin/src/feature/probe_temp_comp.cpp index 8fdf160d0fef..9a975d6763fa 100644 --- a/Marlin/src/feature/probe_temp_comp.cpp +++ b/Marlin/src/feature/probe_temp_comp.cpp @@ -22,41 +22,53 @@ #include "../inc/MarlinConfigPre.h" -#if ENABLED(PROBE_TEMP_COMPENSATION) +#if HAS_PTC + +//#define DEBUG_PTC // Print extra debug output with 'M871' #include "probe_temp_comp.h" #include -ProbeTempComp temp_comp; +ProbeTempComp ptc; + +#if ENABLED(PTC_PROBE) + constexpr int16_t z_offsets_probe_default[PTC_PROBE_COUNT] = PTC_PROBE_ZOFFS; + int16_t ProbeTempComp::z_offsets_probe[PTC_PROBE_COUNT] = PTC_PROBE_ZOFFS; +#endif -int16_t ProbeTempComp::z_offsets_probe[cali_info_init[TSI_PROBE].measurements], // = {0} - ProbeTempComp::z_offsets_bed[cali_info_init[TSI_BED].measurements]; // = {0} +#if ENABLED(PTC_BED) + constexpr int16_t z_offsets_bed_default[PTC_BED_COUNT] = PTC_BED_ZOFFS; + int16_t ProbeTempComp::z_offsets_bed[PTC_BED_COUNT] = PTC_BED_ZOFFS; +#endif -#if ENABLED(USE_TEMP_EXT_COMPENSATION) - int16_t ProbeTempComp::z_offsets_ext[cali_info_init[TSI_EXT].measurements]; // = {0} +#if ENABLED(PTC_HOTEND) + constexpr int16_t z_offsets_hotend_default[PTC_HOTEND_COUNT] = PTC_HOTEND_ZOFFS; + int16_t ProbeTempComp::z_offsets_hotend[PTC_HOTEND_COUNT] = PTC_HOTEND_ZOFFS; #endif int16_t *ProbeTempComp::sensor_z_offsets[TSI_COUNT] = { - ProbeTempComp::z_offsets_probe, ProbeTempComp::z_offsets_bed - #if ENABLED(USE_TEMP_EXT_COMPENSATION) - , ProbeTempComp::z_offsets_ext + #if ENABLED(PTC_PROBE) + ProbeTempComp::z_offsets_probe, #endif -}; - -const temp_calib_t ProbeTempComp::cali_info[TSI_COUNT] = { - cali_info_init[TSI_PROBE], cali_info_init[TSI_BED] - #if ENABLED(USE_TEMP_EXT_COMPENSATION) - , cali_info_init[TSI_EXT] + #if ENABLED(PTC_BED) + ProbeTempComp::z_offsets_bed, + #endif + #if ENABLED(PTC_HOTEND) + ProbeTempComp::z_offsets_hotend, #endif }; -constexpr xyz_pos_t ProbeTempComp::park_point; -constexpr xy_pos_t ProbeTempComp::measure_point; -constexpr int ProbeTempComp::probe_calib_bed_temp; +constexpr temp_calib_t ProbeTempComp::cali_info[TSI_COUNT]; uint8_t ProbeTempComp::calib_idx; // = 0 float ProbeTempComp::init_measurement; // = 0.0 +void ProbeTempComp::reset() { + TERN_(PTC_PROBE, LOOP_L_N(i, PTC_PROBE_COUNT) z_offsets_probe[i] = z_offsets_probe_default[i]); + TERN_(PTC_BED, LOOP_L_N(i, PTC_BED_COUNT) z_offsets_bed[i] = z_offsets_bed_default[i]); + TERN_(PTC_HOTEND, LOOP_L_N(i, PTC_HOTEND_COUNT) z_offsets_hotend[i] = z_offsets_hotend_default[i]); +} + void ProbeTempComp::clear_offsets(const TempSensorID tsi) { LOOP_L_N(i, cali_info[tsi].measurements) sensor_z_offsets[tsi][i] = 0; @@ -71,69 +83,67 @@ bool ProbeTempComp::set_offset(const TempSensorID tsi, const uint8_t idx, const void ProbeTempComp::print_offsets() { LOOP_L_N(s, TSI_COUNT) { - float temp = cali_info[s].start_temp; + celsius_t temp = cali_info[s].start_temp; for (int16_t i = -1; i < cali_info[s].measurements; ++i) { - SERIAL_ECHOPGM_P(s == TSI_BED ? PSTR("Bed") : - #if ENABLED(USE_TEMP_EXT_COMPENSATION) - s == TSI_EXT ? PSTR("Extruder") : - #endif - PSTR("Probe") + SERIAL_ECHOF( + TERN_(PTC_BED, s == TSI_BED ? F("Bed") :) + TERN_(PTC_HOTEND, s == TSI_EXT ? F("Extruder") :) + F("Probe") ); - SERIAL_ECHOLNPAIR( + SERIAL_ECHOLNPGM( " temp: ", temp, "C; Offset: ", i < 0 ? 0.0f : sensor_z_offsets[s][i], " um" ); - temp += cali_info[s].temp_res; + temp += cali_info[s].temp_resolution; } } + #if ENABLED(DEBUG_PTC) + float meas[4] = { 0, 0, 0, 0 }; + compensate_measurement(TSI_PROBE, 27.5, meas[0]); + compensate_measurement(TSI_PROBE, 32.5, meas[1]); + compensate_measurement(TSI_PROBE, 77.5, meas[2]); + compensate_measurement(TSI_PROBE, 82.5, meas[3]); + SERIAL_ECHOLNPGM("DEBUG_PTC 27.5:", meas[0], " 32.5:", meas[1], " 77.5:", meas[2], " 82.5:", meas[3]); + #endif } -void ProbeTempComp::prepare_new_calibration(const float &init_meas_z) { +void ProbeTempComp::prepare_new_calibration(const_float_t init_meas_z) { calib_idx = 0; init_measurement = init_meas_z; } -void ProbeTempComp::push_back_new_measurement(const TempSensorID tsi, const float &meas_z) { - switch (tsi) { - case TSI_PROBE: - case TSI_BED: - //case TSI_EXT: - if (calib_idx >= cali_info[tsi].measurements) return; - sensor_z_offsets[tsi][calib_idx++] = static_cast(meas_z * 1000.0f - init_measurement * 1000.0f); - default: break; - } +void ProbeTempComp::push_back_new_measurement(const TempSensorID tsi, const_float_t meas_z) { + if (calib_idx >= cali_info[tsi].measurements) return; + sensor_z_offsets[tsi][calib_idx++] = static_cast((meas_z - init_measurement) * 1000.0f); } bool ProbeTempComp::finish_calibration(const TempSensorID tsi) { - if (tsi != TSI_PROBE && tsi != TSI_BED) return false; - - if (calib_idx < 3) { - SERIAL_ECHOLNPGM("!Insufficient measurements (min. 3)."); + if (!calib_idx) { + SERIAL_ECHOLNPGM("!No measurements."); clear_offsets(tsi); return false; } const uint8_t measurements = cali_info[tsi].measurements; - const float start_temp = cali_info[tsi].start_temp, - res_temp = cali_info[tsi].temp_res; + const celsius_t start_temp = cali_info[tsi].start_temp, + res_temp = cali_info[tsi].temp_resolution; int16_t * const data = sensor_z_offsets[tsi]; // Extrapolate float k, d; if (calib_idx < measurements) { - SERIAL_ECHOLNPAIR("Got ", calib_idx, " measurements. "); + SERIAL_ECHOLNPGM("Got ", calib_idx, " measurements. "); if (linear_regression(tsi, k, d)) { SERIAL_ECHOPGM("Applying linear extrapolation"); - calib_idx--; for (; calib_idx < measurements; ++calib_idx) { - const float temp = start_temp + float(calib_idx) * res_temp; + const celsius_float_t temp = start_temp + float(calib_idx + 1) * res_temp; data[calib_idx] = static_cast(k * temp + d); } } else { // Simply use the last measured value for higher temperatures SERIAL_ECHOPGM("Failed to extrapolate"); - const int16_t last_val = data[calib_idx]; + const int16_t last_val = data[calib_idx-1]; for (; calib_idx < measurements; ++calib_idx) data[calib_idx] = last_val; } @@ -143,15 +153,15 @@ bool ProbeTempComp::finish_calibration(const TempSensorID tsi) { // Sanity check for (calib_idx = 0; calib_idx < measurements; ++calib_idx) { // Restrict the max. offset - if (abs(data[calib_idx]) > 2000) { + if (ABS(data[calib_idx]) > 2000) { SERIAL_ECHOLNPGM("!Invalid Z-offset detected (0-2)."); clear_offsets(tsi); return false; } // Restrict the max. offset difference between two probings - if (calib_idx > 0 && abs(data[calib_idx - 1] - data[calib_idx]) > 800) { + if (calib_idx > 0 && ABS(data[calib_idx - 1] - data[calib_idx]) > 800) { SERIAL_ECHOLNPGM("!Invalid Z-offset between two probings detected (0-0.8)."); - clear_offsets(TSI_PROBE); + clear_offsets(tsi); return false; } } @@ -159,65 +169,63 @@ bool ProbeTempComp::finish_calibration(const TempSensorID tsi) { return true; } -void ProbeTempComp::compensate_measurement(const TempSensorID tsi, const float &temp, float &meas_z) { - if (WITHIN(temp, cali_info[tsi].start_temp, cali_info[tsi].end_temp)) - meas_z -= get_offset_for_temperature(tsi, temp); -} - -float ProbeTempComp::get_offset_for_temperature(const TempSensorID tsi, const float &temp) { +void ProbeTempComp::compensate_measurement(const TempSensorID tsi, const celsius_t temp, float &meas_z) { const uint8_t measurements = cali_info[tsi].measurements; - const float start_temp = cali_info[tsi].start_temp, - res_temp = cali_info[tsi].temp_res; + const celsius_t start_temp = cali_info[tsi].start_temp, + res_temp = cali_info[tsi].temp_resolution, + end_temp = start_temp + measurements * res_temp; const int16_t * const data = sensor_z_offsets[tsi]; - auto point = [&](uint8_t i) { - return xy_float_t({start_temp + i*res_temp, static_cast(data[i])}); + // Given a data index, return { celsius, zoffset } in the form { x, y } + auto tpoint = [&](uint8_t i) -> xy_float_t { + return xy_float_t({ static_cast(start_temp) + i * res_temp, i ? static_cast(data[i - 1]) : 0.0f }); }; - auto linear_interp = [](float x, xy_float_t p1, xy_float_t p2) { - return (p2.y - p1.y) / (p2.x - p2.y) * (x - p1.x) + p1.y; + // Interpolate Z based on a temperature being within a given range + auto linear_interp = [](const_float_t x, xy_float_t p1, xy_float_t p2) { + // zoffs1 + zoffset_per_toffset * toffset + return p1.y + (p2.y - p1.y) / (p2.x - p1.x) * (x - p1.x); }; - // Linear interpolation - uint8_t idx = static_cast((temp - start_temp) / res_temp); - - // offset in um + // offset in µm float offset = 0.0f; - #if !defined(PTC_LINEAR_EXTRAPOLATION) || PTC_LINEAR_EXTRAPOLATION <= 0 - if (idx < 0) + #if PTC_LINEAR_EXTRAPOLATION + if (temp < start_temp) + offset = linear_interp(temp, tpoint(0), tpoint(PTC_LINEAR_EXTRAPOLATION)); + else if (temp >= end_temp) + offset = linear_interp(temp, tpoint(measurements - PTC_LINEAR_EXTRAPOLATION), tpoint(measurements)); + #else + if (temp < start_temp) offset = 0.0f; - else if (idx > measurements - 2) + else if (temp >= end_temp) offset = static_cast(data[measurements - 1]); - #else - if (idx < 0) - offset = linear_interp(temp, point(0), point(PTC_LINEAR_EXTRAPOLATION)); - else if (idx > measurements - 2) - offset = linear_interp(temp, point(measurements - PTC_LINEAR_EXTRAPOLATION - 1), point(measurements - 1)); #endif - else - offset = linear_interp(temp, point(idx), point(idx + 1)); + else { + // Linear interpolation + const int8_t idx = static_cast((temp - start_temp) / res_temp); + offset = linear_interp(temp, tpoint(idx), tpoint(idx + 1)); + } - // return offset in mm - return offset / 1000.0f; + // convert offset to mm and apply it + meas_z -= offset / 1000.0f; } bool ProbeTempComp::linear_regression(const TempSensorID tsi, float &k, float &d) { - if (tsi != TSI_PROBE && tsi != TSI_BED) return false; - - if (!WITHIN(calib_idx, 2, cali_info[tsi].measurements)) return false; + if (!WITHIN(calib_idx, 1, cali_info[tsi].measurements)) return false; - const float start_temp = cali_info[tsi].start_temp, - res_temp = cali_info[tsi].temp_res; + const celsius_t start_temp = cali_info[tsi].start_temp, + res_temp = cali_info[tsi].temp_resolution; const int16_t * const data = sensor_z_offsets[tsi]; float sum_x = start_temp, sum_x2 = sq(start_temp), sum_xy = 0, sum_y = 0; + float xi = static_cast(start_temp); LOOP_L_N(i, calib_idx) { - const float xi = start_temp + (i + 1) * res_temp, - yi = static_cast(data[i]); + const float yi = static_cast(data[i]); + xi += res_temp; sum_x += xi; sum_x2 += sq(xi); sum_xy += xi * yi; @@ -237,4 +245,4 @@ bool ProbeTempComp::linear_regression(const TempSensorID tsi, float &k, float &d return true; } -#endif // PROBE_TEMP_COMPENSATION +#endif // HAS_PTC diff --git a/Marlin/src/feature/probe_temp_comp.h b/Marlin/src/feature/probe_temp_comp.h index 626dd87f945f..1db7d04e89dc 100644 --- a/Marlin/src/feature/probe_temp_comp.h +++ b/Marlin/src/feature/probe_temp_comp.h @@ -24,19 +24,22 @@ #include "../inc/MarlinConfig.h" enum TempSensorID : uint8_t { - TSI_PROBE, - TSI_BED, - #if ENABLED(USE_TEMP_EXT_COMPENSATION) + #if ENABLED(PTC_PROBE) + TSI_PROBE, + #endif + #if ENABLED(PTC_BED) + TSI_BED, + #endif + #if ENABLED(PTC_HOTEND) TSI_EXT, #endif TSI_COUNT }; typedef struct { - uint8_t measurements; // Max. number of measurements to be stored (35 - 80°C) - float temp_res, // Resolution in °C between measurements - start_temp, // Base measurement; z-offset == 0 - end_temp; + uint8_t measurements; // Max. number of measurements to be stored (35 - 80°C) + celsius_t temp_resolution, // Resolution in °C between measurements + start_temp; // Base measurement; z-offset == 0 } temp_calib_t; /** @@ -45,86 +48,47 @@ typedef struct { * measurement errors/shifts due to changed temperature. */ -// Probe temperature calibration constants -#ifndef PTC_SAMPLE_COUNT - #define PTC_SAMPLE_COUNT 10U -#endif -#ifndef PTC_SAMPLE_RES - #define PTC_SAMPLE_RES 5.0f -#endif -#ifndef PTC_SAMPLE_START - #define PTC_SAMPLE_START 30.0f -#endif -#define PTC_SAMPLE_END ((PTC_SAMPLE_START) + (PTC_SAMPLE_COUNT) * (PTC_SAMPLE_RES)) - -// Bed temperature calibration constants -#ifndef BTC_PROBE_TEMP - #define BTC_PROBE_TEMP 30.0f -#endif -#ifndef BTC_SAMPLE_COUNT - #define BTC_SAMPLE_COUNT 10U -#endif -#ifndef BTC_SAMPLE_STEP - #define BTC_SAMPLE_RES 5.0f -#endif -#ifndef BTC_SAMPLE_START - #define BTC_SAMPLE_START 60.0f -#endif -#define BTC_SAMPLE_END ((BTC_SAMPLE_START) + (BTC_SAMPLE_COUNT) * (BTC_SAMPLE_RES)) - -#ifndef PTC_PROBE_HEATING_OFFSET - #define PTC_PROBE_HEATING_OFFSET 0.5f -#endif - -#ifndef PTC_PROBE_RAISE - #define PTC_PROBE_RAISE 10.0f -#endif - -static constexpr temp_calib_t cali_info_init[TSI_COUNT] = { - { PTC_SAMPLE_COUNT, PTC_SAMPLE_RES, PTC_SAMPLE_START, PTC_SAMPLE_END }, // Probe - { BTC_SAMPLE_COUNT, BTC_SAMPLE_RES, BTC_SAMPLE_START, BTC_SAMPLE_END }, // Bed - #if ENABLED(USE_TEMP_EXT_COMPENSATION) - { 20, 5, 180, 180 + 5 * 20 } // Extruder - #endif -}; - class ProbeTempComp { public: - static const temp_calib_t cali_info[TSI_COUNT]; - - // Where to park nozzle to wait for probe cooldown - static constexpr xyz_pos_t park_point = PTC_PARK_POS; - - // XY coordinates of nozzle for probing the bed - static constexpr xy_pos_t measure_point = PTC_PROBE_POS; // Coordinates to probe - //measure_point = { 12.0f, 7.3f }; // Coordinates for the MK52 magnetic heatbed - - static constexpr int probe_calib_bed_temp = BED_MAX_TARGET, // Bed temperature while calibrating probe - bed_calib_probe_temp = BTC_PROBE_TEMP; // Probe temperature while calibrating bed - - static int16_t *sensor_z_offsets[TSI_COUNT], - z_offsets_probe[cali_info_init[TSI_PROBE].measurements], // (µm) - z_offsets_bed[cali_info_init[TSI_BED].measurements]; // (µm) - - #if ENABLED(USE_TEMP_EXT_COMPENSATION) - static int16_t z_offsets_ext[cali_info_init[TSI_EXT].measurements]; // (µm) + static constexpr temp_calib_t cali_info[TSI_COUNT] = { + #if ENABLED(PTC_PROBE) + { PTC_PROBE_COUNT, PTC_PROBE_RES, PTC_PROBE_START }, // Probe + #endif + #if ENABLED(PTC_BED) + { PTC_BED_COUNT, PTC_BED_RES, PTC_BED_START }, // Bed + #endif + #if ENABLED(PTC_HOTEND) + { PTC_HOTEND_COUNT, PTC_HOTEND_RES, PTC_HOTEND_START }, // Extruder + #endif + }; + + static int16_t *sensor_z_offsets[TSI_COUNT]; + #if ENABLED(PTC_PROBE) + static int16_t z_offsets_probe[PTC_PROBE_COUNT]; // (µm) + #endif + #if ENABLED(PTC_BED) + static int16_t z_offsets_bed[PTC_BED_COUNT]; // (µm) + #endif + #if ENABLED(PTC_HOTEND) + static int16_t z_offsets_hotend[PTC_HOTEND_COUNT]; // (µm) #endif - static inline void reset_index() { calib_idx = 0; }; - static inline uint8_t get_index() { return calib_idx; } + static void reset_index() { calib_idx = 0; }; + static uint8_t get_index() { return calib_idx; } + static void reset(); static void clear_offsets(const TempSensorID tsi); - static inline void clear_all_offsets() { - clear_offsets(TSI_BED); - clear_offsets(TSI_PROBE); - TERN_(USE_TEMP_EXT_COMPENSATION, clear_offsets(TSI_EXT)); + static void clear_all_offsets() { + TERN_(PTC_PROBE, clear_offsets(TSI_PROBE)); + TERN_(PTC_BED, clear_offsets(TSI_BED)); + TERN_(PTC_HOTEND, clear_offsets(TSI_EXT)); } static bool set_offset(const TempSensorID tsi, const uint8_t idx, const int16_t offset); static void print_offsets(); - static void prepare_new_calibration(const float &init_meas_z); - static void push_back_new_measurement(const TempSensorID tsi, const float &meas_z); + static void prepare_new_calibration(const_float_t init_meas_z); + static void push_back_new_measurement(const TempSensorID tsi, const_float_t meas_z); static bool finish_calibration(const TempSensorID tsi); - static void compensate_measurement(const TempSensorID tsi, const float &temp, float &meas_z); + static void compensate_measurement(const TempSensorID tsi, const celsius_t temp, float &meas_z); private: static uint8_t calib_idx; @@ -135,8 +99,6 @@ class ProbeTempComp { */ static float init_measurement; - static float get_offset_for_temperature(const TempSensorID tsi, const float &temp); - /** * Fit a linear function in measured temperature offsets * to allow generating values of higher temperatures. @@ -144,4 +106,4 @@ class ProbeTempComp { static bool linear_regression(const TempSensorID tsi, float &k, float &d); }; -extern ProbeTempComp temp_comp; +extern ProbeTempComp ptc; diff --git a/Marlin/src/feature/repeat.cpp b/Marlin/src/feature/repeat.cpp index 11e4dd6a93bb..165f71fd0fae 100644 --- a/Marlin/src/feature/repeat.cpp +++ b/Marlin/src/feature/repeat.cpp @@ -19,6 +19,7 @@ * along with this program. If not, see . * */ + #include "../inc/MarlinConfig.h" #if ENABLED(GCODE_REPEAT_MARKERS) @@ -43,7 +44,7 @@ void Repeat::add_marker(const uint32_t sdpos, const uint16_t count) { marker[index].sdpos = sdpos; marker[index].counter = count ?: -1; index++; - DEBUG_ECHOLNPAIR("Add Marker ", index, " at ", sdpos, " (", count, ")"); + DEBUG_ECHOLNPGM("Add Marker ", index, " at ", sdpos, " (", count, ")"); } } @@ -53,14 +54,14 @@ void Repeat::loop() { else { const uint8_t ind = index - 1; // Active marker's index if (!marker[ind].counter) { // Did its counter run out? - DEBUG_ECHOLNPAIR("Pass Marker ", index); + DEBUG_ECHOLNPGM("Pass Marker ", index); index--; // Carry on. Previous marker on the next 'M808'. } else { card.setIndex(marker[ind].sdpos); // Loop back to the marker. if (marker[ind].counter > 0) // Ignore a negative (or zero) counter. --marker[ind].counter; // Decrement the counter. If zero this 'M808' will be skipped next time. - DEBUG_ECHOLNPAIR("Goto Marker ", index, " at ", marker[ind].sdpos, " (", marker[ind].counter, ")"); + DEBUG_ECHOLNPGM("Goto Marker ", index, " at ", marker[ind].sdpos, " (", marker[ind].counter, ")"); } } } @@ -69,7 +70,7 @@ void Repeat::cancel() { LOOP_L_N(i, index) marker[i].counter = 0; } void Repeat::early_parse_M808(char * const cmd) { if (is_command_M808(cmd)) { - DEBUG_ECHOLNPAIR("Parsing \"", cmd, "\""); + DEBUG_ECHOLNPGM("Parsing \"", cmd, "\""); parser.parse(cmd); if (parser.seen('L')) add_marker(card.getIndex(), parser.value_ushort()); diff --git a/Marlin/src/feature/repeat.h b/Marlin/src/feature/repeat.h index 0f4d9425b768..fc11e4a9e2cf 100644 --- a/Marlin/src/feature/repeat.h +++ b/Marlin/src/feature/repeat.h @@ -38,8 +38,8 @@ class Repeat { static repeat_marker_t marker[MAX_REPEAT_NESTING]; static uint8_t index; public: - static inline void reset() { index = 0; } - static inline bool is_active() { + static void reset() { index = 0; } + static bool is_active() { LOOP_L_N(i, index) if (marker[i].counter) return true; return false; } diff --git a/Marlin/src/feature/runout.cpp b/Marlin/src/feature/runout.cpp index 531ca1081f0b..8b78b53848f3 100644 --- a/Marlin/src/feature/runout.cpp +++ b/Marlin/src/feature/runout.cpp @@ -68,6 +68,8 @@ bool FilamentMonitorBase::enabled = true, #if ENABLED(EXTENSIBLE_UI) #include "../lcd/extui/ui_api.h" +#elif ENABLED(DWIN_CREALITY_LCD_ENHANCED) + #include "../lcd/e3v2/proui/dwin.h" #endif void event_filament_runout(const uint8_t extruder) { @@ -86,6 +88,7 @@ void event_filament_runout(const uint8_t extruder) { #endif TERN_(EXTENSIBLE_UI, ExtUI::onFilamentRunout(ExtUI::getTool(extruder))); + TERN_(DWIN_CREALITY_LCD_ENHANCED, DWIN_FilamentRunout(extruder)); #if ANY(HOST_PROMPT_SUPPORT, HOST_ACTION_COMMANDS, MULTI_FILAMENT_SENSOR) const char tool = '0' + TERN0(MULTI_FILAMENT_SENSOR, extruder); @@ -93,8 +96,7 @@ void event_filament_runout(const uint8_t extruder) { //action:out_of_filament #if ENABLED(HOST_PROMPT_SUPPORT) - host_action_prompt_begin(PROMPT_FILAMENT_RUNOUT, PSTR("FilamentRunout T"), tool); - host_action_prompt_show(); + hostui.prompt_do(PROMPT_FILAMENT_RUNOUT, F("FilamentRunout T"), tool); //action:out_of_filament #endif const bool run_runout_script = !runout.host_handling; @@ -106,18 +108,18 @@ void event_filament_runout(const uint8_t extruder) { || TERN0(ADVANCED_PAUSE_FEATURE, strstr(FILAMENT_RUNOUT_SCRIPT, "M25")) ) ) { - host_action_paused(false); + hostui.paused(false); } else { // Legacy Repetier command for use until newer version supports standard dialog // To be removed later when pause command also triggers dialog #ifdef ACTION_ON_FILAMENT_RUNOUT - host_action(PSTR(ACTION_ON_FILAMENT_RUNOUT " T"), false); + hostui.action(F(ACTION_ON_FILAMENT_RUNOUT " T"), false); SERIAL_CHAR(tool); SERIAL_EOL(); #endif - host_action_pause(false); + hostui.pause(false); } SERIAL_ECHOPGM(" " ACTION_REASON_ON_FILAMENT_RUNOUT " "); SERIAL_CHAR(tool); @@ -129,7 +131,7 @@ void event_filament_runout(const uint8_t extruder) { char script[strlen(FILAMENT_RUNOUT_SCRIPT) + 1]; sprintf_P(script, PSTR(FILAMENT_RUNOUT_SCRIPT), tool); #if ENABLED(FILAMENT_RUNOUT_SENSOR_DEBUG) - SERIAL_ECHOLNPAIR("Runout Command: ", script); + SERIAL_ECHOLNPGM("Runout Command: ", script); #endif queue.inject(script); #else @@ -137,7 +139,7 @@ void event_filament_runout(const uint8_t extruder) { SERIAL_ECHOPGM("Runout Command: "); SERIAL_ECHOLNPGM(FILAMENT_RUNOUT_SCRIPT); #endif - queue.inject_P(PSTR(FILAMENT_RUNOUT_SCRIPT)); + queue.inject(F(FILAMENT_RUNOUT_SCRIPT)); #endif } } diff --git a/Marlin/src/feature/runout.h b/Marlin/src/feature/runout.h index 34ae67899bc6..e74d857a79ed 100644 --- a/Marlin/src/feature/runout.h +++ b/Marlin/src/feature/runout.h @@ -83,30 +83,30 @@ class TFilamentMonitor : public FilamentMonitorBase { static sensor_t sensor; public: - static inline void setup() { + static void setup() { sensor.setup(); reset(); } - static inline void reset() { + static void reset() { filament_ran_out = false; response.reset(); } // Call this method when filament is present, // so the response can reset its counter. - static inline void filament_present(const uint8_t extruder) { + static void filament_present(const uint8_t extruder) { response.filament_present(extruder); } #if HAS_FILAMENT_RUNOUT_DISTANCE - static inline float& runout_distance() { return response.runout_distance_mm; } - static inline void set_runout_distance(const float &mm) { response.runout_distance_mm = mm; } + static float& runout_distance() { return response.runout_distance_mm; } + static void set_runout_distance(const_float_t mm) { response.runout_distance_mm = mm; } #endif // Handle a block completion. RunoutResponseDelayed uses this to // add up the length of filament moved while the filament is out. - static inline void block_completed(const block_t* const b) { + static void block_completed(const block_t * const b) { if (enabled) { response.block_completed(b); sensor.block_completed(b); @@ -114,7 +114,7 @@ class TFilamentMonitor : public FilamentMonitorBase { } // Give the response a chance to update its counter. - static inline void run() { + static void run() { if (enabled && !filament_ran_out && (printingIsActive() || did_pause_print)) { TERN_(HAS_FILAMENT_RUNOUT_DISTANCE, cli()); // Prevent RunoutResponseDelayed::block_completed from accumulating here response.run(); @@ -145,7 +145,7 @@ class TFilamentMonitor : public FilamentMonitorBase { if (runout_flags) { SERIAL_ECHOPGM("Runout Sensors: "); LOOP_L_N(i, 8) SERIAL_ECHO('0' + TEST(runout_flags, i)); - SERIAL_ECHOPAIR(" -> ", extruder); + SERIAL_ECHOPGM(" -> ", extruder); if (ran_out) SERIAL_ECHOPGM(" RUN OUT"); SERIAL_EOL(); } @@ -168,12 +168,12 @@ class FilamentSensorBase { * Called by FilamentSensorSwitch::run when filament is detected. * Called by FilamentSensorEncoder::block_completed when motion is detected. */ - static inline void filament_present(const uint8_t extruder) { + static void filament_present(const uint8_t extruder) { runout.filament_present(extruder); // ...which calls response.filament_present(extruder) } public: - static inline void setup() { + static void setup() { #define _INIT_RUNOUT_PIN(P,S,U,D) do{ if (ENABLED(U)) SET_INPUT_PULLUP(P); else if (ENABLED(D)) SET_INPUT_PULLDOWN(P); else SET_INPUT(P); }while(0) #define INIT_RUNOUT_PIN(N) _INIT_RUNOUT_PIN(FIL_RUNOUT##N##_PIN, FIL_RUNOUT##N##_STATE, FIL_RUNOUT##N##_PULLUP, FIL_RUNOUT##N##_PULLDOWN) #if NUM_RUNOUT_SENSORS >= 1 @@ -205,14 +205,14 @@ class FilamentSensorBase { } // Return a bitmask of runout pin states - static inline uint8_t poll_runout_pins() { + static uint8_t poll_runout_pins() { #define _OR_RUNOUT(N) | (READ(FIL_RUNOUT##N##_PIN) ? _BV((N) - 1) : 0) - return (0 REPEAT_S(1, INCREMENT(NUM_RUNOUT_SENSORS), _OR_RUNOUT)); + return (0 REPEAT_1(NUM_RUNOUT_SENSORS, _OR_RUNOUT)); #undef _OR_RUNOUT } // Return a bitmask of runout flag states (1 bits always indicates runout) - static inline uint8_t poll_runout_states() { + static uint8_t poll_runout_states() { return poll_runout_pins() ^ uint8_t(0 #if NUM_RUNOUT_SENSORS >= 1 | (FIL_RUNOUT1_STATE ? 0 : _BV(1 - 1)) @@ -254,7 +254,7 @@ class FilamentSensorBase { private: static uint8_t motion_detected; - static inline void poll_motion_sensor() { + static void poll_motion_sensor() { static uint8_t old_state; const uint8_t new_state = poll_runout_pins(), change = old_state ^ new_state; @@ -273,7 +273,7 @@ class FilamentSensorBase { } public: - static inline void block_completed(const block_t* const b) { + static void block_completed(const block_t * const b) { // If the sensor wheel has moved since the last call to // this method reset the runout counter for the extruder. if (TEST(motion_detected, b->extruder)) @@ -283,7 +283,7 @@ class FilamentSensorBase { motion_detected = 0; } - static inline void run() { poll_motion_sensor(); } + static void run() { poll_motion_sensor(); } }; #else @@ -294,7 +294,7 @@ class FilamentSensorBase { */ class FilamentSensorSwitch : public FilamentSensorBase { private: - static inline bool poll_runout_state(const uint8_t extruder) { + static bool poll_runout_state(const uint8_t extruder) { const uint8_t runout_states = poll_runout_states(); #if MULTI_FILAMENT_SENSOR if ( !TERN0(DUAL_X_CARRIAGE, idex_is_duplicating()) @@ -307,9 +307,9 @@ class FilamentSensorBase { } public: - static inline void block_completed(const block_t* const) {} + static void block_completed(const block_t * const) {} - static inline void run() { + static void run() { LOOP_L_N(s, NUM_RUNOUT_SENSORS) { const bool out = poll_runout_state(s); if (!out) filament_present(s); @@ -317,7 +317,7 @@ class FilamentSensorBase { static uint8_t was_out; // = 0 if (out != TEST(was_out, s)) { TBI(was_out, s); - SERIAL_ECHOLNPAIR_P(PSTR("Filament Sensor "), '0' + s, out ? PSTR(" OUT") : PSTR(" IN")); + SERIAL_ECHOLNF(F("Filament Sensor "), AS_DIGIT(s), out ? F(" OUT") : F(" IN")); } #endif } @@ -341,39 +341,39 @@ class FilamentSensorBase { public: static float runout_distance_mm; - static inline void reset() { + static void reset() { LOOP_L_N(i, NUM_RUNOUT_SENSORS) filament_present(i); } - static inline void run() { + static void run() { #if ENABLED(FILAMENT_RUNOUT_SENSOR_DEBUG) static millis_t t = 0; const millis_t ms = millis(); if (ELAPSED(ms, t)) { t = millis() + 1000UL; LOOP_L_N(i, NUM_RUNOUT_SENSORS) - SERIAL_ECHOPAIR_P(i ? PSTR(", ") : PSTR("Remaining mm: "), runout_mm_countdown[i]); + SERIAL_ECHOF(i ? F(", ") : F("Remaining mm: "), runout_mm_countdown[i]); SERIAL_EOL(); } #endif } - static inline uint8_t has_run_out() { + static uint8_t has_run_out() { uint8_t runout_flags = 0; LOOP_L_N(i, NUM_RUNOUT_SENSORS) if (runout_mm_countdown[i] < 0) SBI(runout_flags, i); return runout_flags; } - static inline void filament_present(const uint8_t extruder) { + static void filament_present(const uint8_t extruder) { runout_mm_countdown[extruder] = runout_distance_mm; } - static inline void block_completed(const block_t* const b) { + static void block_completed(const block_t * const b) { if (b->steps.x || b->steps.y || b->steps.z || did_pause_print) { // Allow pause purge move to re-trigger runout state // Only trigger on extrusion with XYZ movement to allow filament change and retract/recover. const uint8_t e = b->extruder; const int32_t steps = b->steps.e; - runout_mm_countdown[e] -= (TEST(b->direction_bits, E_AXIS) ? -steps : steps) * planner.steps_to_mm[E_AXIS_N(e)]; + runout_mm_countdown[e] -= (TEST(b->direction_bits, E_AXIS) ? -steps : steps) * planner.mm_per_step[E_AXIS_N(e)]; } } }; @@ -389,23 +389,23 @@ class FilamentSensorBase { static int8_t runout_count[NUM_RUNOUT_SENSORS]; public: - static inline void reset() { + static void reset() { LOOP_L_N(i, NUM_RUNOUT_SENSORS) filament_present(i); } - static inline void run() { + static void run() { LOOP_L_N(i, NUM_RUNOUT_SENSORS) if (runout_count[i] >= 0) runout_count[i]--; } - static inline uint8_t has_run_out() { + static uint8_t has_run_out() { uint8_t runout_flags = 0; LOOP_L_N(i, NUM_RUNOUT_SENSORS) if (runout_count[i] < 0) SBI(runout_flags, i); return runout_flags; } - static inline void block_completed(const block_t* const) { } + static void block_completed(const block_t * const) { } - static inline void filament_present(const uint8_t extruder) { + static void filament_present(const uint8_t extruder) { runout_count[extruder] = runout_threshold; } }; diff --git a/Marlin/src/feature/solenoid.cpp b/Marlin/src/feature/solenoid.cpp index 623f223caa1b..b6795d1a1ef8 100644 --- a/Marlin/src/feature/solenoid.cpp +++ b/Marlin/src/feature/solenoid.cpp @@ -34,28 +34,12 @@ #include "../module/tool_change.h" #endif -#define HAS_SOLENOID(N) (HAS_SOLENOID_##N && (ENABLED(MANUAL_SOLENOID_CONTROL) || N < EXTRUDERS)) - // Used primarily with MANUAL_SOLENOID_CONTROL static void set_solenoid(const uint8_t num, const bool active) { const uint8_t value = active ? PE_MAGNET_ON_STATE : !PE_MAGNET_ON_STATE; + #define _SOL_CASE(N) case N: TERN_(HAS_SOLENOID_##N, OUT_WRITE(SOL##N##_PIN, value)); break; switch (num) { - case 0: OUT_WRITE(SOL0_PIN, value); break; - #if HAS_SOLENOID(1) - case 1: OUT_WRITE(SOL1_PIN, value); break; - #endif - #if HAS_SOLENOID(2) - case 2: OUT_WRITE(SOL2_PIN, value); break; - #endif - #if HAS_SOLENOID(3) - case 3: OUT_WRITE(SOL3_PIN, value); break; - #endif - #if HAS_SOLENOID(4) - case 4: OUT_WRITE(SOL4_PIN, value); break; - #endif - #if HAS_SOLENOID(5) - case 5: OUT_WRITE(SOL5_PIN, value); break; - #endif + REPEAT(8, _SOL_CASE) default: SERIAL_ECHO_MSG(STR_INVALID_SOLENOID); break; } @@ -67,25 +51,11 @@ static void set_solenoid(const uint8_t num, const bool active) { void enable_solenoid(const uint8_t num) { set_solenoid(num, true); } void disable_solenoid(const uint8_t num) { set_solenoid(num, false); } -void enable_solenoid_on_active_extruder() { enable_solenoid(active_extruder); } +void enable_solenoid_on_active_extruder() { } void disable_all_solenoids() { - disable_solenoid(0); - #if HAS_SOLENOID(1) - disable_solenoid(1); - #endif - #if HAS_SOLENOID(2) - disable_solenoid(2); - #endif - #if HAS_SOLENOID(3) - disable_solenoid(3); - #endif - #if HAS_SOLENOID(4) - disable_solenoid(4); - #endif - #if HAS_SOLENOID(5) - disable_solenoid(5); - #endif + #define _SOL_DISABLE(N) TERN_(HAS_SOLENOID_##N, disable_solenoid(N)); + REPEAT(8, _SOL_DISABLE) } #endif // EXT_SOLENOID || MANUAL_SOLENOID_CONTROL diff --git a/Marlin/src/feature/spindle_laser.cpp b/Marlin/src/feature/spindle_laser.cpp index 66c04a001c0a..52bb471b0f7e 100644 --- a/Marlin/src/feature/spindle_laser.cpp +++ b/Marlin/src/feature/spindle_laser.cpp @@ -34,6 +34,10 @@ #include "../module/servo.h" #endif +#if ENABLED(I2C_AMMETER) + #include "../feature/ammeter.h" +#endif + SpindleLaser cutter; uint8_t SpindleLaser::power; #if ENABLED(LASER_FEATURE) @@ -48,9 +52,9 @@ cutter_power_t SpindleLaser::menuPower, // Power s #endif #define SPINDLE_LASER_PWM_OFF TERN(SPINDLE_LASER_PWM_INVERT, 255, 0) -// -// Init the cutter to a safe OFF state -// +/** + * Init the cutter to a safe OFF state + */ void SpindleLaser::init() { #if ENABLED(SPINDLE_SERVO) MOVE_SERVO(SPINDLE_SERVO_NR, SPINDLE_SERVO_MIN); @@ -58,29 +62,36 @@ void SpindleLaser::init() { OUT_WRITE(SPINDLE_LASER_ENA_PIN, !SPINDLE_LASER_ACTIVE_STATE); // Init spindle to off #endif #if ENABLED(SPINDLE_CHANGE_DIR) - OUT_WRITE(SPINDLE_DIR_PIN, SPINDLE_INVERT_DIR ? 255 : 0); // Init rotation to clockwise (M3) + OUT_WRITE(SPINDLE_DIR_PIN, SPINDLE_INVERT_DIR); // Init rotation to clockwise (M3) #endif - #if ENABLED(SPINDLE_LASER_PWM) + #if ENABLED(SPINDLE_LASER_USE_PWM) SET_PWM(SPINDLE_LASER_PWM_PIN); - analogWrite(pin_t(SPINDLE_LASER_PWM_PIN), SPINDLE_LASER_PWM_OFF); // Set to lowest speed + hal.set_pwm_duty(pin_t(SPINDLE_LASER_PWM_PIN), SPINDLE_LASER_PWM_OFF); // Set to lowest speed #endif - #if ENABLED(HAL_CAN_SET_PWM_FREQ) && defined(SPINDLE_LASER_FREQUENCY) - set_pwm_frequency(pin_t(SPINDLE_LASER_PWM_PIN), SPINDLE_LASER_FREQUENCY); + #if ENABLED(HAL_CAN_SET_PWM_FREQ) && SPINDLE_LASER_FREQUENCY + hal.set_pwm_frequency(pin_t(SPINDLE_LASER_PWM_PIN), SPINDLE_LASER_FREQUENCY); TERN_(MARLIN_DEV_MODE, frequency = SPINDLE_LASER_FREQUENCY); #endif + #if ENABLED(AIR_EVACUATION) + OUT_WRITE(AIR_EVACUATION_PIN, !AIR_EVACUATION_ACTIVE); // Init Vacuum/Blower OFF + #endif + #if ENABLED(AIR_ASSIST) + OUT_WRITE(AIR_ASSIST_PIN, !AIR_ASSIST_ACTIVE); // Init Air Assist OFF + #endif + TERN_(I2C_AMMETER, ammeter.init()); // Init I2C Ammeter } -#if ENABLED(SPINDLE_LASER_PWM) +#if ENABLED(SPINDLE_LASER_USE_PWM) /** * Set the cutter PWM directly to the given ocr value + * + * @param ocr Power value */ void SpindleLaser::_set_ocr(const uint8_t ocr) { - #if NEEDS_HARDWARE_PWM && SPINDLE_LASER_FREQUENCY - set_pwm_frequency(pin_t(SPINDLE_LASER_PWM_PIN), TERN(MARLIN_DEV_MODE, frequency, SPINDLE_LASER_FREQUENCY)); - set_pwm_duty(pin_t(SPINDLE_LASER_PWM_PIN), ocr ^ SPINDLE_LASER_PWM_OFF); - #else - analogWrite(pin_t(SPINDLE_LASER_PWM_PIN), ocr ^ SPINDLE_LASER_PWM_OFF); + #if ENABLED(HAL_CAN_SET_PWM_FREQ) && SPINDLE_LASER_FREQUENCY + hal.set_pwm_frequency(pin_t(SPINDLE_LASER_PWM_PIN), TERN(MARLIN_DEV_MODE, frequency, SPINDLE_LASER_FREQUENCY)); #endif + hal.set_pwm_duty(pin_t(SPINDLE_LASER_PWM_PIN), ocr ^ SPINDLE_LASER_PWM_OFF); } void SpindleLaser::set_ocr(const uint8_t ocr) { @@ -92,17 +103,21 @@ void SpindleLaser::init() { WRITE(SPINDLE_LASER_ENA_PIN, !SPINDLE_LASER_ACTIVE_STATE); // Cutter OFF _set_ocr(0); } -#endif +#endif // SPINDLE_LASER_USE_PWM -// -// Set cutter ON/OFF state (and PWM) to the given cutter power value -// +/** + * Apply power for laser/spindle + * + * Apply cutter power value for PWM, Servo, and on/off pin. + * + * @param opwr Power value. Range 0 to MAX. When 0 disable spindle/laser. + */ void SpindleLaser::apply_power(const uint8_t opwr) { static uint8_t last_power_applied = 0; if (opwr == last_power_applied) return; last_power_applied = opwr; power = opwr; - #if ENABLED(SPINDLE_LASER_PWM) + #if ENABLED(SPINDLE_LASER_USE_PWM) if (cutter.unitPower == 0 && CUTTER_UNIT_IS(RPM)) { ocr_off(); isReady = false; @@ -124,10 +139,10 @@ void SpindleLaser::apply_power(const uint8_t opwr) { } #if ENABLED(SPINDLE_CHANGE_DIR) - // - // Set the spindle direction and apply immediately - // Stop on direction change if SPINDLE_STOP_ON_DIR_CHANGE is enabled - // + /** + * Set the spindle direction and apply immediately + * Stop on direction change if SPINDLE_STOP_ON_DIR_CHANGE is enabled + */ void SpindleLaser::set_reverse(const bool reverse) { const bool dir_state = (reverse == SPINDLE_INVERT_DIR); // Forward (M3) HIGH when not inverted if (TERN0(SPINDLE_STOP_ON_DIR_CHANGE, enabled()) && READ(SPINDLE_DIR_PIN) != dir_state) disable(); @@ -135,4 +150,18 @@ void SpindleLaser::apply_power(const uint8_t opwr) { } #endif +#if ENABLED(AIR_EVACUATION) + // Enable / disable Cutter Vacuum or Laser Blower motor + void SpindleLaser::air_evac_enable() { WRITE(AIR_EVACUATION_PIN, AIR_EVACUATION_ACTIVE); } // Turn ON + void SpindleLaser::air_evac_disable() { WRITE(AIR_EVACUATION_PIN, !AIR_EVACUATION_ACTIVE); } // Turn OFF + void SpindleLaser::air_evac_toggle() { TOGGLE(AIR_EVACUATION_PIN); } // Toggle state +#endif + +#if ENABLED(AIR_ASSIST) + // Enable / disable air assist + void SpindleLaser::air_assist_enable() { WRITE(AIR_ASSIST_PIN, AIR_ASSIST_PIN); } // Turn ON + void SpindleLaser::air_assist_disable() { WRITE(AIR_ASSIST_PIN, !AIR_ASSIST_PIN); } // Turn OFF + void SpindleLaser::air_assist_toggle() { TOGGLE(AIR_ASSIST_PIN); } // Toggle state +#endif + #endif // HAS_CUTTER diff --git a/Marlin/src/feature/spindle_laser.h b/Marlin/src/feature/spindle_laser.h index d50bc7eb42d2..e948d2d37b73 100644 --- a/Marlin/src/feature/spindle_laser.h +++ b/Marlin/src/feature/spindle_laser.h @@ -41,19 +41,11 @@ #define PCT_TO_PWM(X) ((X) * 255 / 100) #define PCT_TO_SERVO(X) ((X) * 180 / 100) -#ifndef SPEED_POWER_INTERCEPT - #define SPEED_POWER_INTERCEPT 0 -#endif - // #define _MAP(N,S1,S2,D1,D2) ((N)*_MAX((D2)-(D1),0)/_MAX((S2)-(S1),1)+(D1)) class SpindleLaser { public: - static constexpr float - min_pct = TERN(CUTTER_POWER_RELATIVE, 0, TERN(SPINDLE_FEATURE, round(100.0f * (SPEED_POWER_MIN) / (SPEED_POWER_MAX)), SPEED_POWER_MIN)), - max_pct = TERN(SPINDLE_FEATURE, 100, SPEED_POWER_MAX); - - static const inline uint8_t pct_to_ocr(const float pct) { return uint8_t(PCT_TO_PWM(pct)); } + static const inline uint8_t pct_to_ocr(const_float_t pct) { return uint8_t(PCT_TO_PWM(pct)); } // cpower = configured values (e.g., SPEED_POWER_MAX) @@ -111,19 +103,19 @@ class SpindleLaser { static void init(); #if ENABLED(MARLIN_DEV_MODE) - static inline void refresh_frequency() { set_pwm_frequency(pin_t(SPINDLE_LASER_PWM_PIN), frequency); } + static void refresh_frequency() { hal.set_pwm_frequency(pin_t(SPINDLE_LASER_PWM_PIN), frequency); } #endif // Modifying this function should update everywhere - static inline bool enabled(const cutter_power_t opwr) { return opwr > 0; } - static inline bool enabled() { return enabled(power); } + static bool enabled(const cutter_power_t opwr) { return opwr > 0; } + static bool enabled() { return enabled(power); } static void apply_power(const uint8_t inpow); FORCE_INLINE static void refresh() { apply_power(power); } FORCE_INLINE static void set_power(const uint8_t upwr) { power = upwr; refresh(); } - #if ENABLED(SPINDLE_LASER_PWM) + #if ENABLED(SPINDLE_LASER_USE_PWM) private: @@ -132,54 +124,53 @@ class SpindleLaser { public: static void set_ocr(const uint8_t ocr); - static inline void set_ocr_power(const uint8_t ocr) { power = ocr; set_ocr(ocr); } + static void ocr_set_power(const uint8_t ocr) { power = ocr; set_ocr(ocr); } static void ocr_off(); - // Used to update output for power->OCR translation - static inline uint8_t upower_to_ocr(const cutter_power_t upwr) { - return ( + + /** + * Update output for power->OCR translation + */ + static uint8_t upower_to_ocr(const cutter_power_t upwr) { + return uint8_t( #if CUTTER_UNIT_IS(PWM255) - uint8_t(upwr) + upwr #elif CUTTER_UNIT_IS(PERCENT) pct_to_ocr(upwr) #else - uint8_t(pct_to_ocr(cpwr_to_pct(upwr))) + pct_to_ocr(cpwr_to_pct(upwr)) #endif ); } - // Correct power to configured range - static inline cutter_power_t power_to_range(const cutter_power_t pwr) { - return power_to_range(pwr, ( - #if CUTTER_UNIT_IS(PWM255) - 0 - #elif CUTTER_UNIT_IS(PERCENT) - 1 - #elif CUTTER_UNIT_IS(RPM) - 2 - #else - #error "CUTTER_UNIT_IS(unknown)" - #endif - )); + /** + * Correct power to configured range + */ + static cutter_power_t power_to_range(const cutter_power_t pwr) { + return power_to_range(pwr, _CUTTER_POWER(CUTTER_POWER_UNIT)); } - static inline cutter_power_t power_to_range(const cutter_power_t pwr, const uint8_t pwrUnit) { + + static cutter_power_t power_to_range(const cutter_power_t pwr, const uint8_t pwrUnit) { + static constexpr float + min_pct = TERN(CUTTER_POWER_RELATIVE, 0, TERN(SPINDLE_FEATURE, round(100.0f * (SPEED_POWER_MIN) / (SPEED_POWER_MAX)), SPEED_POWER_MIN)), + max_pct = TERN(SPINDLE_FEATURE, 100, SPEED_POWER_MAX); if (pwr <= 0) return 0; cutter_power_t upwr; switch (pwrUnit) { - case 0: // PWM + case _CUTTER_POWER_PWM255: upwr = cutter_power_t( (pwr < pct_to_ocr(min_pct)) ? pct_to_ocr(min_pct) // Use minimum if set below : (pwr > pct_to_ocr(max_pct)) ? pct_to_ocr(max_pct) // Use maximum if set above : pwr ); break; - case 1: // PERCENT + case _CUTTER_POWER_PERCENT: upwr = cutter_power_t( (pwr < min_pct) ? min_pct // Use minimum if set below : (pwr > max_pct) ? max_pct // Use maximum if set above : pwr // PCT ); break; - case 2: // RPM + case _CUTTER_POWER_RPM: upwr = cutter_power_t( (pwr < SPEED_POWER_MIN) ? SPEED_POWER_MIN // Use minimum if set below : (pwr > SPEED_POWER_MAX) ? SPEED_POWER_MAX // Use maximum if set above @@ -191,14 +182,35 @@ class SpindleLaser { return upwr; } - #endif // SPINDLE_LASER_PWM - - static inline void set_enabled(const bool enable) { - set_power(enable ? TERN(SPINDLE_LASER_PWM, (power ?: (unitPower ? upower_to_ocr(cpwr_to_upwr(SPEED_POWER_STARTUP)) : 0)), 255) : 0); + #endif // SPINDLE_LASER_USE_PWM + + /** + * Enable/Disable spindle/laser + * @param enable true = enable; false = disable + */ + static void set_enabled(const bool enable) { + uint8_t value = 0; + if (enable) { + #if ENABLED(SPINDLE_LASER_USE_PWM) + if (power) + value = power; + else if (unitPower) + value = upower_to_ocr(cpwr_to_upwr(SPEED_POWER_STARTUP)); + #else + value = 255; + #endif + } + set_power(value); } - // Wait for spindle to spin up or spin down - static inline void power_delay(const bool on) { + static void disable() { isReady = false; set_enabled(false); } + + /** + * Wait for spindle to spin up or spin down + * + * @param on true = state to on; false = state to off. + */ + static void power_delay(const bool on) { #if DISABLED(LASER_POWER_INLINE) safe_delay(on ? SPINDLE_LASER_POWERUP_DELAY : SPINDLE_LASER_POWERDOWN_DELAY); #endif @@ -208,17 +220,32 @@ class SpindleLaser { static void set_reverse(const bool reverse); static bool is_reverse() { return READ(SPINDLE_DIR_PIN) == SPINDLE_INVERT_DIR; } #else - static inline void set_reverse(const bool) {} + static void set_reverse(const bool) {} static bool is_reverse() { return false; } #endif - static inline void disable() { isReady = false; set_enabled(false); } + #if ENABLED(AIR_EVACUATION) + static void air_evac_enable(); // Turn On Cutter Vacuum or Laser Blower motor + static void air_evac_disable(); // Turn Off Cutter Vacuum or Laser Blower motor + static void air_evac_toggle(); // Toggle Cutter Vacuum or Laser Blower motor + static bool air_evac_state() { // Get current state + return (READ(AIR_EVACUATION_PIN) == AIR_EVACUATION_ACTIVE); + } + #endif - #if HAS_LCD_MENU + #if ENABLED(AIR_ASSIST) + static void air_assist_enable(); // Turn on air assist + static void air_assist_disable(); // Turn off air assist + static void air_assist_toggle(); // Toggle air assist + static bool air_assist_state() { // Get current state + return (READ(AIR_ASSIST_PIN) == AIR_ASSIST_ACTIVE); + } + #endif - static inline void enable_with_dir(const bool reverse) { + #if HAS_MARLINUI_MENU + static void enable_with_dir(const bool reverse) { isReady = true; - const uint8_t ocr = TERN(SPINDLE_LASER_PWM, upower_to_ocr(menuPower), 255); + const uint8_t ocr = TERN(SPINDLE_LASER_USE_PWM, upower_to_ocr(menuPower), 255); if (menuPower) power = ocr; else @@ -231,8 +258,8 @@ class SpindleLaser { FORCE_INLINE static void enable_reverse() { enable_with_dir(true); } FORCE_INLINE static void enable_same_dir() { enable_with_dir(is_reverse()); } - #if ENABLED(SPINDLE_LASER_PWM) - static inline void update_from_mpower() { + #if ENABLED(SPINDLE_LASER_USE_PWM) + static void update_from_mpower() { if (isReady) power = upower_to_ocr(menuPower); unitPower = menuPower; } @@ -244,15 +271,15 @@ class SpindleLaser { * Also fires with any PWM power that was previous set * If not set defaults to 80% power */ - static inline void test_fire_pulse() { - enable_forward(); // Turn Laser on (Spindle speak but same funct) + static void test_fire_pulse() { TERN_(USE_BEEPER, buzzer.tone(30, 3000)); + enable_forward(); // Turn Laser on (Spindle speak but same funct) delay(testPulse); // Delay for time set by user in pulse ms menu screen. disable(); // Turn laser off } #endif - #endif // HAS_LCD_MENU + #endif // HAS_MARLINUI_MENU #if ENABLED(LASER_POWER_INLINE) /** @@ -261,7 +288,7 @@ class SpindleLaser { */ // Force disengage planner power control - static inline void inline_disable() { + static void inline_disable() { isReady = false; unitPower = 0; planner.laser_inline.status.isPlanned = false; @@ -270,21 +297,21 @@ class SpindleLaser { } // Inline modes of all other functions; all enable planner inline power control - static inline void set_inline_enabled(const bool enable) { + static void set_inline_enabled(const bool enable) { if (enable) inline_power(255); else { isReady = false; unitPower = menuPower = 0; planner.laser_inline.status.isPlanned = false; - TERN(SPINDLE_LASER_PWM, inline_ocr_power, inline_power)(0); + TERN(SPINDLE_LASER_USE_PWM, inline_ocr_power, inline_power)(0); } } // Set the power for subsequent movement blocks static void inline_power(const cutter_power_t upwr) { unitPower = menuPower = upwr; - #if ENABLED(SPINDLE_LASER_PWM) + #if ENABLED(SPINDLE_LASER_USE_PWM) #if ENABLED(SPEED_POWER_RELATIVE) && !CUTTER_UNIT_IS(RPM) // relative mode does not turn laser off at 0, except for RPM planner.laser_inline.status.isEnabled = true; planner.laser_inline.power = upower_to_ocr(upwr); @@ -299,18 +326,18 @@ class SpindleLaser { #endif } - static inline void inline_direction(const bool) { /* never */ } + static void inline_direction(const bool) { /* never */ } - #if ENABLED(SPINDLE_LASER_PWM) - static inline void inline_ocr_power(const uint8_t ocrpwr) { + #if ENABLED(SPINDLE_LASER_USE_PWM) + static void inline_ocr_power(const uint8_t ocrpwr) { isReady = ocrpwr > 0; planner.laser_inline.status.isEnabled = ocrpwr > 0; planner.laser_inline.power = ocrpwr; } #endif - #endif // LASER_POWER_INLINE + #endif // LASER_POWER_INLINE - static inline void kill() { + static void kill() { TERN_(LASER_POWER_INLINE, inline_disable()); disable(); } diff --git a/Marlin/src/feature/spindle_laser_types.h b/Marlin/src/feature/spindle_laser_types.h index 0075e5481900..d249a20e7531 100644 --- a/Marlin/src/feature/spindle_laser_types.h +++ b/Marlin/src/feature/spindle_laser_types.h @@ -28,12 +28,34 @@ #include "../inc/MarlinConfigPre.h" +#define MSG_CUTTER(M) _MSG_CUTTER(M) + +#ifndef SPEED_POWER_INTERCEPT + #define SPEED_POWER_INTERCEPT 0 +#endif #if ENABLED(SPINDLE_FEATURE) #define _MSG_CUTTER(M) MSG_SPINDLE_##M + #ifndef SPEED_POWER_MIN + #define SPEED_POWER_MIN 5000 + #endif + #ifndef SPEED_POWER_MAX + #define SPEED_POWER_MAX 30000 + #endif + #ifndef SPEED_POWER_STARTUP + #define SPEED_POWER_STARTUP 25000 + #endif #else #define _MSG_CUTTER(M) MSG_LASER_##M + #ifndef SPEED_POWER_MIN + #define SPEED_POWER_MIN 0 + #endif + #ifndef SPEED_POWER_MAX + #define SPEED_POWER_MAX 255 + #endif + #ifndef SPEED_POWER_STARTUP + #define SPEED_POWER_STARTUP 255 + #endif #endif -#define MSG_CUTTER(M) _MSG_CUTTER(M) typedef IF<(SPEED_POWER_MAX > 255), uint16_t, uint8_t>::type cutter_cpower_t; diff --git a/Marlin/src/feature/stepper_driver_safety.cpp b/Marlin/src/feature/stepper_driver_safety.cpp new file mode 100644 index 000000000000..11b90954b4f5 --- /dev/null +++ b/Marlin/src/feature/stepper_driver_safety.cpp @@ -0,0 +1,117 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2021 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + * + */ +#include "../inc/MarlinConfig.h" +#include "../lcd/marlinui.h" + +#if HAS_DRIVER_SAFE_POWER_PROTECT + +#include "stepper_driver_safety.h" + +static uint32_t axis_plug_backward = 0; + +void stepper_driver_backward_error(FSTR_P const fstr) { + SERIAL_ERROR_START(); + SERIAL_ECHOF(fstr); + SERIAL_ECHOLNPGM(" driver is backward!"); + ui.status_printf(2, F(S_FMT S_FMT), FTOP(fstr), GET_TEXT(MSG_DRIVER_BACKWARD)); +} + +void stepper_driver_backward_check() { + + OUT_WRITE(SAFE_POWER_PIN, LOW); + + #define _TEST_BACKWARD(AXIS, BIT) do { \ + SET_INPUT(AXIS##_ENABLE_PIN); \ + OUT_WRITE(AXIS##_STEP_PIN, false); \ + delay(20); \ + if (READ(AXIS##_ENABLE_PIN) == false) { \ + SBI(axis_plug_backward, BIT); \ + stepper_driver_backward_error(F(STRINGIFY(AXIS))); \ + } \ + }while(0) + + #define TEST_BACKWARD(AXIS, BIT) TERN_(HAS_##AXIS##_ENABLE, _TEST_BACKWARD(AXIS, BIT)) + + TEST_BACKWARD(X, 0); + TEST_BACKWARD(X2, 1); + + TEST_BACKWARD(Y, 2); + TEST_BACKWARD(Y2, 3); + + TEST_BACKWARD(Z, 4); + TEST_BACKWARD(Z2, 5); + TEST_BACKWARD(Z3, 6); + TEST_BACKWARD(Z4, 7); + + TEST_BACKWARD(I, 8); + TEST_BACKWARD(J, 9); + TEST_BACKWARD(K, 10); + + TEST_BACKWARD(E0, 11); + TEST_BACKWARD(E1, 12); + TEST_BACKWARD(E2, 13); + TEST_BACKWARD(E3, 14); + TEST_BACKWARD(E4, 15); + TEST_BACKWARD(E5, 16); + TEST_BACKWARD(E6, 17); + TEST_BACKWARD(E7, 18); + + if (!axis_plug_backward) + WRITE(SAFE_POWER_PIN, HIGH); +} + +void stepper_driver_backward_report() { + if (!axis_plug_backward) return; + + auto _report_if_backward = [](FSTR_P const axis, uint8_t bit) { + if (TEST(axis_plug_backward, bit)) + stepper_driver_backward_error(axis); + }; + + #define REPORT_BACKWARD(axis, bit) TERN_(HAS_##axis##_ENABLE, _report_if_backward(F(STRINGIFY(axis)), bit)) + + REPORT_BACKWARD(X, 0); + REPORT_BACKWARD(X2, 1); + + REPORT_BACKWARD(Y, 2); + REPORT_BACKWARD(Y2, 3); + + REPORT_BACKWARD(Z, 4); + REPORT_BACKWARD(Z2, 5); + REPORT_BACKWARD(Z3, 6); + REPORT_BACKWARD(Z4, 7); + + REPORT_BACKWARD(I, 8); + REPORT_BACKWARD(J, 9); + REPORT_BACKWARD(K, 10); + + REPORT_BACKWARD(E0, 11); + REPORT_BACKWARD(E1, 12); + REPORT_BACKWARD(E2, 13); + REPORT_BACKWARD(E3, 14); + REPORT_BACKWARD(E4, 15); + REPORT_BACKWARD(E5, 16); + REPORT_BACKWARD(E6, 17); + REPORT_BACKWARD(E7, 18); +} + +#endif // HAS_DRIVER_SAFE_POWER_PROTECT diff --git a/Marlin/src/feature/stepper_driver_safety.h b/Marlin/src/feature/stepper_driver_safety.h new file mode 100644 index 000000000000..46edf3390d77 --- /dev/null +++ b/Marlin/src/feature/stepper_driver_safety.h @@ -0,0 +1,28 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2021 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + * + */ +#pragma once + + +#include "../inc/MarlinConfigPre.h" + +void stepper_driver_backward_check(); +void stepper_driver_backward_report(); diff --git a/Marlin/src/feature/tmc_util.cpp b/Marlin/src/feature/tmc_util.cpp index a4f71414a6b6..cf3fa3b7b0b7 100644 --- a/Marlin/src/feature/tmc_util.cpp +++ b/Marlin/src/feature/tmc_util.cpp @@ -40,7 +40,7 @@ #endif #endif -#if HAS_LCD_MENU +#if HAS_MARLINUI_MENU #include "../module/stepper.h" #endif @@ -208,11 +208,11 @@ #if ENABLED(STOP_ON_ERROR) void report_driver_error(const TMC_driver_data &data) { SERIAL_ECHOPGM(" driver error detected: 0x"); - SERIAL_PRINTLN(data.drv_status, HEX); + SERIAL_PRINTLN(data.drv_status, PrintBase::Hex); if (data.is_ot) SERIAL_ECHOLNPGM("overtemperature"); if (data.is_s2g) SERIAL_ECHOLNPGM("coil short circuit"); - TERN_(TMC_DEBUG, tmc_report_all(true, true, true, true)); - kill(PSTR("Driver error")); + TERN_(TMC_DEBUG, tmc_report_all()); + kill(F("Driver error")); } #endif @@ -226,7 +226,7 @@ SERIAL_ECHO(timestamp); SERIAL_ECHOPGM(": "); st.printLabel(); - SERIAL_ECHOLNPAIR(" driver overtemperature warning! (", st.getMilliamps(), "mA)"); + SERIAL_ECHOLNPGM(" driver overtemperature warning! (", st.getMilliamps(), "mA)"); } template @@ -271,7 +271,7 @@ st.rms_current(I_rms); #if ENABLED(REPORT_CURRENT_CHANGE) st.printLabel(); - SERIAL_ECHOLNPAIR(" current decreased to ", I_rms); + SERIAL_ECHOLNPGM(" current decreased to ", I_rms); #endif } } @@ -417,6 +417,19 @@ } #endif + #if AXIS_IS_TMC(I) + if (monitor_tmc_driver(stepperI, need_update_error_counters, need_debug_reporting)) + step_current_down(stepperI); + #endif + #if AXIS_IS_TMC(J) + if (monitor_tmc_driver(stepperJ, need_update_error_counters, need_debug_reporting)) + step_current_down(stepperJ); + #endif + #if AXIS_IS_TMC(K) + if (monitor_tmc_driver(stepperK, need_update_error_counters, need_debug_reporting)) + step_current_down(stepperK); + #endif + #if AXIS_IS_TMC(E0) (void)monitor_tmc_driver(stepperE0, need_update_error_counters, need_debug_reporting); #endif @@ -457,12 +470,8 @@ void tmc_set_report_interval(const uint16_t update_interval) { if ((report_tmc_status_interval = update_interval)) SERIAL_ECHOLNPGM("axis:pwm_scale" - #if HAS_STEALTHCHOP - "/curr_scale" - #endif - #if HAS_STALLGUARD - "/mech_load" - #endif + TERN_(HAS_STEALTHCHOP, "/curr_scale") + TERN_(HAS_STALLGUARD, "/mech_load") "|flags|warncount" ); } @@ -546,7 +555,7 @@ }; template - static void print_vsense(TMC &st) { SERIAL_ECHOPGM_P(st.vsense() ? PSTR("1=.18") : PSTR("0=.325")); } + static void print_vsense(TMC &st) { SERIAL_ECHOF(st.vsense() ? F("1=.18") : F("0=.325")); } #if HAS_DRIVER(TMC2130) || HAS_DRIVER(TMC5130) static void _tmc_status(TMC2130Stepper &st, const TMC_debug_enum i) { @@ -717,7 +726,7 @@ SERIAL_ECHO(st.cs()); SERIAL_ECHOPGM("/31"); break; - case TMC_VSENSE: SERIAL_ECHOPGM_P(st.vsense() ? PSTR("1=.165") : PSTR("0=.310")); break; + case TMC_VSENSE: SERIAL_ECHOF(st.vsense() ? F("1=.165") : F("0=.310")); break; case TMC_MICROSTEPS: SERIAL_ECHO(st.microsteps()); break; //case TMC_OTPW: serialprint_truefalse(st.otpw()); break; //case TMC_OTPW_TRIGGERED: serialprint_truefalse(st.getOTPW()); break; @@ -757,128 +766,148 @@ } } - static void tmc_debug_loop(const TMC_debug_enum i, const bool print_x, const bool print_y, const bool print_z, const bool print_e) { - if (print_x) { + static void tmc_debug_loop(const TMC_debug_enum n, LOGICAL_AXIS_ARGS(const bool)) { + if (x) { #if AXIS_IS_TMC(X) - tmc_status(stepperX, i); + tmc_status(stepperX, n); #endif #if AXIS_IS_TMC(X2) - tmc_status(stepperX2, i); + tmc_status(stepperX2, n); #endif } - if (print_y) { + if (TERN0(HAS_Y_AXIS, y)) { #if AXIS_IS_TMC(Y) - tmc_status(stepperY, i); + tmc_status(stepperY, n); #endif #if AXIS_IS_TMC(Y2) - tmc_status(stepperY2, i); + tmc_status(stepperY2, n); #endif } - if (print_z) { + if (TERN0(HAS_Z_AXIS, z)) { #if AXIS_IS_TMC(Z) - tmc_status(stepperZ, i); + tmc_status(stepperZ, n); #endif #if AXIS_IS_TMC(Z2) - tmc_status(stepperZ2, i); + tmc_status(stepperZ2, n); #endif #if AXIS_IS_TMC(Z3) - tmc_status(stepperZ3, i); + tmc_status(stepperZ3, n); #endif #if AXIS_IS_TMC(Z4) - tmc_status(stepperZ4, i); + tmc_status(stepperZ4, n); #endif } - if (print_e) { + #if AXIS_IS_TMC(I) + if (i) tmc_status(stepperI, n); + #endif + #if AXIS_IS_TMC(J) + if (j) tmc_status(stepperJ, n); + #endif + #if AXIS_IS_TMC(K) + if (k) tmc_status(stepperK, n); + #endif + + if (TERN0(HAS_EXTRUDERS, e)) { #if AXIS_IS_TMC(E0) - tmc_status(stepperE0, i); + tmc_status(stepperE0, n); #endif #if AXIS_IS_TMC(E1) - tmc_status(stepperE1, i); + tmc_status(stepperE1, n); #endif #if AXIS_IS_TMC(E2) - tmc_status(stepperE2, i); + tmc_status(stepperE2, n); #endif #if AXIS_IS_TMC(E3) - tmc_status(stepperE3, i); + tmc_status(stepperE3, n); #endif #if AXIS_IS_TMC(E4) - tmc_status(stepperE4, i); + tmc_status(stepperE4, n); #endif #if AXIS_IS_TMC(E5) - tmc_status(stepperE5, i); + tmc_status(stepperE5, n); #endif #if AXIS_IS_TMC(E6) - tmc_status(stepperE6, i); + tmc_status(stepperE6, n); #endif #if AXIS_IS_TMC(E7) - tmc_status(stepperE7, i); + tmc_status(stepperE7, n); #endif } SERIAL_EOL(); } - static void drv_status_loop(const TMC_drv_status_enum i, const bool print_x, const bool print_y, const bool print_z, const bool print_e) { - if (print_x) { + static void drv_status_loop(const TMC_drv_status_enum n, LOGICAL_AXIS_ARGS(const bool)) { + if (x) { #if AXIS_IS_TMC(X) - tmc_parse_drv_status(stepperX, i); + tmc_parse_drv_status(stepperX, n); #endif #if AXIS_IS_TMC(X2) - tmc_parse_drv_status(stepperX2, i); + tmc_parse_drv_status(stepperX2, n); #endif } - if (print_y) { + if (TERN0(HAS_Y_AXIS, y)) { #if AXIS_IS_TMC(Y) - tmc_parse_drv_status(stepperY, i); + tmc_parse_drv_status(stepperY, n); #endif #if AXIS_IS_TMC(Y2) - tmc_parse_drv_status(stepperY2, i); + tmc_parse_drv_status(stepperY2, n); #endif } - if (print_z) { + if (TERN0(HAS_Z_AXIS, z)) { #if AXIS_IS_TMC(Z) - tmc_parse_drv_status(stepperZ, i); + tmc_parse_drv_status(stepperZ, n); #endif #if AXIS_IS_TMC(Z2) - tmc_parse_drv_status(stepperZ2, i); + tmc_parse_drv_status(stepperZ2, n); #endif #if AXIS_IS_TMC(Z3) - tmc_parse_drv_status(stepperZ3, i); + tmc_parse_drv_status(stepperZ3, n); #endif #if AXIS_IS_TMC(Z4) - tmc_parse_drv_status(stepperZ4, i); + tmc_parse_drv_status(stepperZ4, n); #endif } - if (print_e) { + #if AXIS_IS_TMC(I) + if (i) tmc_parse_drv_status(stepperI, n); + #endif + #if AXIS_IS_TMC(J) + if (j) tmc_parse_drv_status(stepperJ, n); + #endif + #if AXIS_IS_TMC(K) + if (k) tmc_parse_drv_status(stepperK, n); + #endif + + if (TERN0(HAS_EXTRUDERS, e)) { #if AXIS_IS_TMC(E0) - tmc_parse_drv_status(stepperE0, i); + tmc_parse_drv_status(stepperE0, n); #endif #if AXIS_IS_TMC(E1) - tmc_parse_drv_status(stepperE1, i); + tmc_parse_drv_status(stepperE1, n); #endif #if AXIS_IS_TMC(E2) - tmc_parse_drv_status(stepperE2, i); + tmc_parse_drv_status(stepperE2, n); #endif #if AXIS_IS_TMC(E3) - tmc_parse_drv_status(stepperE3, i); + tmc_parse_drv_status(stepperE3, n); #endif #if AXIS_IS_TMC(E4) - tmc_parse_drv_status(stepperE4, i); + tmc_parse_drv_status(stepperE4, n); #endif #if AXIS_IS_TMC(E5) - tmc_parse_drv_status(stepperE5, i); + tmc_parse_drv_status(stepperE5, n); #endif #if AXIS_IS_TMC(E6) - tmc_parse_drv_status(stepperE6, i); + tmc_parse_drv_status(stepperE6, n); #endif #if AXIS_IS_TMC(E7) - tmc_parse_drv_status(stepperE7, i); + tmc_parse_drv_status(stepperE7, n); #endif } @@ -889,9 +918,10 @@ * M122 report functions */ - void tmc_report_all(bool print_x, const bool print_y, const bool print_z, const bool print_e) { - #define TMC_REPORT(LABEL, ITEM) do{ SERIAL_ECHOPGM(LABEL); tmc_debug_loop(ITEM, print_x, print_y, print_z, print_e); }while(0) - #define DRV_REPORT(LABEL, ITEM) do{ SERIAL_ECHOPGM(LABEL); drv_status_loop(ITEM, print_x, print_y, print_z, print_e); }while(0) + void tmc_report_all(LOGICAL_AXIS_ARGS(const bool)) { + #define TMC_REPORT(LABEL, ITEM) do{ SERIAL_ECHOPGM(LABEL); tmc_debug_loop(ITEM, LOGICAL_AXIS_ARGS()); }while(0) + #define DRV_REPORT(LABEL, ITEM) do{ SERIAL_ECHOPGM(LABEL); drv_status_loop(ITEM, LOGICAL_AXIS_ARGS()); }while(0) + TMC_REPORT("\t", TMC_CODES); #if HAS_DRIVER(TMC2209) TMC_REPORT("Address\t", TMC_UART_ADDR); @@ -1015,72 +1045,82 @@ } #endif - static void tmc_get_registers(TMC_get_registers_enum i, const bool print_x, const bool print_y, const bool print_z, const bool print_e) { - if (print_x) { + static void tmc_get_registers(TMC_get_registers_enum n, LOGICAL_AXIS_ARGS(const bool)) { + if (x) { #if AXIS_IS_TMC(X) - tmc_get_registers(stepperX, i); + tmc_get_registers(stepperX, n); #endif #if AXIS_IS_TMC(X2) - tmc_get_registers(stepperX2, i); + tmc_get_registers(stepperX2, n); #endif } - if (print_y) { + if (TERN0(HAS_Y_AXIS, y)) { #if AXIS_IS_TMC(Y) - tmc_get_registers(stepperY, i); + tmc_get_registers(stepperY, n); #endif #if AXIS_IS_TMC(Y2) - tmc_get_registers(stepperY2, i); + tmc_get_registers(stepperY2, n); #endif } - if (print_z) { + if (TERN0(HAS_Z_AXIS, z)) { #if AXIS_IS_TMC(Z) - tmc_get_registers(stepperZ, i); + tmc_get_registers(stepperZ, n); #endif #if AXIS_IS_TMC(Z2) - tmc_get_registers(stepperZ2, i); + tmc_get_registers(stepperZ2, n); #endif #if AXIS_IS_TMC(Z3) - tmc_get_registers(stepperZ3, i); + tmc_get_registers(stepperZ3, n); #endif #if AXIS_IS_TMC(Z4) - tmc_get_registers(stepperZ4, i); + tmc_get_registers(stepperZ4, n); #endif } - if (print_e) { + #if AXIS_IS_TMC(I) + if (i) tmc_get_registers(stepperI, n); + #endif + #if AXIS_IS_TMC(J) + if (j) tmc_get_registers(stepperJ, n); + #endif + #if AXIS_IS_TMC(K) + if (k) tmc_get_registers(stepperK, n); + #endif + + if (TERN0(HAS_EXTRUDERS, e)) { #if AXIS_IS_TMC(E0) - tmc_get_registers(stepperE0, i); + tmc_get_registers(stepperE0, n); #endif #if AXIS_IS_TMC(E1) - tmc_get_registers(stepperE1, i); + tmc_get_registers(stepperE1, n); #endif #if AXIS_IS_TMC(E2) - tmc_get_registers(stepperE2, i); + tmc_get_registers(stepperE2, n); #endif #if AXIS_IS_TMC(E3) - tmc_get_registers(stepperE3, i); + tmc_get_registers(stepperE3, n); #endif #if AXIS_IS_TMC(E4) - tmc_get_registers(stepperE4, i); + tmc_get_registers(stepperE4, n); #endif #if AXIS_IS_TMC(E5) - tmc_get_registers(stepperE5, i); + tmc_get_registers(stepperE5, n); #endif #if AXIS_IS_TMC(E6) - tmc_get_registers(stepperE6, i); + tmc_get_registers(stepperE6, n); #endif #if AXIS_IS_TMC(E7) - tmc_get_registers(stepperE7, i); + tmc_get_registers(stepperE7, n); #endif } SERIAL_EOL(); } - void tmc_get_registers(bool print_x, bool print_y, bool print_z, bool print_e) { - #define _TMC_GET_REG(LABEL, ITEM) do{ SERIAL_ECHOPGM(LABEL); tmc_get_registers(ITEM, print_x, print_y, print_z, print_e); }while(0) + void tmc_get_registers(LOGICAL_AXIS_ARGS(bool)) { + #define _TMC_GET_REG(LABEL, ITEM) do{ SERIAL_ECHOPGM(LABEL); tmc_get_registers(ITEM, LOGICAL_AXIS_ARGS()); }while(0) #define TMC_GET_REG(NAME, TABS) _TMC_GET_REG(STRINGIFY(NAME) TABS, TMC_GET_##NAME) _TMC_GET_REG("\t", TMC_AXIS_CODES); TMC_GET_REG(GCONF, "\t\t"); @@ -1165,6 +1205,15 @@ #if AXIS_HAS_SPI(Z4) SET_CS_PIN(Z4); #endif + #if AXIS_HAS_SPI(I) + SET_CS_PIN(I); + #endif + #if AXIS_HAS_SPI(J) + SET_CS_PIN(J); + #endif + #if AXIS_HAS_SPI(K) + SET_CS_PIN(K); + #endif #if AXIS_HAS_SPI(E0) SET_CS_PIN(E0); #endif @@ -1201,23 +1250,22 @@ static bool test_connection(TMC &st) { if (test_result > 0) SERIAL_ECHOPGM("Error: All "); - const char *stat; + FSTR_P stat; switch (test_result) { default: - case 0: stat = PSTR("OK"); break; - case 1: stat = PSTR("HIGH"); break; - case 2: stat = PSTR("LOW"); break; + case 0: stat = F("OK"); break; + case 1: stat = F("HIGH"); break; + case 2: stat = F("LOW"); break; } - SERIAL_ECHOPGM_P(stat); - SERIAL_EOL(); + SERIAL_ECHOLNF(stat); return test_result; } -void test_tmc_connection(const bool test_x, const bool test_y, const bool test_z, const bool test_e) { +void test_tmc_connection(LOGICAL_AXIS_ARGS(const bool)) { uint8_t axis_connection = 0; - if (test_x) { + if (x) { #if AXIS_IS_TMC(X) axis_connection += test_connection(stepperX); #endif @@ -1226,7 +1274,7 @@ void test_tmc_connection(const bool test_x, const bool test_y, const bool test_z #endif } - if (test_y) { + if (TERN0(HAS_Y_AXIS, y)) { #if AXIS_IS_TMC(Y) axis_connection += test_connection(stepperY); #endif @@ -1235,7 +1283,7 @@ void test_tmc_connection(const bool test_x, const bool test_y, const bool test_z #endif } - if (test_z) { + if (TERN0(HAS_Z_AXIS, z)) { #if AXIS_IS_TMC(Z) axis_connection += test_connection(stepperZ); #endif @@ -1250,7 +1298,17 @@ void test_tmc_connection(const bool test_x, const bool test_y, const bool test_z #endif } - if (test_e) { + #if AXIS_IS_TMC(I) + if (i) axis_connection += test_connection(stepperI); + #endif + #if AXIS_IS_TMC(J) + if (j) axis_connection += test_connection(stepperJ); + #endif + #if AXIS_IS_TMC(K) + if (k) axis_connection += test_connection(stepperK); + #endif + + if (TERN0(HAS_EXTRUDERS, e)) { #if AXIS_IS_TMC(E0) axis_connection += test_connection(stepperE0); #endif @@ -1277,7 +1335,7 @@ void test_tmc_connection(const bool test_x, const bool test_y, const bool test_z #endif } - if (axis_connection) LCD_MESSAGEPGM(MSG_ERROR_TMC); + if (axis_connection) LCD_MESSAGE(MSG_ERROR_TMC); } #endif // HAS_TRINAMIC_CONFIG diff --git a/Marlin/src/feature/tmc_util.h b/Marlin/src/feature/tmc_util.h index b21b89f68bcb..892d33768b6f 100644 --- a/Marlin/src/feature/tmc_util.h +++ b/Marlin/src/feature/tmc_util.h @@ -45,6 +45,12 @@ constexpr uint16_t _tmc_thrs(const uint16_t msteps, const uint32_t thrs, const u return 12650000UL * msteps / (256 * thrs * spmm); } +typedef struct { + uint8_t toff; + int8_t hend; + uint8_t hstrt; +} chopper_timing_t; + template class TMCStorage { protected: @@ -58,21 +64,21 @@ class TMCStorage { uint8_t otpw_count = 0, error_count = 0; bool flag_otpw = false; - inline bool getOTPW() { return flag_otpw; } - inline void clear_otpw() { flag_otpw = 0; } + bool getOTPW() { return flag_otpw; } + void clear_otpw() { flag_otpw = 0; } #endif - inline uint16_t getMilliamps() { return val_mA; } + uint16_t getMilliamps() { return val_mA; } - inline void printLabel() { + void printLabel() { SERIAL_CHAR(AXIS_LETTER); if (DRIVER_ID > '0') SERIAL_CHAR(DRIVER_ID); } struct { - TERN_(HAS_STEALTHCHOP, bool stealthChop_enabled = false); - TERN_(HYBRID_THRESHOLD, uint8_t hybrid_thrs = 0); - TERN_(USE_SENSORLESS, int16_t homing_thrs = 0); + OPTCODE(HAS_STEALTHCHOP, bool stealthChop_enabled = false) + OPTCODE(HYBRID_THRESHOLD, uint8_t hybrid_thrs = 0) + OPTCODE(USE_SENSORLESS, int16_t homing_thrs = 0) } stored; }; @@ -91,55 +97,61 @@ class TMCMarlin : public TMC, public TMCStorage { TMCMarlin(const uint16_t CS, const float RS, const uint16_t pinMOSI, const uint16_t pinMISO, const uint16_t pinSCK, const uint8_t axis_chain_index) : TMC(CS, RS, pinMOSI, pinMISO, pinSCK, axis_chain_index) {} - inline uint16_t rms_current() { return TMC::rms_current(); } - inline void rms_current(uint16_t mA) { + uint16_t rms_current() { return TMC::rms_current(); } + void rms_current(uint16_t mA) { this->val_mA = mA; TMC::rms_current(mA); } - inline void rms_current(const uint16_t mA, const float mult) { + void rms_current(const uint16_t mA, const float mult) { this->val_mA = mA; TMC::rms_current(mA, mult); } - inline uint16_t get_microstep_counter() { return TMC::MSCNT(); } + uint16_t get_microstep_counter() { return TMC::MSCNT(); } #if HAS_STEALTHCHOP - inline bool get_stealthChop() { return this->en_pwm_mode(); } - inline bool get_stored_stealthChop() { return this->stored.stealthChop_enabled; } - inline void refresh_stepping_mode() { this->en_pwm_mode(this->stored.stealthChop_enabled); } - inline void set_stealthChop(const bool stch) { this->stored.stealthChop_enabled = stch; refresh_stepping_mode(); } - inline bool toggle_stepping_mode() { set_stealthChop(!this->stored.stealthChop_enabled); return get_stealthChop(); } + bool get_stealthChop() { return this->en_pwm_mode(); } + bool get_stored_stealthChop() { return this->stored.stealthChop_enabled; } + void refresh_stepping_mode() { this->en_pwm_mode(this->stored.stealthChop_enabled); } + void set_stealthChop(const bool stch) { this->stored.stealthChop_enabled = stch; refresh_stepping_mode(); } + bool toggle_stepping_mode() { set_stealthChop(!this->stored.stealthChop_enabled); return get_stealthChop(); } #endif + void set_chopper_times(const chopper_timing_t &ct) { + TMC::toff(ct.toff); + TMC::hysteresis_end(ct.hend); + TMC::hysteresis_start(ct.hstrt); + } + #if ENABLED(HYBRID_THRESHOLD) uint32_t get_pwm_thrs() { return _tmc_thrs(this->microsteps(), this->TPWMTHRS(), planner.settings.axis_steps_per_mm[AXIS_ID]); } void set_pwm_thrs(const uint32_t thrs) { TMC::TPWMTHRS(_tmc_thrs(this->microsteps(), thrs, planner.settings.axis_steps_per_mm[AXIS_ID])); - TERN_(HAS_LCD_MENU, this->stored.hybrid_thrs = thrs); + TERN_(HAS_MARLINUI_MENU, this->stored.hybrid_thrs = thrs); } #endif #if USE_SENSORLESS - inline int16_t homing_threshold() { return TMC::sgt(); } + int16_t homing_threshold() { return TMC::sgt(); } void homing_threshold(int16_t sgt_val) { sgt_val = (int16_t)constrain(sgt_val, sgt_min, sgt_max); TMC::sgt(sgt_val); - TERN_(HAS_LCD_MENU, this->stored.homing_thrs = sgt_val); + TERN_(HAS_MARLINUI_MENU, this->stored.homing_thrs = sgt_val); } #if ENABLED(SPI_ENDSTOPS) bool test_stall_status(); #endif #endif - #if HAS_LCD_MENU - inline void refresh_stepper_current() { rms_current(this->val_mA); } + #if HAS_MARLINUI_MENU + void refresh_stepper_current() { rms_current(this->val_mA); } #if ENABLED(HYBRID_THRESHOLD) - inline void refresh_hybrid_thrs() { set_pwm_thrs(this->stored.hybrid_thrs); } + void refresh_hybrid_thrs() { set_pwm_thrs(this->stored.hybrid_thrs); } #endif #if USE_SENSORLESS - inline void refresh_homing_thrs() { homing_threshold(this->stored.homing_thrs); } + void refresh_homing_thrs() { homing_threshold(this->stored.homing_thrs); } #endif #endif @@ -161,39 +173,45 @@ class TMCMarlin : public TMC220 {} uint16_t rms_current() { return TMC2208Stepper::rms_current(); } - inline void rms_current(const uint16_t mA) { + void rms_current(const uint16_t mA) { this->val_mA = mA; TMC2208Stepper::rms_current(mA); } - inline void rms_current(const uint16_t mA, const float mult) { + void rms_current(const uint16_t mA, const float mult) { this->val_mA = mA; TMC2208Stepper::rms_current(mA, mult); } - inline uint16_t get_microstep_counter() { return TMC2208Stepper::MSCNT(); } + uint16_t get_microstep_counter() { return TMC2208Stepper::MSCNT(); } #if HAS_STEALTHCHOP - inline bool get_stealthChop() { return !this->en_spreadCycle(); } - inline bool get_stored_stealthChop() { return this->stored.stealthChop_enabled; } - inline void refresh_stepping_mode() { this->en_spreadCycle(!this->stored.stealthChop_enabled); } - inline void set_stealthChop(const bool stch) { this->stored.stealthChop_enabled = stch; refresh_stepping_mode(); } - inline bool toggle_stepping_mode() { set_stealthChop(!this->stored.stealthChop_enabled); return get_stealthChop(); } + bool get_stealthChop() { return !this->en_spreadCycle(); } + bool get_stored_stealthChop() { return this->stored.stealthChop_enabled; } + void refresh_stepping_mode() { this->en_spreadCycle(!this->stored.stealthChop_enabled); } + void set_stealthChop(const bool stch) { this->stored.stealthChop_enabled = stch; refresh_stepping_mode(); } + bool toggle_stepping_mode() { set_stealthChop(!this->stored.stealthChop_enabled); return get_stealthChop(); } #endif + void set_chopper_times(const chopper_timing_t &ct) { + TMC2208Stepper::toff(ct.toff); + TMC2208Stepper::hysteresis_end(ct.hend); + TMC2208Stepper::hysteresis_start(ct.hstrt); + } + #if ENABLED(HYBRID_THRESHOLD) uint32_t get_pwm_thrs() { return _tmc_thrs(this->microsteps(), this->TPWMTHRS(), planner.settings.axis_steps_per_mm[AXIS_ID]); } void set_pwm_thrs(const uint32_t thrs) { TMC2208Stepper::TPWMTHRS(_tmc_thrs(this->microsteps(), thrs, planner.settings.axis_steps_per_mm[AXIS_ID])); - TERN_(HAS_LCD_MENU, this->stored.hybrid_thrs = thrs); + TERN_(HAS_MARLINUI_MENU, this->stored.hybrid_thrs = thrs); } #endif - #if HAS_LCD_MENU - inline void refresh_stepper_current() { rms_current(this->val_mA); } + #if HAS_MARLINUI_MENU + void refresh_stepper_current() { rms_current(this->val_mA); } #if ENABLED(HYBRID_THRESHOLD) - inline void refresh_hybrid_thrs() { set_pwm_thrs(this->stored.hybrid_thrs); } + void refresh_hybrid_thrs() { set_pwm_thrs(this->stored.hybrid_thrs); } #endif #endif }; @@ -209,50 +227,56 @@ class TMCMarlin : public TMC220 {} uint8_t get_address() { return slave_address; } uint16_t rms_current() { return TMC2209Stepper::rms_current(); } - inline void rms_current(const uint16_t mA) { + void rms_current(const uint16_t mA) { this->val_mA = mA; TMC2209Stepper::rms_current(mA); } - inline void rms_current(const uint16_t mA, const float mult) { + void rms_current(const uint16_t mA, const float mult) { this->val_mA = mA; TMC2209Stepper::rms_current(mA, mult); } - inline uint16_t get_microstep_counter() { return TMC2209Stepper::MSCNT(); } + uint16_t get_microstep_counter() { return TMC2209Stepper::MSCNT(); } #if HAS_STEALTHCHOP - inline bool get_stealthChop() { return !this->en_spreadCycle(); } - inline bool get_stored_stealthChop() { return this->stored.stealthChop_enabled; } - inline void refresh_stepping_mode() { this->en_spreadCycle(!this->stored.stealthChop_enabled); } - inline void set_stealthChop(const bool stch) { this->stored.stealthChop_enabled = stch; refresh_stepping_mode(); } - inline bool toggle_stepping_mode() { set_stealthChop(!this->stored.stealthChop_enabled); return get_stealthChop(); } + bool get_stealthChop() { return !this->en_spreadCycle(); } + bool get_stored_stealthChop() { return this->stored.stealthChop_enabled; } + void refresh_stepping_mode() { this->en_spreadCycle(!this->stored.stealthChop_enabled); } + void set_stealthChop(const bool stch) { this->stored.stealthChop_enabled = stch; refresh_stepping_mode(); } + bool toggle_stepping_mode() { set_stealthChop(!this->stored.stealthChop_enabled); return get_stealthChop(); } #endif + void set_chopper_times(const chopper_timing_t &ct) { + TMC2209Stepper::toff(ct.toff); + TMC2209Stepper::hysteresis_end(ct.hend); + TMC2209Stepper::hysteresis_start(ct.hstrt); + } + #if ENABLED(HYBRID_THRESHOLD) uint32_t get_pwm_thrs() { return _tmc_thrs(this->microsteps(), this->TPWMTHRS(), planner.settings.axis_steps_per_mm[AXIS_ID]); } void set_pwm_thrs(const uint32_t thrs) { TMC2209Stepper::TPWMTHRS(_tmc_thrs(this->microsteps(), thrs, planner.settings.axis_steps_per_mm[AXIS_ID])); - TERN_(HAS_LCD_MENU, this->stored.hybrid_thrs = thrs); + TERN_(HAS_MARLINUI_MENU, this->stored.hybrid_thrs = thrs); } #endif #if USE_SENSORLESS - inline int16_t homing_threshold() { return TMC2209Stepper::SGTHRS(); } + int16_t homing_threshold() { return TMC2209Stepper::SGTHRS(); } void homing_threshold(int16_t sgt_val) { sgt_val = (int16_t)constrain(sgt_val, sgt_min, sgt_max); TMC2209Stepper::SGTHRS(sgt_val); - TERN_(HAS_LCD_MENU, this->stored.homing_thrs = sgt_val); + TERN_(HAS_MARLINUI_MENU, this->stored.homing_thrs = sgt_val); } #endif - #if HAS_LCD_MENU - inline void refresh_stepper_current() { rms_current(this->val_mA); } + #if HAS_MARLINUI_MENU + void refresh_stepper_current() { rms_current(this->val_mA); } #if ENABLED(HYBRID_THRESHOLD) - inline void refresh_hybrid_thrs() { set_pwm_thrs(this->stored.hybrid_thrs); } + void refresh_hybrid_thrs() { set_pwm_thrs(this->stored.hybrid_thrs); } #endif #if USE_SENSORLESS - inline void refresh_homing_thrs() { homing_threshold(this->stored.homing_thrs); } + void refresh_homing_thrs() { homing_threshold(this->stored.homing_thrs); } #endif #endif @@ -269,27 +293,33 @@ class TMCMarlin : public TMC266 TMCMarlin(const uint16_t CS, const float RS, const uint16_t pinMOSI, const uint16_t pinMISO, const uint16_t pinSCK, const uint8_t) : TMC2660Stepper(CS, RS, pinMOSI, pinMISO, pinSCK) {} - inline uint16_t rms_current() { return TMC2660Stepper::rms_current(); } - inline void rms_current(const uint16_t mA) { + uint16_t rms_current() { return TMC2660Stepper::rms_current(); } + void rms_current(const uint16_t mA) { this->val_mA = mA; TMC2660Stepper::rms_current(mA); } - inline uint16_t get_microstep_counter() { return TMC2660Stepper::mstep(); } + uint16_t get_microstep_counter() { return TMC2660Stepper::mstep(); } + + void set_chopper_times(const chopper_timing_t &ct) { + TMC2660Stepper::toff(ct.toff); + TMC2660Stepper::hysteresis_end(ct.hend); + TMC2660Stepper::hysteresis_start(ct.hstrt); + } #if USE_SENSORLESS - inline int16_t homing_threshold() { return TMC2660Stepper::sgt(); } + int16_t homing_threshold() { return TMC2660Stepper::sgt(); } void homing_threshold(int16_t sgt_val) { sgt_val = (int16_t)constrain(sgt_val, sgt_min, sgt_max); TMC2660Stepper::sgt(sgt_val); - TERN_(HAS_LCD_MENU, this->stored.homing_thrs = sgt_val); + TERN_(HAS_MARLINUI_MENU, this->stored.homing_thrs = sgt_val); } #endif - #if HAS_LCD_MENU - inline void refresh_stepper_current() { rms_current(this->val_mA); } + #if HAS_MARLINUI_MENU + void refresh_stepper_current() { rms_current(this->val_mA); } #if USE_SENSORLESS - inline void refresh_homing_thrs() { homing_threshold(this->stored.homing_thrs); } + void refresh_homing_thrs() { homing_threshold(this->stored.homing_thrs); } #endif #endif @@ -297,52 +327,15 @@ class TMCMarlin : public TMC266 sgt_max = 63; }; -template -void tmc_print_current(TMC &st) { - st.printLabel(); - SERIAL_ECHOLNPAIR(" driver current: ", st.getMilliamps()); -} - -#if ENABLED(MONITOR_DRIVER_STATUS) - template - void tmc_report_otpw(TMC &st) { - st.printLabel(); - SERIAL_ECHOPGM(" temperature prewarn triggered: "); - serialprint_truefalse(st.getOTPW()); - SERIAL_EOL(); - } - template - void tmc_clear_otpw(TMC &st) { - st.clear_otpw(); - st.printLabel(); - SERIAL_ECHOLNPGM(" prewarn flag cleared"); - } -#endif -#if ENABLED(HYBRID_THRESHOLD) - template - void tmc_print_pwmthrs(TMC &st) { - st.printLabel(); - SERIAL_ECHOLNPAIR(" stealthChop max speed: ", st.get_pwm_thrs()); - } -#endif -#if USE_SENSORLESS - template - void tmc_print_sgt(TMC &st) { - st.printLabel(); - SERIAL_ECHOPGM(" homing sensitivity: "); - SERIAL_PRINTLN(st.homing_threshold(), DEC); - } -#endif - void monitor_tmc_drivers(); -void test_tmc_connection(const bool test_x, const bool test_y, const bool test_z, const bool test_e); +void test_tmc_connection(LOGICAL_AXIS_DECL(const bool, true)); #if ENABLED(TMC_DEBUG) #if ENABLED(MONITOR_DRIVER_STATUS) void tmc_set_report_interval(const uint16_t update_interval); #endif - void tmc_report_all(const bool print_x, const bool print_y, const bool print_z, const bool print_e); - void tmc_get_registers(const bool print_x, const bool print_y, const bool print_z, const bool print_e); + void tmc_report_all(LOGICAL_AXIS_DECL(const bool, true)); + void tmc_get_registers(LOGICAL_AXIS_ARGS(const bool)); #endif /** @@ -355,16 +348,11 @@ void test_tmc_connection(const bool test_x, const bool test_y, const bool test_z #if USE_SENSORLESS // Track enabled status of stealthChop and only re-enable where applicable - struct sensorless_t { bool x, y, z, x2, y2, z2, z3, z4; }; + struct sensorless_t { bool LINEAR_AXIS_ARGS(), x2, y2, z2, z3, z4; }; #if ENABLED(IMPROVE_HOMING_RELIABILITY) extern millis_t sg_guard_period; constexpr uint16_t default_sg_guard_duration = 400; - - struct slow_homing_t { - xy_ulong_t acceleration; - TERN_(HAS_CLASSIC_JERK, xy_float_t jerk_xy); - }; #endif bool tmc_enable_stallguard(TMC2130Stepper &st); diff --git a/Marlin/src/feature/tramming.h b/Marlin/src/feature/tramming.h index eb27fe82fe2a..925659e29db2 100644 --- a/Marlin/src/feature/tramming.h +++ b/Marlin/src/feature/tramming.h @@ -28,12 +28,12 @@ #error "TRAMMING_SCREW_THREAD must be equal to 30, 31, 40, 41, 50, or 51." #endif -constexpr xy_pos_t screws_tilt_adjust_pos[] = TRAMMING_POINT_XY; +constexpr xy_pos_t tramming_points[] = TRAMMING_POINT_XY; -#define G35_PROBE_COUNT COUNT(screws_tilt_adjust_pos) +#define G35_PROBE_COUNT COUNT(tramming_points) static_assert(WITHIN(G35_PROBE_COUNT, 3, 6), "TRAMMING_POINT_XY requires between 3 and 6 XY positions."); -#define VALIDATE_TRAMMING_POINT(N) static_assert(N >= G35_PROBE_COUNT || Probe::build_time::can_reach(screws_tilt_adjust_pos[N]), \ +#define VALIDATE_TRAMMING_POINT(N) static_assert(N >= G35_PROBE_COUNT || Probe::build_time::can_reach(tramming_points[N]), \ "TRAMMING_POINT_XY point " STRINGIFY(N) " is not reachable with the default NOZZLE_TO_PROBE offset and PROBING_MARGIN.") VALIDATE_TRAMMING_POINT(0); VALIDATE_TRAMMING_POINT(1); VALIDATE_TRAMMING_POINT(2); VALIDATE_TRAMMING_POINT(3); VALIDATE_TRAMMING_POINT(4); VALIDATE_TRAMMING_POINT(5); diff --git a/Marlin/src/feature/twibus.cpp b/Marlin/src/feature/twibus.cpp index 755224544c6e..9aec6b030537 100644 --- a/Marlin/src/feature/twibus.cpp +++ b/Marlin/src/feature/twibus.cpp @@ -28,13 +28,24 @@ #include +#include "../libs/hex_print.h" + TWIBus i2c; TWIBus::TWIBus() { #if I2C_SLAVE_ADDRESS == 0 - Wire.begin(); // No address joins the BUS as the master + + #if PINS_EXIST(I2C_SCL, I2C_SDA) && DISABLED(SOFT_I2C_EEPROM) + Wire.setSDA(pin_t(I2C_SDA_PIN)); + Wire.setSCL(pin_t(I2C_SCL_PIN)); + #endif + + Wire.begin(); // No address joins the BUS as the master + #else - Wire.begin(I2C_SLAVE_ADDRESS); // Join the bus as a slave + + Wire.begin(I2C_SLAVE_ADDRESS); // Join the bus as a slave + #endif reset(); } @@ -45,33 +56,32 @@ void TWIBus::reset() { } void TWIBus::address(const uint8_t adr) { - if (!WITHIN(adr, 8, 127)) { + if (!WITHIN(adr, 8, 127)) SERIAL_ECHO_MSG("Bad I2C address (8-127)"); - } addr = adr; - debug(PSTR("address"), adr); + debug(F("address"), adr); } void TWIBus::addbyte(const char c) { if (buffer_s >= COUNT(buffer)) return; buffer[buffer_s++] = c; - debug(PSTR("addbyte"), c); + debug(F("addbyte"), c); } void TWIBus::addbytes(char src[], uint8_t bytes) { - debug(PSTR("addbytes"), bytes); + debug(F("addbytes"), bytes); while (bytes--) addbyte(*src++); } void TWIBus::addstring(char str[]) { - debug(PSTR("addstring"), str); + debug(F("addstring"), str); while (char c = *str++) addbyte(c); } void TWIBus::send() { - debug(PSTR("send"), addr); + debug(F("send"), addr); Wire.beginTransmission(I2C_ADDRESS(addr)); Wire.write(buffer, buffer_s); @@ -81,21 +91,60 @@ void TWIBus::send() { } // static -void TWIBus::echoprefix(uint8_t bytes, const char pref[], uint8_t adr) { +void TWIBus::echoprefix(uint8_t bytes, FSTR_P const pref, uint8_t adr) { SERIAL_ECHO_START(); - SERIAL_ECHOPGM_P(pref); - SERIAL_ECHOPAIR(": from:", adr, " bytes:", bytes, " data:"); + SERIAL_ECHOF(pref); + SERIAL_ECHOPGM(": from:", adr, " bytes:", bytes, " data:"); } // static -void TWIBus::echodata(uint8_t bytes, const char pref[], uint8_t adr) { +void TWIBus::echodata(uint8_t bytes, FSTR_P const pref, uint8_t adr, const uint8_t style/*=0*/) { + union TwoBytesToInt16 { uint8_t bytes[2]; int16_t integervalue; }; + TwoBytesToInt16 ConversionUnion; + echoprefix(bytes, pref, adr); - while (bytes-- && Wire.available()) SERIAL_CHAR(Wire.read()); + + while (bytes-- && Wire.available()) { + int value = Wire.read(); + switch (style) { + + // Style 1, HEX DUMP + case 1: + SERIAL_CHAR(hex_nybble((value & 0xF0) >> 4)); + SERIAL_CHAR(hex_nybble(value & 0x0F)); + if (bytes) SERIAL_CHAR(' '); + break; + + // Style 2, signed two byte integer (int16) + case 2: + if (bytes == 1) + ConversionUnion.bytes[1] = (uint8_t)value; + else if (bytes == 0) { + ConversionUnion.bytes[0] = (uint8_t)value; + // Output value in base 10 (standard decimal) + SERIAL_ECHO(ConversionUnion.integervalue); + } + break; + + // Style 3, unsigned byte, base 10 (uint8) + case 3: + SERIAL_ECHO(value); + if (bytes) SERIAL_CHAR(' '); + break; + + // Default style (zero), raw serial output + default: + // This can cause issues with some serial consoles, Pronterface is an example where things go wrong + SERIAL_CHAR(value); + break; + } + } + SERIAL_EOL(); } -void TWIBus::echobuffer(const char pref[], uint8_t adr) { - echoprefix(buffer_s, pref, adr); +void TWIBus::echobuffer(FSTR_P const prefix, uint8_t adr) { + echoprefix(buffer_s, prefix, adr); LOOP_L_N(i, buffer_s) SERIAL_CHAR(buffer[i]); SERIAL_EOL(); } @@ -103,22 +152,22 @@ void TWIBus::echobuffer(const char pref[], uint8_t adr) { bool TWIBus::request(const uint8_t bytes) { if (!addr) return false; - debug(PSTR("request"), bytes); + debug(F("request"), bytes); // requestFrom() is a blocking function if (Wire.requestFrom(I2C_ADDRESS(addr), bytes) == 0) { - debug("request fail", I2C_ADDRESS(addr)); + debug(F("request fail"), I2C_ADDRESS(addr)); return false; } return true; } -void TWIBus::relay(const uint8_t bytes) { - debug(PSTR("relay"), bytes); +void TWIBus::relay(const uint8_t bytes, const uint8_t style/*=0*/) { + debug(F("relay"), bytes); if (request(bytes)) - echodata(bytes, PSTR("i2c-reply"), addr); + echodata(bytes, F("i2c-reply"), addr, style); } uint8_t TWIBus::capture(char *dst, const uint8_t bytes) { @@ -127,7 +176,7 @@ uint8_t TWIBus::capture(char *dst, const uint8_t bytes) { while (count < bytes && Wire.available()) dst[count++] = Wire.read(); - debug(PSTR("capture"), count); + debug(F("capture"), count); return count; } @@ -140,12 +189,12 @@ void TWIBus::flush() { #if I2C_SLAVE_ADDRESS > 0 void TWIBus::receive(uint8_t bytes) { - debug(PSTR("receive"), bytes); - echodata(bytes, PSTR("i2c-receive"), 0); + debug(F("receive"), bytes); + echodata(bytes, F("i2c-receive"), 0); } void TWIBus::reply(char str[]/*=nullptr*/) { - debug(PSTR("reply"), str); + debug(F("reply"), str); if (str) { reset(); @@ -170,18 +219,16 @@ void TWIBus::flush() { #if ENABLED(DEBUG_TWIBUS) // static - void TWIBus::prefix(const char func[]) { - SERIAL_ECHOPGM("TWIBus::"); - SERIAL_ECHOPGM_P(func); - SERIAL_ECHOPGM(": "); + void TWIBus::prefix(FSTR_P const func) { + SERIAL_ECHOPGM("TWIBus::", func, ": "); } - void TWIBus::debug(const char func[], uint32_t adr) { + void TWIBus::debug(FSTR_P const func, uint32_t adr) { if (DEBUGGING(INFO)) { prefix(func); SERIAL_ECHOLN(adr); } } - void TWIBus::debug(const char func[], char c) { + void TWIBus::debug(FSTR_P const func, char c) { if (DEBUGGING(INFO)) { prefix(func); SERIAL_ECHOLN(c); } } - void TWIBus::debug(const char func[], char str[]) { + void TWIBus::debug(FSTR_P const func, char str[]) { if (DEBUGGING(INFO)) { prefix(func); SERIAL_ECHOLN(str); } } diff --git a/Marlin/src/feature/twibus.h b/Marlin/src/feature/twibus.h index 59391534824c..806e2a147a7d 100644 --- a/Marlin/src/feature/twibus.h +++ b/Marlin/src/feature/twibus.h @@ -142,7 +142,7 @@ class TWIBus { * * @param bytes the number of bytes to request */ - static void echoprefix(uint8_t bytes, const char prefix[], uint8_t adr); + static void echoprefix(uint8_t bytes, FSTR_P const prefix, uint8_t adr); /** * @brief Echo data on the bus to serial @@ -150,8 +150,9 @@ class TWIBus { * to serial in a parser-friendly format. * * @param bytes the number of bytes to request + * @param style Output format for the bytes, 0 = Raw byte [default], 1 = Hex characters, 2 = uint16_t */ - static void echodata(uint8_t bytes, const char prefix[], uint8_t adr); + static void echodata(uint8_t bytes, FSTR_P const prefix, uint8_t adr, const uint8_t style=0); /** * @brief Echo data in the buffer to serial @@ -160,7 +161,7 @@ class TWIBus { * * @param bytes the number of bytes to request */ - void echobuffer(const char prefix[], uint8_t adr); + void echobuffer(FSTR_P const prefix, uint8_t adr); /** * @brief Request data from the slave device and wait. @@ -192,10 +193,11 @@ class TWIBus { * @brief Request data from the slave device, echo to serial. * @details Request a number of bytes from a slave device and output * the returned data to serial in a parser-friendly format. + * @style Output format for the bytes, 0 = raw byte [default], 1 = Hex characters, 2 = uint16_t * * @param bytes the number of bytes to request */ - void relay(const uint8_t bytes); + void relay(const uint8_t bytes, const uint8_t style=0); #if I2C_SLAVE_ADDRESS > 0 @@ -237,17 +239,16 @@ class TWIBus { * @brief Prints a debug message * @details Prints a simple debug message "TWIBus::function: value" */ - static void prefix(const char func[]); - static void debug(const char func[], uint32_t adr); - static void debug(const char func[], char c); - static void debug(const char func[], char adr[]); - static inline void debug(const char func[], uint8_t v) { debug(func, (uint32_t)v); } + static void prefix(FSTR_P const func); + static void debug(FSTR_P const func, uint32_t adr); + static void debug(FSTR_P const func, char c); + static void debug(FSTR_P const func, char adr[]); #else - static inline void debug(const char[], uint32_t) {} - static inline void debug(const char[], char) {} - static inline void debug(const char[], char[]) {} - static inline void debug(const char[], uint8_t) {} + static void debug(FSTR_P const, uint32_t) {} + static void debug(FSTR_P const, char) {} + static void debug(FSTR_P const, char[]) {} #endif + static void debug(FSTR_P const func, uint8_t v) { debug(func, (uint32_t)v); } }; extern TWIBus i2c; diff --git a/Marlin/src/gcode/bedlevel/G26.cpp b/Marlin/src/gcode/bedlevel/G26.cpp index ed2995905589..6691d6c9ab55 100644 --- a/Marlin/src/gcode/bedlevel/G26.cpp +++ b/Marlin/src/gcode/bedlevel/G26.cpp @@ -71,8 +71,8 @@ * pliers while holding the LCD Click wheel in a depressed state. If you do not have * an LCD, you must specify a value if you use P. * - * Q # Multiplier Retraction Multiplier. Normally not needed. Retraction defaults to 1.0mm and - * un-retraction is at 1.2mm These numbers will be scaled by the specified amount + * Q # Multiplier Retraction Multiplier. (Normally not needed.) During G26 retraction will use the length + * specified by this parameter (1mm by default). Recover will be 1.2x the retract distance. * * R # Repeat Prints the number of patterns given as a parameter, starting at the current location. * If a parameter isn't given, every point will be printed unless G26 is interrupted. @@ -113,6 +113,14 @@ #include "../../module/temperature.h" #include "../../lcd/marlinui.h" +#if ENABLED(EXTENSIBLE_UI) + #include "../../lcd/extui/ui_api.h" +#endif + +#if ENABLED(UBL_HILBERT_CURVE) + #include "../../feature/bedlevel/hilbert_curve.h" +#endif + #define EXTRUSION_MULTIPLIER 1.0 #define PRIME_LENGTH 10.0 #define OOZE_AMOUNT 0.3 @@ -145,85 +153,35 @@ constexpr float g26_e_axis_feedrate = 0.025; -static MeshFlags circle_flags, horizontal_mesh_line_flags, vertical_mesh_line_flags; +static MeshFlags circle_flags; float g26_random_deviation = 0.0; -static bool g26_retracted = false; // Track the retracted state of the nozzle so mismatched - // retracts/recovers won't result in a bad state. - -float g26_extrusion_multiplier, - g26_retraction_multiplier, - g26_layer_height, - g26_prime_length; - -xy_pos_t g26_xy_pos; // = { 0, 0 } - -int16_t g26_bed_temp, - g26_hotend_temp; - -int8_t g26_prime_flag; - -#if HAS_LCD_MENU +#if HAS_MARLINUI_MENU /** * If the LCD is clicked, cancel, wait for release, return true */ bool user_canceled() { if (!ui.button_pressed()) return false; // Return if the button isn't pressed - ui.set_status_P(GET_TEXT(MSG_G26_CANCELED), 99); - TERN_(HAS_LCD_MENU, ui.quick_feedback()); + ui.set_status(GET_TEXT_F(MSG_G26_CANCELED), 99); + TERN_(HAS_MARLINUI_MENU, ui.quick_feedback()); ui.wait_for_release(); return true; } #endif -mesh_index_pair find_closest_circle_to_print(const xy_pos_t &pos) { - float closest = 99999.99; - mesh_index_pair out_point; - - out_point.pos = -1; - - GRID_LOOP(i, j) { - if (!circle_flags.marked(i, j)) { - // We found a circle that needs to be printed - const xy_pos_t m = { _GET_MESH_X(i), _GET_MESH_Y(j) }; - - // Get the distance to this intersection - float f = (pos - m).magnitude(); - - // It is possible that we are being called with the values - // to let us find the closest circle to the start position. - // But if this is not the case, add a small weighting to the - // distance calculation to help it choose a better place to continue. - f += (g26_xy_pos - m).magnitude() / 15.0f; - - // Add the specified amount of Random Noise to our search - if (g26_random_deviation > 1.0) f += random(0.0, g26_random_deviation); - - if (f < closest) { - closest = f; // Found a closer un-printed location - out_point.pos.set(i, j); // Save its data - out_point.distance = closest; - } - } - } - circle_flags.mark(out_point); // Mark this location as done. - return out_point; -} - -void move_to(const float &rx, const float &ry, const float &z, const float &e_delta) { +void move_to(const_float_t rx, const_float_t ry, const_float_t z, const_float_t e_delta) { static float last_z = -999.99; const xy_pos_t dest = { rx, ry }; - const bool has_xy_component = dest != current_position; // Check if X or Y is involved in the movement. - const bool has_e_component = e_delta != 0.0; - - destination = current_position; + const bool has_xy_component = dest != current_position, // Check if X or Y is involved in the movement. + has_e_component = e_delta != 0.0; if (z != last_z) { - last_z = destination.z = z; + last_z = z; + destination.set(current_position.x, current_position.y, z, current_position.e); const feedRate_t fr_mm_s = planner.settings.max_feedrate_mm_s[Z_AXIS] * 0.5f; // Use half of the Z_AXIS max feed rate prepare_internal_move_to_destination(fr_mm_s); } @@ -239,241 +197,286 @@ void move_to(const float &rx, const float &ry, const float &z, const float &e_de prepare_internal_move_to_destination(fr_mm_s); } -FORCE_INLINE void move_to(const xyz_pos_t &where, const float &de) { move_to(where.x, where.y, where.z, de); } +void move_to(const xyz_pos_t &where, const_float_t de) { move_to(where.x, where.y, where.z, de); } -void retract_filament(const xyz_pos_t &where) { - if (!g26_retracted) { // Only retract if we are not already retracted! - g26_retracted = true; - move_to(where, -1.0f * g26_retraction_multiplier); - } -} +typedef struct { + float extrusion_multiplier = EXTRUSION_MULTIPLIER, + retraction_multiplier = G26_RETRACT_MULTIPLIER, + layer_height = MESH_TEST_LAYER_HEIGHT, + prime_length = PRIME_LENGTH; -// TODO: Parameterize the Z lift with a define -void retract_lift_move(const xyz_pos_t &s) { - retract_filament(destination); - move_to(current_position.x, current_position.y, current_position.z + 0.5f, 0.0); // Z lift to minimize scraping - move_to(s.x, s.y, s.z + 0.5f, 0.0); // Get to the starting point with no extrusion while lifted -} + celsius_t bed_temp = MESH_TEST_BED_TEMP, + hotend_temp = MESH_TEST_HOTEND_TEMP; + + float nozzle = MESH_TEST_NOZZLE_SIZE, + filament_diameter = DEFAULT_NOMINAL_FILAMENT_DIA, + ooze_amount; // 'O' ... OOZE_AMOUNT + + bool continue_with_closest, // 'C' + keep_heaters_on; // 'K' + + xy_pos_t xy_pos; // = { 0, 0 } + + int8_t prime_flag = 0; + + bool g26_retracted = false; // Track the retracted state during G26 so mismatched + // retracts/recovers don't result in a bad state. -void recover_filament(const xyz_pos_t &where) { - if (g26_retracted) { // Only un-retract if we are retracted. - move_to(where, 1.2f * g26_retraction_multiplier); - g26_retracted = false; + void retract_filament(const xyz_pos_t &where) { + if (!g26_retracted) { // Only retract if we are not already retracted! + g26_retracted = true; + move_to(where, -1.0f * retraction_multiplier); + } } -} -/** - * print_line_from_here_to_there() takes two cartesian coordinates and draws a line from one - * to the other. But there are really three sets of coordinates involved. The first coordinate - * is the present location of the nozzle. We don't necessarily want to print from this location. - * We first need to move the nozzle to the start of line segment where we want to print. Once - * there, we can use the two coordinates supplied to draw the line. - * - * Note: Although we assume the first set of coordinates is the start of the line and the second - * set of coordinates is the end of the line, it does not always work out that way. This function - * optimizes the movement to minimize the travel distance before it can start printing. This saves - * a lot of time and eliminates a lot of nonsensical movement of the nozzle. However, it does - * cause a lot of very little short retracement of th nozzle when it draws the very first line - * segment of a 'circle'. The time this requires is very short and is easily saved by the other - * cases where the optimization comes into play. - */ -void print_line_from_here_to_there(const xyz_pos_t &s, const xyz_pos_t &e) { + // TODO: Parameterize the Z lift with a define + void retract_lift_move(const xyz_pos_t &s) { + retract_filament(destination); + move_to(current_position.x, current_position.y, current_position.z + 0.5f, 0.0f); // Z lift to minimize scraping + move_to(s.x, s.y, s.z + 0.5f, 0.0f); // Get to the starting point with no extrusion while lifted + } - // Distances to the start / end of the line - xy_float_t svec = current_position - s, evec = current_position - e; + void recover_filament(const xyz_pos_t &where) { + if (g26_retracted) { // Only un-retract if we are retracted. + move_to(where, 1.2f * retraction_multiplier); + g26_retracted = false; + } + } - const float dist_start = HYPOT2(svec.x, svec.y), - dist_end = HYPOT2(evec.x, evec.y), - line_length = HYPOT(e.x - s.x, e.y - s.y); + /** + * print_line_from_here_to_there() takes two cartesian coordinates and draws a line from one + * to the other. But there are really three sets of coordinates involved. The first coordinate + * is the present location of the nozzle. We don't necessarily want to print from this location. + * We first need to move the nozzle to the start of line segment where we want to print. Once + * there, we can use the two coordinates supplied to draw the line. + * + * Note: Although we assume the first set of coordinates is the start of the line and the second + * set of coordinates is the end of the line, it does not always work out that way. This function + * optimizes the movement to minimize the travel distance before it can start printing. This saves + * a lot of time and eliminates a lot of nonsensical movement of the nozzle. However, it does + * cause a lot of very little short retracement of th nozzle when it draws the very first line + * segment of a 'circle'. The time this requires is very short and is easily saved by the other + * cases where the optimization comes into play. + */ + void print_line_from_here_to_there(const xyz_pos_t &s, const xyz_pos_t &e) { - // If the end point of the line is closer to the nozzle, flip the direction, - // moving from the end to the start. On very small lines the optimization isn't worth it. - if (dist_end < dist_start && (INTERSECTION_CIRCLE_RADIUS) < ABS(line_length)) - return print_line_from_here_to_there(e, s); + // Distances to the start / end of the line + xy_float_t svec = current_position - s, evec = current_position - e; - // Decide whether to retract & lift - if (dist_start > 2.0) retract_lift_move(s); + const float dist_start = HYPOT2(svec.x, svec.y), + dist_end = HYPOT2(evec.x, evec.y), + line_length = HYPOT(e.x - s.x, e.y - s.y); - move_to(s, 0.0); // Get to the starting point with no extrusion / un-Z lift + // If the end point of the line is closer to the nozzle, flip the direction, + // moving from the end to the start. On very small lines the optimization isn't worth it. + if (dist_end < dist_start && (INTERSECTION_CIRCLE_RADIUS) < ABS(line_length)) + return print_line_from_here_to_there(e, s); - const float e_pos_delta = line_length * g26_e_axis_feedrate * g26_extrusion_multiplier; + // Decide whether to retract & lift + if (dist_start > 2.0) retract_lift_move(s); - recover_filament(destination); - move_to(e, e_pos_delta); // Get to the ending point with an appropriate amount of extrusion -} + move_to(s, 0.0); // Get to the starting point with no extrusion / un-Z lift + + const float e_pos_delta = line_length * g26_e_axis_feedrate * extrusion_multiplier; -inline bool look_for_lines_to_connect() { - xyz_pos_t s, e; - s.z = e.z = g26_layer_height; + recover_filament(destination); + move_to(e, e_pos_delta); // Get to the ending point with an appropriate amount of extrusion + } - GRID_LOOP(i, j) { + void connect_neighbor_with_line(const xy_int8_t &p1, int8_t dx, int8_t dy) { + xy_int8_t p2; + p2.x = p1.x + dx; + p2.y = p1.y + dy; + + if (p2.x < 0 || p2.x >= (GRID_MAX_POINTS_X)) return; + if (p2.y < 0 || p2.y >= (GRID_MAX_POINTS_Y)) return; + + if (circle_flags.marked(p1.x, p1.y) && circle_flags.marked(p2.x, p2.y)) { + xyz_pos_t s, e; + s.x = _GET_MESH_X(p1.x) + (INTERSECTION_CIRCLE_RADIUS - (CROSSHAIRS_SIZE)) * dx; + e.x = _GET_MESH_X(p2.x) - (INTERSECTION_CIRCLE_RADIUS - (CROSSHAIRS_SIZE)) * dx; + s.y = _GET_MESH_Y(p1.y) + (INTERSECTION_CIRCLE_RADIUS - (CROSSHAIRS_SIZE)) * dy; + e.y = _GET_MESH_Y(p2.y) - (INTERSECTION_CIRCLE_RADIUS - (CROSSHAIRS_SIZE)) * dy; + s.z = e.z = layer_height; + + #if HAS_ENDSTOPS + LIMIT(s.y, Y_MIN_POS + 1, Y_MAX_POS - 1); + LIMIT(e.y, Y_MIN_POS + 1, Y_MAX_POS - 1); + LIMIT(s.x, X_MIN_POS + 1, X_MAX_POS - 1); + LIMIT(e.x, X_MIN_POS + 1, X_MAX_POS - 1); + #endif - if (TERN0(HAS_LCD_MENU, user_canceled())) return true; + if (position_is_reachable(s.x, s.y) && position_is_reachable(e.x, e.y)) + print_line_from_here_to_there(s, e); + } + } - if (i < (GRID_MAX_POINTS_X)) { // Can't connect to anything farther to the right than GRID_MAX_POINTS_X. - // Already a half circle at the edge of the bed. + /** + * Turn on the bed and nozzle heat and + * wait for them to get up to temperature. + */ + bool turn_on_heaters() { - if (circle_flags.marked(i, j) && circle_flags.marked(i + 1, j)) { // Test whether a leftward line can be done - if (!horizontal_mesh_line_flags.marked(i, j)) { - // Two circles need a horizontal line to connect them - s.x = _GET_MESH_X( i ) + (INTERSECTION_CIRCLE_RADIUS - (CROSSHAIRS_SIZE)); // right edge - e.x = _GET_MESH_X(i + 1) - (INTERSECTION_CIRCLE_RADIUS - (CROSSHAIRS_SIZE)); // left edge + SERIAL_ECHOLNPGM("Waiting for heatup."); - #if HAS_ENDSTOPS - LIMIT(s.x, X_MIN_POS + 1, X_MAX_POS - 1); - s.y = e.y = constrain(_GET_MESH_Y(j), Y_MIN_POS + 1, Y_MAX_POS - 1); - LIMIT(e.x, X_MIN_POS + 1, X_MAX_POS - 1); - #else - s.y = e.y = _GET_MESH_Y(j); - #endif + #if HAS_HEATED_BED - if (position_is_reachable(s.x, s.y) && position_is_reachable(e.x, e.y)) - print_line_from_here_to_there(s, e); + if (bed_temp > 25) { + #if HAS_WIRED_LCD + ui.set_status(GET_TEXT_F(MSG_G26_HEATING_BED), 99); + ui.quick_feedback(); + TERN_(HAS_MARLINUI_MENU, ui.capture()); + #endif + thermalManager.setTargetBed(bed_temp); - horizontal_mesh_line_flags.mark(i, j); // Mark done, even if skipped - } + // Wait for the temperature to stabilize + if (!thermalManager.wait_for_bed(true OPTARG(G26_CLICK_CAN_CANCEL, true))) + return G26_ERR; } - if (j < (GRID_MAX_POINTS_Y)) { // Can't connect to anything further back than GRID_MAX_POINTS_Y. - // Already a half circle at the edge of the bed. + #else - if (circle_flags.marked(i, j) && circle_flags.marked(i, j + 1)) { // Test whether a downward line can be done - if (!vertical_mesh_line_flags.marked(i, j)) { - // Two circles that need a vertical line to connect them - s.y = _GET_MESH_Y( j ) + (INTERSECTION_CIRCLE_RADIUS - (CROSSHAIRS_SIZE)); // top edge - e.y = _GET_MESH_Y(j + 1) - (INTERSECTION_CIRCLE_RADIUS - (CROSSHAIRS_SIZE)); // bottom edge + UNUSED(bed_temp); - #if HAS_ENDSTOPS - s.x = e.x = constrain(_GET_MESH_X(i), X_MIN_POS + 1, X_MAX_POS - 1); - LIMIT(s.y, Y_MIN_POS + 1, Y_MAX_POS - 1); - LIMIT(e.y, Y_MIN_POS + 1, Y_MAX_POS - 1); - #else - s.x = e.x = _GET_MESH_X(i); - #endif + #endif // HAS_HEATED_BED - if (position_is_reachable(s.x, s.y) && position_is_reachable(e.x, e.y)) - print_line_from_here_to_there(s, e); + // Start heating the active nozzle + #if HAS_WIRED_LCD + ui.set_status(GET_TEXT_F(MSG_G26_HEATING_NOZZLE), 99); + ui.quick_feedback(); + #endif + thermalManager.setTargetHotend(hotend_temp, active_extruder); - vertical_mesh_line_flags.mark(i, j); // Mark done, even if skipped - } - } - } - } + // Wait for the temperature to stabilize + if (!thermalManager.wait_for_hotend(active_extruder, true OPTARG(G26_CLICK_CAN_CANCEL, true))) + return G26_ERR; + + #if HAS_WIRED_LCD + ui.reset_status(); + ui.quick_feedback(); + #endif + + return G26_OK; } - return false; -} -/** - * Turn on the bed and nozzle heat and - * wait for them to get up to temperature. - */ -inline bool turn_on_heaters() { + /** + * Prime the nozzle if needed. Return true on error. + */ + bool prime_nozzle() { - SERIAL_ECHOLNPGM("Waiting for heatup."); + const feedRate_t fr_slow_e = planner.settings.max_feedrate_mm_s[E_AXIS] / 15.0f; + #if HAS_MARLINUI_MENU && !HAS_TOUCH_BUTTONS // ui.button_pressed issue with touchscreen + #if ENABLED(PREVENT_LENGTHY_EXTRUDE) + float Total_Prime = 0.0; + #endif - #if HAS_HEATED_BED + if (prime_flag == -1) { // The user wants to control how much filament gets purged + ui.capture(); + ui.set_status(GET_TEXT_F(MSG_G26_MANUAL_PRIME), 99); + ui.chirp(); + + destination = current_position; - if (g26_bed_temp > 25) { + recover_filament(destination); // Make sure G26 doesn't think the filament is retracted(). + + while (!ui.button_pressed()) { + ui.chirp(); + destination.e += 0.25; + #if ENABLED(PREVENT_LENGTHY_EXTRUDE) + Total_Prime += 0.25; + if (Total_Prime >= EXTRUDE_MAXLENGTH) { + ui.release(); + return G26_ERR; + } + #endif + prepare_internal_move_to_destination(fr_slow_e); + destination = current_position; + planner.synchronize(); // Without this synchronize, the purge is more consistent, + // but because the planner has a buffer, we won't be able + // to stop as quickly. So we put up with the less smooth + // action to give the user a more responsive 'Stop'. + } + + ui.wait_for_release(); + + ui.set_status(GET_TEXT_F(MSG_G26_PRIME_DONE), 99); + ui.quick_feedback(); + ui.release(); + } + else + #endif + { #if HAS_WIRED_LCD - ui.set_status_P(GET_TEXT(MSG_G26_HEATING_BED), 99); + ui.set_status(GET_TEXT_F(MSG_G26_FIXED_LENGTH), 99); ui.quick_feedback(); - TERN_(HAS_LCD_MENU, ui.capture()); #endif - thermalManager.setTargetBed(g26_bed_temp); - - // Wait for the temperature to stabilize - if (!thermalManager.wait_for_bed(true - #if G26_CLICK_CAN_CANCEL - , true - #endif - ) - ) return G26_ERR; + destination = current_position; + destination.e += prime_length; + prepare_internal_move_to_destination(fr_slow_e); + destination.e -= prime_length; + retract_filament(destination); } - #endif // HAS_HEATED_BED + return G26_OK; + } - // Start heating the active nozzle - #if HAS_WIRED_LCD - ui.set_status_P(GET_TEXT(MSG_G26_HEATING_NOZZLE), 99); - ui.quick_feedback(); - #endif - thermalManager.setTargetHotend(g26_hotend_temp, active_extruder); + /** + * Find the nearest point at which to print a circle + */ + mesh_index_pair find_closest_circle_to_print(const xy_pos_t &pos) { - // Wait for the temperature to stabilize - if (!thermalManager.wait_for_hotend(active_extruder, true - #if G26_CLICK_CAN_CANCEL - , true - #endif - )) return G26_ERR; + mesh_index_pair out_point; + out_point.pos = -1; - #if HAS_WIRED_LCD - ui.reset_status(); - ui.quick_feedback(); - #endif + #if ENABLED(UBL_HILBERT_CURVE) - return G26_OK; -} + auto test_func = [](uint8_t i, uint8_t j, void *data) -> bool { + if (!circle_flags.marked(i, j)) { + mesh_index_pair *out_point = (mesh_index_pair*)data; + out_point->pos.set(i, j); // Save its data + return true; + } + return false; + }; -/** - * Prime the nozzle if needed. Return true on error. - */ -inline bool prime_nozzle() { + hilbert_curve::search_from_closest(pos, test_func, &out_point); - const feedRate_t fr_slow_e = planner.settings.max_feedrate_mm_s[E_AXIS] / 15.0f; - #if HAS_LCD_MENU && !HAS_TOUCH_BUTTONS // ui.button_pressed issue with touchscreen - #if ENABLED(PREVENT_LENGTHY_EXTRUDE) - float Total_Prime = 0.0; - #endif + #else - if (g26_prime_flag == -1) { // The user wants to control how much filament gets purged - ui.capture(); - ui.set_status_P(GET_TEXT(MSG_G26_MANUAL_PRIME), 99); - ui.chirp(); + float closest = 99999.99; - destination = current_position; + GRID_LOOP(i, j) { + if (!circle_flags.marked(i, j)) { + // We found a circle that needs to be printed + const xy_pos_t m = { _GET_MESH_X(i), _GET_MESH_Y(j) }; - recover_filament(destination); // Make sure G26 doesn't think the filament is retracted(). + // Get the distance to this intersection + float f = (pos - m).magnitude(); - while (!ui.button_pressed()) { - ui.chirp(); - destination.e += 0.25; - #if ENABLED(PREVENT_LENGTHY_EXTRUDE) - Total_Prime += 0.25; - if (Total_Prime >= EXTRUDE_MAXLENGTH) { - ui.release(); - return G26_ERR; + // It is possible that we are being called with the values + // to let us find the closest circle to the start position. + // But if this is not the case, add a small weighting to the + // distance calculation to help it choose a better place to continue. + f += (xy_pos - m).magnitude() / 15.0f; + + // Add the specified amount of Random Noise to our search + if (g26_random_deviation > 1.0) f += random(0.0, g26_random_deviation); + + if (f < closest) { + closest = f; // Found a closer un-printed location + out_point.pos.set(i, j); // Save its data + out_point.distance = closest; } - #endif - prepare_internal_move_to_destination(fr_slow_e); - destination = current_position; - planner.synchronize(); // Without this synchronize, the purge is more consistent, - // but because the planner has a buffer, we won't be able - // to stop as quickly. So we put up with the less smooth - // action to give the user a more responsive 'Stop'. + } } - ui.wait_for_release(); - - ui.set_status_P(GET_TEXT(MSG_G26_PRIME_DONE), 99); - ui.quick_feedback(); - ui.release(); - } - else - #endif - { - #if HAS_WIRED_LCD - ui.set_status_P(GET_TEXT(MSG_G26_FIXED_LENGTH), 99); - ui.quick_feedback(); #endif - destination = current_position; - destination.e += g26_prime_length; - prepare_internal_move_to_destination(fr_slow_e); - destination.e -= g26_prime_length; - retract_filament(destination); + + circle_flags.mark(out_point); // Mark this location as done. + return out_point; } - return G26_OK; -} +} g26_helper_t; /** * G26: Mesh Validation Pattern generation. @@ -510,33 +513,24 @@ void GcodeSuite::G26() { // Change the tool first, if specified if (parser.seenval('T')) tool_change(parser.value_int()); - g26_extrusion_multiplier = EXTRUSION_MULTIPLIER; - g26_retraction_multiplier = G26_RETRACT_MULTIPLIER; - g26_layer_height = MESH_TEST_LAYER_HEIGHT; - g26_prime_length = PRIME_LENGTH; - g26_bed_temp = MESH_TEST_BED_TEMP; - g26_hotend_temp = MESH_TEST_HOTEND_TEMP; - g26_prime_flag = 0; + g26_helper_t g26; - float g26_nozzle = MESH_TEST_NOZZLE_SIZE, - g26_filament_diameter = DEFAULT_NOMINAL_FILAMENT_DIA, - g26_ooze_amount = parser.linearval('O', OOZE_AMOUNT); - - bool g26_continue_with_closest = parser.boolval('C'), - g26_keep_heaters_on = parser.boolval('K'); + g26.ooze_amount = parser.linearval('O', OOZE_AMOUNT); + g26.continue_with_closest = parser.boolval('C'); + g26.keep_heaters_on = parser.boolval('K'); // Accept 'I' if temperature presets are defined - #if PREHEAT_COUNT + #if HAS_PREHEAT const uint8_t preset_index = parser.seenval('I') ? _MIN(parser.value_byte(), PREHEAT_COUNT - 1) + 1 : 0; #endif #if HAS_HEATED_BED // Get a temperature from 'I' or 'B' - int16_t bedtemp = 0; + celsius_t bedtemp = 0; // Use the 'I' index if temperature presets are defined - #if PREHEAT_COUNT + #if HAS_PREHEAT if (preset_index) bedtemp = ui.material_preset[preset_index - 1].bed_temp; #endif @@ -545,17 +539,17 @@ void GcodeSuite::G26() { if (bedtemp) { if (!WITHIN(bedtemp, 40, BED_MAX_TARGET)) { - SERIAL_ECHOLNPAIR("?Specified bed temperature not plausible (40-", BED_MAX_TARGET, "C)."); + SERIAL_ECHOLNPGM("?Specified bed temperature not plausible (40-", BED_MAX_TARGET, "C)."); return; } - g26_bed_temp = bedtemp; + g26.bed_temp = bedtemp; } #endif // HAS_HEATED_BED if (parser.seenval('L')) { - g26_layer_height = parser.value_linear_units(); - if (!WITHIN(g26_layer_height, 0.0, 2.0)) { + g26.layer_height = parser.value_linear_units(); + if (!WITHIN(g26.layer_height, 0.0, 2.0)) { SERIAL_ECHOLNPGM("?Specified layer height not plausible."); return; } @@ -563,8 +557,8 @@ void GcodeSuite::G26() { if (parser.seen('Q')) { if (parser.has_value()) { - g26_retraction_multiplier = parser.value_float(); - if (!WITHIN(g26_retraction_multiplier, 0.05, 15.0)) { + g26.retraction_multiplier = parser.value_float(); + if (!WITHIN(g26.retraction_multiplier, 0.05, 15.0)) { SERIAL_ECHOLNPGM("?Specified Retraction Multiplier not plausible."); return; } @@ -576,8 +570,8 @@ void GcodeSuite::G26() { } if (parser.seenval('S')) { - g26_nozzle = parser.value_float(); - if (!WITHIN(g26_nozzle, 0.1, 2.0)) { + g26.nozzle = parser.value_float(); + if (!WITHIN(g26.nozzle, 0.1, 2.0)) { SERIAL_ECHOLNPGM("?Specified nozzle size not plausible."); return; } @@ -585,17 +579,17 @@ void GcodeSuite::G26() { if (parser.seen('P')) { if (!parser.has_value()) { - #if HAS_LCD_MENU - g26_prime_flag = -1; + #if HAS_MARLINUI_MENU + g26.prime_flag = -1; #else SERIAL_ECHOLNPGM("?Prime length must be specified when not using an LCD."); return; #endif } else { - g26_prime_flag++; - g26_prime_length = parser.value_linear_units(); - if (!WITHIN(g26_prime_length, 0.0, 25.0)) { + g26.prime_flag++; + g26.prime_length = parser.value_linear_units(); + if (!WITHIN(g26.prime_length, 0.0, 25.0)) { SERIAL_ECHOLNPGM("?Specified prime length not plausible."); return; } @@ -603,23 +597,23 @@ void GcodeSuite::G26() { } if (parser.seenval('F')) { - g26_filament_diameter = parser.value_linear_units(); - if (!WITHIN(g26_filament_diameter, 1.0, 4.0)) { + g26.filament_diameter = parser.value_linear_units(); + if (!WITHIN(g26.filament_diameter, 1.0, 4.0)) { SERIAL_ECHOLNPGM("?Specified filament size not plausible."); return; } } - g26_extrusion_multiplier *= sq(1.75) / sq(g26_filament_diameter); // If we aren't using 1.75mm filament, we need to + g26.extrusion_multiplier *= sq(1.75) / sq(g26.filament_diameter); // If we aren't using 1.75mm filament, we need to // scale up or down the length needed to get the // same volume of filament - g26_extrusion_multiplier *= g26_filament_diameter * sq(g26_nozzle) / sq(0.3); // Scale up by nozzle size + g26.extrusion_multiplier *= g26.filament_diameter * sq(g26.nozzle) / sq(0.3); // Scale up by nozzle size // Get a temperature from 'I' or 'H' - int16_t noztemp = 0; + celsius_t noztemp = 0; // Accept 'I' if temperature presets are defined - #if PREHEAT_COUNT + #if HAS_PREHEAT if (preset_index) noztemp = ui.material_preset[preset_index - 1].hotend_temp; #endif @@ -632,7 +626,7 @@ void GcodeSuite::G26() { SERIAL_ECHOLNPGM("?Specified nozzle temperature not plausible."); return; } - g26_hotend_temp = noztemp; + g26.hotend_temp = noztemp; } // 'U' to Randomize and optionally set circle deviation @@ -644,15 +638,15 @@ void GcodeSuite::G26() { // Get repeat from 'R', otherwise do one full circuit int16_t g26_repeats; - #if HAS_LCD_MENU + #if HAS_MARLINUI_MENU g26_repeats = parser.intval('R', GRID_MAX_POINTS + 1); #else - if (!parser.seen('R')) { + if (parser.seen('R')) + g26_repeats = parser.has_value() ? parser.value_int() : GRID_MAX_POINTS + 1; + else { SERIAL_ECHOLNPGM("?(R)epeat must be specified when not using an LCD."); return; } - else - g26_repeats = parser.has_value() ? parser.value_int() : GRID_MAX_POINTS + 1; #endif if (g26_repeats < 1) { SERIAL_ECHOLNPGM("?(R)epeat value not plausible; must be at least 1."); @@ -660,9 +654,9 @@ void GcodeSuite::G26() { } // Set a position with 'X' and/or 'Y'. Default: current_position - g26_xy_pos.set(parser.seenval('X') ? RAW_X_POSITION(parser.value_linear_units()) : current_position.x, + g26.xy_pos.set(parser.seenval('X') ? RAW_X_POSITION(parser.value_linear_units()) : current_position.x, parser.seenval('Y') ? RAW_Y_POSITION(parser.value_linear_units()) : current_position.y); - if (!position_is_reachable(g26_xy_pos)) { + if (!position_is_reachable(g26.xy_pos)) { SERIAL_ECHOLNPGM("?Specified X,Y coordinate out of bounds."); return; } @@ -670,7 +664,7 @@ void GcodeSuite::G26() { /** * Wait until all parameters are verified before altering the state! */ - set_bed_leveling_enabled(!parser.seen('D')); + set_bed_leveling_enabled(!parser.seen_test('D')); do_z_clearance(Z_CLEARANCE_BETWEEN_PROBES); @@ -680,12 +674,12 @@ void GcodeSuite::G26() { planner.calculate_volumetric_multipliers(); #endif - if (turn_on_heaters() != G26_OK) goto LEAVE; + if (g26.turn_on_heaters() != G26_OK) goto LEAVE; current_position.e = 0.0; sync_plan_position_e(); - if (g26_prime_flag && prime_nozzle() != G26_OK) goto LEAVE; + if (g26.prime_flag && g26.prime_nozzle() != G26_OK) goto LEAVE; /** * Bed is preheated @@ -698,16 +692,14 @@ void GcodeSuite::G26() { */ circle_flags.reset(); - horizontal_mesh_line_flags.reset(); - vertical_mesh_line_flags.reset(); // Move nozzle to the specified height for the first layer destination = current_position; - destination.z = g26_layer_height; + destination.z = g26.layer_height; move_to(destination, 0.0); - move_to(destination, g26_ooze_amount); + move_to(destination, g26.ooze_amount); - TERN_(HAS_LCD_MENU, ui.capture()); + TERN_(HAS_MARLINUI_MENU, ui.capture()); #if DISABLED(ARC_SUPPORT) @@ -730,11 +722,13 @@ void GcodeSuite::G26() { #endif // !ARC_SUPPORT mesh_index_pair location; + TERN_(EXTENSIBLE_UI, ExtUI::onMeshUpdate(location.pos, ExtUI::G26_START)); do { // Find the nearest confluence - location = find_closest_circle_to_print(g26_continue_with_closest ? xy_pos_t(current_position) : g26_xy_pos); + location = g26.find_closest_circle_to_print(g26.continue_with_closest ? xy_pos_t(current_position) : g26.xy_pos); if (location.valid()) { + TERN_(EXTENSIBLE_UI, ExtUI::onMeshUpdate(location.pos, ExtUI::G26_POINT_START)); const xy_pos_t circle = _GET_MESH_POS(location.pos); // If this mesh location is outside the printable radius, skip it. @@ -762,7 +756,7 @@ void GcodeSuite::G26() { if (!b) { e.x = circle.x; e.y += INTERSECTION_CIRCLE_RADIUS; } arc_length = (f || b) ? ARC_LENGTH(1) : ARC_LENGTH(2); } - else if (r) { // right edge + else if (r) { // right edge if (b) s.set(circle.x - (INTERSECTION_CIRCLE_RADIUS), circle.y); else s.set(circle.x, circle.y + INTERSECTION_CIRCLE_RADIUS); if (f) e.set(circle.x - (INTERSECTION_CIRCLE_RADIUS), circle.y); @@ -782,27 +776,26 @@ void GcodeSuite::G26() { const xy_float_t dist = current_position - s; // Distance from the start of the actual circle const float dist_start = HYPOT2(dist.x, dist.y); const xyze_pos_t endpoint = { - e.x, e.y, g26_layer_height, - current_position.e + (arc_length * g26_e_axis_feedrate * g26_extrusion_multiplier) + e.x, e.y, g26.layer_height, + current_position.e + (arc_length * g26_e_axis_feedrate * g26.extrusion_multiplier) }; if (dist_start > 2.0) { - s.z = g26_layer_height + 0.5f; - retract_lift_move(s); + s.z = g26.layer_height + 0.5f; + g26.retract_lift_move(s); } - s.z = g26_layer_height; + s.z = g26.layer_height; move_to(s, 0.0); // Get to the starting point with no extrusion / un-Z lift - recover_filament(destination); + g26.recover_filament(destination); - const feedRate_t old_feedrate = feedrate_mm_s; - feedrate_mm_s = PLANNER_XY_FEEDRATE() * 0.1f; - plan_arc(endpoint, arc_offset, false, 0); // Draw a counter-clockwise arc - feedrate_mm_s = old_feedrate; - destination = current_position; + { REMEMBER(fr, feedrate_mm_s, PLANNER_XY_FEEDRATE() * 0.1f); + plan_arc(endpoint, arc_offset, false, 0); // Draw a counter-clockwise arc + destination = current_position; + } - if (TERN0(HAS_LCD_MENU, user_canceled())) goto LEAVE; // Check if the user wants to stop the Mesh Validation + if (TERN0(HAS_MARLINUI_MENU, user_canceled())) goto LEAVE; // Check if the user wants to stop the Mesh Validation #else // !ARC_SUPPORT @@ -826,10 +819,10 @@ void GcodeSuite::G26() { for (int8_t ind = start_ind; ind <= end_ind; ind++) { - if (TERN0(HAS_LCD_MENU, user_canceled())) goto LEAVE; // Check if the user wants to stop the Mesh Validation + if (TERN0(HAS_MARLINUI_MENU, user_canceled())) goto LEAVE; // Check if the user wants to stop the Mesh Validation - xyz_float_t p = { circle.x + _COS(ind ), circle.y + _SIN(ind ), g26_layer_height }, - q = { circle.x + _COS(ind + 1), circle.y + _SIN(ind + 1), g26_layer_height }; + xyz_float_t p = { circle.x + _COS(ind ), circle.y + _SIN(ind ), g26.layer_height }, + q = { circle.x + _COS(ind + 1), circle.y + _SIN(ind + 1), g26.layer_height }; #if IS_KINEMATIC // Check to make sure this segment is entirely on the bed, skip if not. @@ -841,13 +834,19 @@ void GcodeSuite::G26() { LIMIT(q.y, Y_MIN_POS + 1, Y_MAX_POS - 1); #endif - print_line_from_here_to_there(p, q); + g26.print_line_from_here_to_there(p, q); SERIAL_FLUSH(); // Prevent host M105 buffer overrun. } #endif // !ARC_SUPPORT - if (look_for_lines_to_connect()) goto LEAVE; + g26.connect_neighbor_with_line(location.pos, -1, 0); + g26.connect_neighbor_with_line(location.pos, 1, 0); + g26.connect_neighbor_with_line(location.pos, 0, -1); + g26.connect_neighbor_with_line(location.pos, 0, 1); + planner.synchronize(); + TERN_(EXTENSIBLE_UI, ExtUI::onMeshUpdate(location.pos, ExtUI::G26_POINT_FINISH)); + if (TERN0(HAS_MARLINUI_MENU, user_canceled())) goto LEAVE; } SERIAL_FLUSH(); // Prevent host M105 buffer overrun. @@ -855,23 +854,21 @@ void GcodeSuite::G26() { } while (--g26_repeats && location.valid()); LEAVE: - ui.set_status_P(GET_TEXT(MSG_G26_LEAVING), -1); + ui.set_status(GET_TEXT_F(MSG_G26_LEAVING), -1); + TERN_(EXTENSIBLE_UI, ExtUI::onMeshUpdate(location, ExtUI::G26_FINISH)); - retract_filament(destination); + g26.retract_filament(destination); destination.z = Z_CLEARANCE_BETWEEN_PROBES; - move_to(destination, 0); // Raise the nozzle - - destination = g26_xy_pos; // Move back to the starting XY position - move_to(destination, 0); // Move back to the starting position + move_to(destination, 0); // Raise the nozzle #if DISABLED(NO_VOLUMETRICS) parser.volumetric_enabled = volumetric_was_enabled; planner.calculate_volumetric_multipliers(); #endif - TERN_(HAS_LCD_MENU, ui.release()); // Give back control of the LCD + TERN_(HAS_MARLINUI_MENU, ui.release()); // Give back control of the LCD - if (!g26_keep_heaters_on) { + if (!g26.keep_heaters_on) { TERN_(HAS_HEATED_BED, thermalManager.setTargetBed(0)); thermalManager.setTargetHotend(active_extruder, 0); } diff --git a/Marlin/src/gcode/bedlevel/G35.cpp b/Marlin/src/gcode/bedlevel/G35.cpp index 88f02e6de2f9..dd828bf0c873 100644 --- a/Marlin/src/gcode/bedlevel/G35.cpp +++ b/Marlin/src/gcode/bedlevel/G35.cpp @@ -33,6 +33,10 @@ #include "../../module/tool_change.h" #endif +#if ENABLED(BLTOUCH) + #include "../../feature/bltouch.h" +#endif + #define DEBUG_OUT ENABLED(DEBUG_LEVELING_FEATURE) #include "../../core/debug_out.h" @@ -72,7 +76,9 @@ void GcodeSuite::G35() { // Disable the leveling matrix before auto-aligning #if HAS_LEVELING - TERN_(RESTORE_LEVELING_AFTER_G35, const bool leveling_was_active = planner.leveling_active); + #if ENABLED(RESTORE_LEVELING_AFTER_G35) + const bool leveling_was_active = planner.leveling_active; + #endif set_bed_leveling_enabled(false); #endif @@ -89,8 +95,8 @@ void GcodeSuite::G35() { // Disable duplication mode on homing TERN_(HAS_DUPLICATION_MODE, set_duplication_enabled(false)); - // Home all before this procedure - home_all_axes(); + // Home only Z axis when X and Y is trusted, otherwise all axes, if needed before this procedure + if (!all_axes_trusted()) process_subcommands_now(F("G28Z")); bool err_break = false; @@ -100,23 +106,25 @@ void GcodeSuite::G35() { // In BLTOUCH HS mode, the probe travels in a deployed state. // Users of G35 might have a badly misaligned bed, so raise Z by the // length of the deployed pin (BLTOUCH stroke < 7mm) - do_blocking_move_to_z((Z_CLEARANCE_BETWEEN_PROBES) + TERN0(BLTOUCH_HS_MODE, 7)); - const float z_probed_height = probe.probe_at_point(screws_tilt_adjust_pos[i], PROBE_PT_RAISE, 0, true); + + // Unsure if this is even required. The probe seems to lift correctly after probe done. + do_blocking_move_to_z(SUM_TERN(BLTOUCH, Z_CLEARANCE_BETWEEN_PROBES, bltouch.z_extra_clearance())); + const float z_probed_height = probe.probe_at_point(tramming_points[i], PROBE_PT_RAISE, 0, true); if (isnan(z_probed_height)) { - SERIAL_ECHOPAIR("G35 failed at point ", i, " ("); + SERIAL_ECHOPGM("G35 failed at point ", i + 1, " ("); SERIAL_ECHOPGM_P((char *)pgm_read_ptr(&tramming_point_name[i])); SERIAL_CHAR(')'); - SERIAL_ECHOLNPAIR_P(SP_X_STR, screws_tilt_adjust_pos[i].x, SP_Y_STR, screws_tilt_adjust_pos[i].y); + SERIAL_ECHOLNPGM_P(SP_X_STR, tramming_points[i].x, SP_Y_STR, tramming_points[i].y); err_break = true; break; } if (DEBUGGING(LEVELING)) { - DEBUG_ECHOPAIR("Probing point ", i, " ("); - DEBUG_ECHOPGM_P((char *)pgm_read_ptr(&tramming_point_name[i])); + DEBUG_ECHOPGM("Probing point ", i + 1, " ("); + DEBUG_ECHOF(FPSTR(pgm_read_ptr(&tramming_point_name[i]))); DEBUG_CHAR(')'); - DEBUG_ECHOLNPAIR_P(SP_X_STR, screws_tilt_adjust_pos[i].x, SP_Y_STR, screws_tilt_adjust_pos[i].y, SP_Z_STR, z_probed_height); + DEBUG_ECHOLNPGM_P(SP_X_STR, tramming_points[i].x, SP_Y_STR, tramming_points[i].y, SP_Z_STR, z_probed_height); } z_measured[i] = z_probed_height; @@ -128,7 +136,7 @@ void GcodeSuite::G35() { // Calculate adjusts LOOP_S_L_N(i, 1, G35_PROBE_COUNT) { const float diff = z_measured[0] - z_measured[i], - adjust = abs(diff) < 0.001f ? 0 : diff / threads_factor[(screw_thread - 30) / 10]; + adjust = ABS(diff) < 0.001f ? 0 : diff / threads_factor[(screw_thread - 30) / 10]; const int full_turns = trunc(adjust); const float decimal_part = adjust - float(full_turns); @@ -136,9 +144,9 @@ void GcodeSuite::G35() { SERIAL_ECHOPGM("Turn "); SERIAL_ECHOPGM_P((char *)pgm_read_ptr(&tramming_point_name[i])); - SERIAL_ECHOPAIR(" ", (screw_thread & 1) == (adjust > 0) ? "CCW" : "CW", " by ", abs(full_turns), " turns"); - if (minutes) SERIAL_ECHOPAIR(" and ", abs(minutes), " minutes"); - if (ENABLED(REPORT_TRAMMING_MM)) SERIAL_ECHOPAIR(" (", -diff, "mm)"); + SERIAL_ECHOPGM(" ", (screw_thread & 1) == (adjust > 0) ? "CCW" : "CW", " by ", ABS(full_turns), " turns"); + if (minutes) SERIAL_ECHOPGM(" and ", ABS(minutes), " minutes"); + if (ENABLED(REPORT_TRAMMING_MM)) SERIAL_ECHOPGM(" (", -diff, "mm)"); SERIAL_EOL(); } } @@ -147,7 +155,7 @@ void GcodeSuite::G35() { // Restore the active tool after homing #if HAS_MULTI_HOTEND - tool_change(old_tool_index, DISABLED(PARKING_EXTRUDER)); // Fetch previous toolhead if not PARKING_EXTRUDER + if (old_tool_index != 0) tool_change(old_tool_index, DISABLED(PARKING_EXTRUDER)); // Fetch previous toolhead if not PARKING_EXTRUDER #endif #if BOTH(HAS_LEVELING, RESTORE_LEVELING_AFTER_G35) diff --git a/Marlin/src/gcode/bedlevel/M420.cpp b/Marlin/src/gcode/bedlevel/M420.cpp index 96122c18f882..3c23e85a1dfb 100644 --- a/Marlin/src/gcode/bedlevel/M420.cpp +++ b/Marlin/src/gcode/bedlevel/M420.cpp @@ -68,17 +68,17 @@ void GcodeSuite::M420() { y_min = probe.min_y(), y_max = probe.max_y(); #if ENABLED(AUTO_BED_LEVELING_BILINEAR) bilinear_start.set(x_min, y_min); - bilinear_grid_spacing.set((x_max - x_min) / (GRID_MAX_POINTS_X - 1), - (y_max - y_min) / (GRID_MAX_POINTS_Y - 1)); + bilinear_grid_spacing.set((x_max - x_min) / (GRID_MAX_CELLS_X), + (y_max - y_min) / (GRID_MAX_CELLS_Y)); #endif GRID_LOOP(x, y) { Z_VALUES(x, y) = 0.001 * random(-200, 200); TERN_(EXTENSIBLE_UI, ExtUI::onMeshUpdate(x, y, Z_VALUES(x, y))); } SERIAL_ECHOPGM("Simulated " STRINGIFY(GRID_MAX_POINTS_X) "x" STRINGIFY(GRID_MAX_POINTS_Y) " mesh "); - SERIAL_ECHOPAIR(" (", x_min); + SERIAL_ECHOPGM(" (", x_min); SERIAL_CHAR(','); SERIAL_ECHO(y_min); - SERIAL_ECHOPAIR(")-(", x_max); + SERIAL_ECHOPGM(")-(", x_max); SERIAL_CHAR(','); SERIAL_ECHO(y_max); SERIAL_ECHOLNPGM(")"); } @@ -108,7 +108,7 @@ void GcodeSuite::M420() { if (!WITHIN(storage_slot, 0, a - 1)) { SERIAL_ECHOLNPGM("?Invalid storage slot."); - SERIAL_ECHOLNPAIR("?Use 0 to ", a - 1); + SERIAL_ECHOLNPGM("?Use 0 to ", a - 1); return; } @@ -128,12 +128,12 @@ void GcodeSuite::M420() { ubl.display_map(parser.byteval('T')); SERIAL_ECHOPGM("Mesh is "); if (!ubl.mesh_is_valid()) SERIAL_ECHOPGM("in"); - SERIAL_ECHOLNPAIR("valid\nStorage slot: ", ubl.storage_slot); + SERIAL_ECHOLNPGM("valid\nStorage slot: ", ubl.storage_slot); } #endif // AUTO_BED_LEVELING_UBL - const bool seenV = parser.seen('V'); + const bool seenV = parser.seen_test('V'); #if HAS_MESH @@ -156,16 +156,16 @@ void GcodeSuite::M420() { GRID_LOOP(x, y) mesh_sum += Z_VALUES(x, y); const float zmean = mesh_sum / float(GRID_MAX_POINTS); - #else + #else // midrange - // Find the low and high mesh values + // Find the low and high mesh values. float lo_val = 100, hi_val = -100; GRID_LOOP(x, y) { const float z = Z_VALUES(x, y); NOMORE(lo_val, z); NOLESS(hi_val, z); } - // Take the mean of the lowest and highest + // Get the midrange plus C value. (The median may be better.) const float zmean = (lo_val + hi_val) / 2.0 + cval; #endif @@ -195,7 +195,7 @@ void GcodeSuite::M420() { // V to print the matrix or mesh if (seenV) { #if ABL_PLANAR - planner.bed_level_matrix.debug(PSTR("Bed Level Correction Matrix:")); + planner.bed_level_matrix.debug(F("Bed Level Correction Matrix:")); #else if (leveling_is_valid()) { #if ENABLED(AUTO_BED_LEVELING_BILINEAR) @@ -242,4 +242,18 @@ void GcodeSuite::M420() { report_current_position(); } +void GcodeSuite::M420_report(const bool forReplay/*=true*/) { + report_heading_etc(forReplay, F( + TERN(MESH_BED_LEVELING, "Mesh Bed Leveling", TERN(AUTO_BED_LEVELING_UBL, "Unified Bed Leveling", "Auto Bed Leveling")) + )); + SERIAL_ECHOF( + F(" M420 S"), planner.leveling_active + #if ENABLED(ENABLE_LEVELING_FADE_HEIGHT) + , FPSTR(SP_Z_STR), LINEAR_UNIT(planner.z_fade_height) + #endif + , F(" ; Leveling ") + ); + serialprintln_onoff(planner.leveling_active); +} + #endif // HAS_LEVELING diff --git a/Marlin/src/gcode/bedlevel/abl/G29.cpp b/Marlin/src/gcode/bedlevel/abl/G29.cpp index e642c1ccf256..9ec43fe49ddf 100644 --- a/Marlin/src/gcode/bedlevel/abl/G29.cpp +++ b/Marlin/src/gcode/bedlevel/abl/G29.cpp @@ -36,12 +36,12 @@ #include "../../../module/probe.h" #include "../../queue.h" -#if ENABLED(PROBE_TEMP_COMPENSATION) +#if HAS_PTC #include "../../../feature/probe_temp_comp.h" #include "../../../module/temperature.h" #endif -#if HAS_DISPLAY +#if HAS_STATUS_MESSAGE #include "../../../lcd/marlinui.h" #endif @@ -58,32 +58,91 @@ #if ENABLED(EXTENSIBLE_UI) #include "../../../lcd/extui/ui_api.h" -#endif - -#if ENABLED(DWIN_CREALITY_LCD) - #include "../../../lcd/dwin/e3v2/dwin.h" +#elif ENABLED(DWIN_CREALITY_LCD) + #include "../../../lcd/e3v2/creality/dwin.h" +#elif ENABLED(DWIN_CREALITY_LCD_ENHANCED) + #include "../../../lcd/e3v2/proui/dwin.h" #endif #if HAS_MULTI_HOTEND #include "../../../module/tool_change.h" #endif -#if ABL_GRID +#if ABL_USES_GRID #if ENABLED(PROBE_Y_FIRST) - #define PR_OUTER_VAR meshCount.x - #define PR_OUTER_END abl_grid_points.x - #define PR_INNER_VAR meshCount.y - #define PR_INNER_END abl_grid_points.y + #define PR_OUTER_VAR abl.meshCount.x + #define PR_OUTER_SIZE abl.grid_points.x + #define PR_INNER_VAR abl.meshCount.y + #define PR_INNER_SIZE abl.grid_points.y #else - #define PR_OUTER_VAR meshCount.y - #define PR_OUTER_END abl_grid_points.y - #define PR_INNER_VAR meshCount.x - #define PR_INNER_END abl_grid_points.x + #define PR_OUTER_VAR abl.meshCount.y + #define PR_OUTER_SIZE abl.grid_points.y + #define PR_INNER_VAR abl.meshCount.x + #define PR_INNER_SIZE abl.grid_points.x #endif #endif #define G29_RETURN(b) return TERN_(G29_RETRY_AND_RECOVER, b) +// For manual probing values persist over multiple G29 +class G29_State { +public: + int verbose_level; + xy_pos_t probePos; + float measured_z; + bool dryrun, + reenable; + + #if HAS_MULTI_HOTEND + uint8_t tool_index; + #endif + + #if EITHER(PROBE_MANUALLY, AUTO_BED_LEVELING_LINEAR) + int abl_probe_index; + #endif + + #if ENABLED(AUTO_BED_LEVELING_LINEAR) + int abl_points; + #elif ENABLED(AUTO_BED_LEVELING_3POINT) + static constexpr int abl_points = 3; + #elif ABL_USES_GRID + static constexpr int abl_points = GRID_MAX_POINTS; + #endif + + #if ABL_USES_GRID + + xy_int8_t meshCount; + + xy_pos_t probe_position_lf, + probe_position_rb; + + xy_float_t gridSpacing; // = { 0.0f, 0.0f } + + #if ENABLED(AUTO_BED_LEVELING_LINEAR) + bool topography_map; + xy_uint8_t grid_points; + #else // Bilinear + static constexpr xy_uint8_t grid_points = { GRID_MAX_POINTS_X, GRID_MAX_POINTS_Y }; + #endif + + #if ENABLED(AUTO_BED_LEVELING_BILINEAR) + float Z_offset; + #endif + + #if ENABLED(AUTO_BED_LEVELING_LINEAR) + int indexIntoAB[GRID_MAX_POINTS_X][GRID_MAX_POINTS_Y]; + float eqnAMatrix[(GRID_MAX_POINTS) * 3], // "A" matrix of the linear system of equations + eqnBVector[GRID_MAX_POINTS], // "B" vector of Z points + mean; + #endif + #endif +}; + +#if ABL_USES_GRID && EITHER(AUTO_BED_LEVELING_3POINT, AUTO_BED_LEVELING_BILINEAR) + constexpr xy_uint8_t G29_State::grid_points; + constexpr int G29_State::abl_points; +#endif + /** * G29: Detailed Z probe, probes the bed at 3 or more points. * Will fail if the printer has not been homed with G28. @@ -162,22 +221,23 @@ * There's no extra effect if you have a fixed Z probe. */ G29_TYPE GcodeSuite::G29() { + DEBUG_SECTION(log_G29, "G29", DEBUGGING(LEVELING)); + + TERN_(PROBE_MANUALLY, static) G29_State abl; + + TERN_(FULL_REPORT_TO_HOST_FEATURE, set_and_report_grblstate(M_PROBE)); reset_stepper_timeout(); - const bool seenQ = EITHER(DEBUG_LEVELING_FEATURE, PROBE_MANUALLY) && parser.seen('Q'); + const bool seenQ = EITHER(DEBUG_LEVELING_FEATURE, PROBE_MANUALLY) && parser.seen_test('Q'); // G29 Q is also available if debugging #if ENABLED(DEBUG_LEVELING_FEATURE) - const uint8_t old_debug_flags = marlin_debug_flags; - if (seenQ) marlin_debug_flags |= MARLIN_DEBUG_LEVELING; - DEBUG_SECTION(log_G29, "G29", DEBUGGING(LEVELING)); - if (DEBUGGING(LEVELING)) log_machine_info(); - marlin_debug_flags = old_debug_flags; + if (seenQ || DEBUGGING(LEVELING)) log_machine_info(); if (DISABLED(PROBE_MANUALLY) && seenQ) G29_RETURN(false); #endif - const bool seenA = TERN0(PROBE_MANUALLY, parser.seen('A')), + const bool seenA = TERN0(PROBE_MANUALLY, parser.seen_test('A')), no_action = seenA || seenQ, faux = ENABLED(DEBUG_LEVELING_FEATURE) && DISABLED(PROBE_MANUALLY) ? parser.boolval('C') : no_action; @@ -187,69 +247,16 @@ G29_TYPE GcodeSuite::G29() { } // Send 'N' to force homing before G29 (internal only) - if (parser.seen('N')) - process_subcommands_now_P(TERN(G28_L0_ENSURES_LEVELING_OFF, PSTR("G28L0"), G28_STR)); + if (parser.seen_test('N')) + process_subcommands_now(TERN(CAN_SET_LEVELING_AFTER_G28, F("G28L0"), FPSTR(G28_STR))); // Don't allow auto-leveling without homing first if (homing_needed_error()) G29_RETURN(false); - // Define local vars 'static' for manual probing, 'auto' otherwise - #define ABL_VAR TERN_(PROBE_MANUALLY, static) - - ABL_VAR int verbose_level; - ABL_VAR xy_pos_t probePos; - ABL_VAR float measured_z; - ABL_VAR bool dryrun, abl_should_enable; - - #if EITHER(PROBE_MANUALLY, AUTO_BED_LEVELING_LINEAR) - ABL_VAR int abl_probe_index; - #endif - - #if ABL_GRID - - #if ENABLED(PROBE_MANUALLY) - ABL_VAR xy_int8_t meshCount; - #endif - - ABL_VAR xy_pos_t probe_position_lf, probe_position_rb; - ABL_VAR xy_float_t gridSpacing = { 0, 0 }; - - #if ENABLED(AUTO_BED_LEVELING_LINEAR) - ABL_VAR bool do_topography_map; - ABL_VAR xy_uint8_t abl_grid_points; - #else // Bilinear - constexpr xy_uint8_t abl_grid_points = { GRID_MAX_POINTS_X, GRID_MAX_POINTS_Y }; - #endif - - #if ENABLED(AUTO_BED_LEVELING_LINEAR) - ABL_VAR int abl_points; - #else - int constexpr abl_points = GRID_MAX_POINTS; - #endif - - #if ENABLED(AUTO_BED_LEVELING_BILINEAR) - - ABL_VAR float zoffset; - - #elif ENABLED(AUTO_BED_LEVELING_LINEAR) - - ABL_VAR int indexIntoAB[GRID_MAX_POINTS_X][GRID_MAX_POINTS_Y]; - - ABL_VAR float eqnAMatrix[(GRID_MAX_POINTS) * 3], // "A" matrix of the linear system of equations - eqnBVector[GRID_MAX_POINTS], // "B" vector of Z points - mean; - #endif - - #elif ENABLED(AUTO_BED_LEVELING_3POINT) - - #if ENABLED(PROBE_MANUALLY) - int constexpr abl_points = 3; // used to show total points - #endif - + #if ENABLED(AUTO_BED_LEVELING_3POINT) vector_3 points[3]; probe.get_three_points(points); - - #endif // AUTO_BED_LEVELING_3POINT + #endif #if ENABLED(AUTO_BED_LEVELING_LINEAR) struct linear_fit_data lsf_results; @@ -260,17 +267,20 @@ G29_TYPE GcodeSuite::G29() { */ if (!g29_in_progress) { - TERN_(HAS_MULTI_HOTEND, if (active_extruder) tool_change(0)); + #if HAS_MULTI_HOTEND + abl.tool_index = active_extruder; + if (active_extruder != 0) tool_change(0, true); + #endif #if EITHER(PROBE_MANUALLY, AUTO_BED_LEVELING_LINEAR) - abl_probe_index = -1; + abl.abl_probe_index = -1; #endif - abl_should_enable = planner.leveling_active; + abl.reenable = planner.leveling_active; #if ENABLED(AUTO_BED_LEVELING_BILINEAR) - const bool seen_w = parser.seen('W'); + const bool seen_w = parser.seen_test('W'); if (seen_w) { if (!leveling_is_valid()) { SERIAL_ERROR_MSG("No bilinear grid"); @@ -287,23 +297,29 @@ G29_TYPE GcodeSuite::G29() { ry = RAW_Y_POSITION(parser.linearval('Y', NAN)); int8_t i = parser.byteval('I', -1), j = parser.byteval('J', -1); + #pragma GCC diagnostic push + #pragma GCC diagnostic ignored "-Wmaybe-uninitialized" + if (!isnan(rx) && !isnan(ry)) { // Get nearest i / j from rx / ry - i = (rx - bilinear_start.x + 0.5 * gridSpacing.x) / gridSpacing.x; - j = (ry - bilinear_start.y + 0.5 * gridSpacing.y) / gridSpacing.y; - LIMIT(i, 0, GRID_MAX_POINTS_X - 1); - LIMIT(j, 0, GRID_MAX_POINTS_Y - 1); + i = (rx - bilinear_start.x + 0.5 * abl.gridSpacing.x) / abl.gridSpacing.x; + j = (ry - bilinear_start.y + 0.5 * abl.gridSpacing.y) / abl.gridSpacing.y; + LIMIT(i, 0, (GRID_MAX_POINTS_X) - 1); + LIMIT(j, 0, (GRID_MAX_POINTS_Y) - 1); } - if (WITHIN(i, 0, GRID_MAX_POINTS_X - 1) && WITHIN(j, 0, GRID_MAX_POINTS_Y)) { + + #pragma GCC diagnostic pop + + if (WITHIN(i, 0, (GRID_MAX_POINTS_X) - 1) && WITHIN(j, 0, (GRID_MAX_POINTS_Y) - 1)) { set_bed_leveling_enabled(false); z_values[i][j] = rz; TERN_(ABL_BILINEAR_SUBDIVISION, bed_level_virt_interpolate()); TERN_(EXTENSIBLE_UI, ExtUI::onMeshUpdate(i, j, rz)); - set_bed_leveling_enabled(abl_should_enable); - if (abl_should_enable) report_current_position(); + set_bed_leveling_enabled(abl.reenable); + if (abl.reenable) report_current_position(); } G29_RETURN(false); - } // parser.seen('W') + } // parser.seen_test('W') #else @@ -312,52 +328,52 @@ G29_TYPE GcodeSuite::G29() { #endif // Jettison bed leveling data - if (!seen_w && parser.seen('J')) { + if (!seen_w && parser.seen_test('J')) { reset_bed_level(); G29_RETURN(false); } - verbose_level = parser.intval('V'); - if (!WITHIN(verbose_level, 0, 4)) { + abl.verbose_level = parser.intval('V'); + if (!WITHIN(abl.verbose_level, 0, 4)) { SERIAL_ECHOLNPGM("?(V)erbose level implausible (0-4)."); G29_RETURN(false); } - dryrun = parser.boolval('D') || TERN0(PROBE_MANUALLY, no_action); + abl.dryrun = parser.boolval('D') || TERN0(PROBE_MANUALLY, no_action); #if ENABLED(AUTO_BED_LEVELING_LINEAR) incremental_LSF_reset(&lsf_results); - do_topography_map = verbose_level > 2 || parser.boolval('T'); + abl.topography_map = abl.verbose_level > 2 || parser.boolval('T'); // X and Y specify points in each direction, overriding the default // These values may be saved with the completed mesh - abl_grid_points.set( + abl.grid_points.set( parser.byteval('X', GRID_MAX_POINTS_X), parser.byteval('Y', GRID_MAX_POINTS_Y) ); - if (parser.seenval('P')) abl_grid_points.x = abl_grid_points.y = parser.value_int(); + if (parser.seenval('P')) abl.grid_points.x = abl.grid_points.y = parser.value_int(); - if (!WITHIN(abl_grid_points.x, 2, GRID_MAX_POINTS_X)) { + if (!WITHIN(abl.grid_points.x, 2, GRID_MAX_POINTS_X)) { SERIAL_ECHOLNPGM("?Probe points (X) implausible (2-" STRINGIFY(GRID_MAX_POINTS_X) ")."); G29_RETURN(false); } - if (!WITHIN(abl_grid_points.y, 2, GRID_MAX_POINTS_Y)) { + if (!WITHIN(abl.grid_points.y, 2, GRID_MAX_POINTS_Y)) { SERIAL_ECHOLNPGM("?Probe points (Y) implausible (2-" STRINGIFY(GRID_MAX_POINTS_Y) ")."); G29_RETURN(false); } - abl_points = abl_grid_points.x * abl_grid_points.y; - mean = 0; + abl.abl_points = abl.grid_points.x * abl.grid_points.y; + abl.mean = 0; #elif ENABLED(AUTO_BED_LEVELING_BILINEAR) - zoffset = parser.linearval('Z'); + abl.Z_offset = parser.linearval('Z'); #endif - #if ABL_GRID + #if ABL_USES_GRID xy_probe_feedrate_mm_s = MMM_TO_MMS(parser.linearval('S', XY_PROBE_FEEDRATE)); @@ -366,32 +382,32 @@ G29_TYPE GcodeSuite::G29() { if (parser.seen('H')) { const int16_t size = (int16_t)parser.value_linear_units(); - probe_position_lf.set(_MAX((X_CENTER) - size / 2, x_min), _MAX((Y_CENTER) - size / 2, y_min)); - probe_position_rb.set(_MIN(probe_position_lf.x + size, x_max), _MIN(probe_position_lf.y + size, y_max)); + abl.probe_position_lf.set(_MAX((X_CENTER) - size / 2, x_min), _MAX((Y_CENTER) - size / 2, y_min)); + abl.probe_position_rb.set(_MIN(abl.probe_position_lf.x + size, x_max), _MIN(abl.probe_position_lf.y + size, y_max)); } else { - probe_position_lf.set(parser.linearval('L', x_min), parser.linearval('F', y_min)); - probe_position_rb.set(parser.linearval('R', x_max), parser.linearval('B', y_max)); + abl.probe_position_lf.set(parser.linearval('L', x_min), parser.linearval('F', y_min)); + abl.probe_position_rb.set(parser.linearval('R', x_max), parser.linearval('B', y_max)); } - if (!probe.good_bounds(probe_position_lf, probe_position_rb)) { + if (!probe.good_bounds(abl.probe_position_lf, abl.probe_position_rb)) { if (DEBUGGING(LEVELING)) { - DEBUG_ECHOLNPAIR("G29 L", probe_position_lf.x, " R", probe_position_rb.x, - " F", probe_position_lf.y, " B", probe_position_rb.y); + DEBUG_ECHOLNPGM("G29 L", abl.probe_position_lf.x, " R", abl.probe_position_rb.x, + " F", abl.probe_position_lf.y, " B", abl.probe_position_rb.y); } SERIAL_ECHOLNPGM("? (L,R,F,B) out of bounds."); G29_RETURN(false); } // Probe at the points of a lattice grid - gridSpacing.set((probe_position_rb.x - probe_position_lf.x) / (abl_grid_points.x - 1), - (probe_position_rb.y - probe_position_lf.y) / (abl_grid_points.y - 1)); + abl.gridSpacing.set((abl.probe_position_rb.x - abl.probe_position_lf.x) / (abl.grid_points.x - 1), + (abl.probe_position_rb.y - abl.probe_position_lf.y) / (abl.grid_points.y - 1)); - #endif // ABL_GRID + #endif // ABL_USES_GRID - if (verbose_level > 0) { + if (abl.verbose_level > 0) { SERIAL_ECHOPGM("G29 Auto Bed Leveling"); - if (dryrun) SERIAL_ECHOPGM(" (DRYRUN)"); + if (abl.dryrun) SERIAL_ECHOPGM(" (DRYRUN)"); SERIAL_EOL(); } @@ -400,17 +416,16 @@ G29_TYPE GcodeSuite::G29() { #if ENABLED(AUTO_BED_LEVELING_3POINT) if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPGM("> 3-point Leveling"); points[0].z = points[1].z = points[2].z = 0; // Probe at 3 arbitrary points - #endif - - #if BOTH(AUTO_BED_LEVELING_BILINEAR, EXTENSIBLE_UI) - ExtUI::onMeshLevelingStart(); + #elif ENABLED(AUTO_BED_LEVELING_BILINEAR) + TERN_(EXTENSIBLE_UI, ExtUI::onMeshLevelingStart()); + TERN_(DWIN_CREALITY_LCD_ENHANCED, DWIN_MeshLevelingStart()); #endif if (!faux) { remember_feedrate_scaling_off(); #if ENABLED(PREHEAT_BEFORE_LEVELING) - if (!dryrun) probe.preheat_for_probing(LEVELING_NOZZLE_TEMP, LEVELING_BED_TEMP); + if (!abl.dryrun) probe.preheat_for_probing(LEVELING_NOZZLE_TEMP, LEVELING_BED_TEMP); #endif } @@ -423,24 +438,24 @@ G29_TYPE GcodeSuite::G29() { if (ENABLED(BLTOUCH)) do_z_clearance(Z_CLEARANCE_DEPLOY_PROBE); else if (probe.deploy()) { - set_bed_leveling_enabled(abl_should_enable); + set_bed_leveling_enabled(abl.reenable); G29_RETURN(false); } #endif #if ENABLED(AUTO_BED_LEVELING_BILINEAR) if (TERN1(PROBE_MANUALLY, !no_action) - && (gridSpacing != bilinear_grid_spacing || probe_position_lf != bilinear_start) + && (abl.gridSpacing != bilinear_grid_spacing || abl.probe_position_lf != bilinear_start) ) { // Reset grid to 0.0 or "not probed". (Also disables ABL) reset_bed_level(); // Initialize a grid with the given dimensions - bilinear_grid_spacing = gridSpacing; - bilinear_start = probe_position_lf; + bilinear_grid_spacing = abl.gridSpacing; + bilinear_start = abl.probe_position_lf; // Can't re-enable (on error) until the new grid is written - abl_should_enable = false; + abl.reenable = false; } #endif // AUTO_BED_LEVELING_BILINEAR @@ -451,7 +466,7 @@ G29_TYPE GcodeSuite::G29() { // For manual probing, get the next index to probe now. // On the first probe this will be incremented to 0. if (!no_action) { - ++abl_probe_index; + ++abl.abl_probe_index; g29_in_progress = true; } @@ -459,25 +474,23 @@ G29_TYPE GcodeSuite::G29() { if (seenA && g29_in_progress) { SERIAL_ECHOLNPGM("Manual G29 aborted"); SET_SOFT_ENDSTOP_LOOSE(false); - set_bed_leveling_enabled(abl_should_enable); + set_bed_leveling_enabled(abl.reenable); g29_in_progress = false; TERN_(LCD_BED_LEVELING, ui.wait_for_move = false); } // Query G29 status - if (verbose_level || seenQ) { + if (abl.verbose_level || seenQ) { SERIAL_ECHOPGM("Manual G29 "); - if (g29_in_progress) { - SERIAL_ECHOPAIR("point ", _MIN(abl_probe_index + 1, abl_points)); - SERIAL_ECHOLNPAIR(" of ", abl_points); - } + if (g29_in_progress) + SERIAL_ECHOLNPGM("point ", _MIN(abl.abl_probe_index + 1, abl.abl_points), " of ", abl.abl_points); else SERIAL_ECHOLNPGM("idle"); } if (no_action) G29_RETURN(false); - if (abl_probe_index == 0) { + if (abl.abl_probe_index == 0) { // For the initial G29 S2 save software endstop state SET_SOFT_ENDSTOP_LOOSE(true); // Move close to the bed before the first point @@ -486,34 +499,34 @@ G29_TYPE GcodeSuite::G29() { else { #if EITHER(AUTO_BED_LEVELING_LINEAR, AUTO_BED_LEVELING_3POINT) - const uint16_t index = abl_probe_index - 1; + const uint16_t index = abl.abl_probe_index - 1; #endif // For G29 after adjusting Z. // Save the previous Z before going to the next point - measured_z = current_position.z; + abl.measured_z = current_position.z; #if ENABLED(AUTO_BED_LEVELING_LINEAR) - mean += measured_z; - eqnBVector[index] = measured_z; - eqnAMatrix[index + 0 * abl_points] = probePos.x; - eqnAMatrix[index + 1 * abl_points] = probePos.y; - eqnAMatrix[index + 2 * abl_points] = 1; + abl.mean += abl.measured_z; + abl.eqnBVector[index] = abl.measured_z; + abl.eqnAMatrix[index + 0 * abl.abl_points] = abl.probePos.x; + abl.eqnAMatrix[index + 1 * abl.abl_points] = abl.probePos.y; + abl.eqnAMatrix[index + 2 * abl.abl_points] = 1; - incremental_LSF(&lsf_results, probePos, measured_z); + incremental_LSF(&lsf_results, abl.probePos, abl.measured_z); #elif ENABLED(AUTO_BED_LEVELING_3POINT) - points[index].z = measured_z; + points[index].z = abl.measured_z; #elif ENABLED(AUTO_BED_LEVELING_BILINEAR) - const float newz = measured_z + zoffset; - z_values[meshCount.x][meshCount.y] = newz; - TERN_(EXTENSIBLE_UI, ExtUI::onMeshUpdate(meshCount, newz)); + const float newz = abl.measured_z + abl.Z_offset; + z_values[abl.meshCount.x][abl.meshCount.y] = newz; + TERN_(EXTENSIBLE_UI, ExtUI::onMeshUpdate(abl.meshCount, newz)); - if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPAIR_P(PSTR("Save X"), meshCount.x, SP_Y_STR, meshCount.y, SP_Z_STR, measured_z + zoffset); + if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPGM_P(PSTR("Save X"), abl.meshCount.x, SP_Y_STR, abl.meshCount.y, SP_Z_STR, abl.measured_z + abl.Z_offset); #endif } @@ -522,31 +535,31 @@ G29_TYPE GcodeSuite::G29() { // If there's another point to sample, move there with optional lift. // - #if ABL_GRID + #if ABL_USES_GRID // Skip any unreachable points - while (abl_probe_index < abl_points) { + while (abl.abl_probe_index < abl.abl_points) { - // Set meshCount.x, meshCount.y based on abl_probe_index, with zig-zag - PR_OUTER_VAR = abl_probe_index / PR_INNER_END; - PR_INNER_VAR = abl_probe_index - (PR_OUTER_VAR * PR_INNER_END); + // Set abl.meshCount.x, abl.meshCount.y based on abl.abl_probe_index, with zig-zag + PR_OUTER_VAR = abl.abl_probe_index / PR_INNER_SIZE; + PR_INNER_VAR = abl.abl_probe_index - (PR_OUTER_VAR * PR_INNER_SIZE); // Probe in reverse order for every other row/column - const bool zig = (PR_OUTER_VAR & 1); // != ((PR_OUTER_END) & 1); - if (zig) PR_INNER_VAR = (PR_INNER_END - 1) - PR_INNER_VAR; + const bool zig = (PR_OUTER_VAR & 1); // != ((PR_OUTER_SIZE) & 1); + if (zig) PR_INNER_VAR = (PR_INNER_SIZE - 1) - PR_INNER_VAR; - probePos = probe_position_lf + gridSpacing * meshCount.asFloat(); + abl.probePos = abl.probe_position_lf + abl.gridSpacing * abl.meshCount.asFloat(); - TERN_(AUTO_BED_LEVELING_LINEAR, indexIntoAB[meshCount.x][meshCount.y] = abl_probe_index); + TERN_(AUTO_BED_LEVELING_LINEAR, abl.indexIntoAB[abl.meshCount.x][abl.meshCount.y] = abl.abl_probe_index); // Keep looping till a reachable point is found - if (position_is_reachable(probePos)) break; - ++abl_probe_index; + if (position_is_reachable(abl.probePos)) break; + ++abl.abl_probe_index; } // Is there a next point to move to? - if (abl_probe_index < abl_points) { - _manual_goto_xy(probePos); // Can be used here too! + if (abl.abl_probe_index < abl.abl_points) { + _manual_goto_xy(abl.probePos); // Can be used here too! // Disable software endstops to allow manual adjustment // If G29 is not completed, they will not be re-enabled SET_SOFT_ENDSTOP_LOOSE(true); @@ -562,9 +575,9 @@ G29_TYPE GcodeSuite::G29() { #elif ENABLED(AUTO_BED_LEVELING_3POINT) // Probe at 3 arbitrary points - if (abl_probe_index < abl_points) { - probePos = points[abl_probe_index]; - _manual_goto_xy(probePos); + if (abl.abl_probe_index < abl.abl_points) { + abl.probePos = xy_pos_t(points[abl.abl_probe_index]); + _manual_goto_xy(abl.probePos); // Disable software endstops to allow manual adjustment // If G29 is not completed, they will not be re-enabled SET_SOFT_ENDSTOP_LOOSE(true); @@ -577,13 +590,13 @@ G29_TYPE GcodeSuite::G29() { // Re-enable software endstops, if needed SET_SOFT_ENDSTOP_LOOSE(false); - if (!dryrun) { + if (!abl.dryrun) { vector_3 planeNormal = vector_3::cross(points[0] - points[1], points[2] - points[1]).get_normal(); if (planeNormal.z < 0) planeNormal *= -1; planner.bed_level_matrix = matrix_3x3::create_look_at(planeNormal); // Can't re-enable (on error) until the new grid is written - abl_should_enable = false; + abl.reenable = false; } } @@ -594,84 +607,80 @@ G29_TYPE GcodeSuite::G29() { { const ProbePtRaise raise_after = parser.boolval('E') ? PROBE_PT_STOW : PROBE_PT_RAISE; - measured_z = 0; - - #if ABL_GRID + abl.measured_z = 0; - bool zig = PR_OUTER_END & 1; // Always end at RIGHT and BACK_PROBE_BED_POSITION + #if ABL_USES_GRID - measured_z = 0; + bool zig = PR_OUTER_SIZE & 1; // Always end at RIGHT and BACK_PROBE_BED_POSITION - xy_int8_t meshCount; + abl.measured_z = 0; // Outer loop is X with PROBE_Y_FIRST enabled // Outer loop is Y with PROBE_Y_FIRST disabled - for (PR_OUTER_VAR = 0; PR_OUTER_VAR < PR_OUTER_END && !isnan(measured_z); PR_OUTER_VAR++) { + for (PR_OUTER_VAR = 0; PR_OUTER_VAR < PR_OUTER_SIZE && !isnan(abl.measured_z); PR_OUTER_VAR++) { int8_t inStart, inStop, inInc; - if (zig) { // Zig away from origin - inStart = 0; // Left or front - inStop = PR_INNER_END; // Right or back - inInc = 1; // Zig right + if (zig) { // Zig away from origin + inStart = 0; // Left or front + inStop = PR_INNER_SIZE; // Right or back + inInc = 1; // Zig right } - else { // Zag towards origin - inStart = PR_INNER_END - 1; // Right or back - inStop = -1; // Left or front - inInc = -1; // Zag left + else { // Zag towards origin + inStart = PR_INNER_SIZE - 1; // Right or back + inStop = -1; // Left or front + inInc = -1; // Zag left } zig ^= true; // zag // An index to print current state - uint8_t pt_index = (PR_OUTER_VAR) * (PR_INNER_END) + 1; + uint8_t pt_index = (PR_OUTER_VAR) * (PR_INNER_SIZE) + 1; // Inner loop is Y with PROBE_Y_FIRST enabled // Inner loop is X with PROBE_Y_FIRST disabled for (PR_INNER_VAR = inStart; PR_INNER_VAR != inStop; pt_index++, PR_INNER_VAR += inInc) { - probePos = probe_position_lf + gridSpacing * meshCount.asFloat(); + abl.probePos = abl.probe_position_lf + abl.gridSpacing * abl.meshCount.asFloat(); - TERN_(AUTO_BED_LEVELING_LINEAR, indexIntoAB[meshCount.x][meshCount.y] = ++abl_probe_index); // 0... + TERN_(AUTO_BED_LEVELING_LINEAR, abl.indexIntoAB[abl.meshCount.x][abl.meshCount.y] = ++abl.abl_probe_index); // 0... // Avoid probing outside the round or hexagonal area - if (TERN0(IS_KINEMATIC, !probe.can_reach(probePos))) continue; + if (TERN0(IS_KINEMATIC, !probe.can_reach(abl.probePos))) continue; - if (verbose_level) SERIAL_ECHOLNPAIR("Probing mesh point ", pt_index, "/", abl_points, "."); - TERN_(HAS_DISPLAY, ui.status_printf_P(0, PSTR(S_FMT " %i/%i"), GET_TEXT(MSG_PROBING_MESH), int(pt_index), int(abl_points))); + if (abl.verbose_level) SERIAL_ECHOLNPGM("Probing mesh point ", pt_index, "/", abl.abl_points, "."); + TERN_(HAS_STATUS_MESSAGE, ui.status_printf(0, F(S_FMT " %i/%i"), GET_TEXT(MSG_PROBING_POINT), int(pt_index), int(abl.abl_points))); - measured_z = faux ? 0.001f * random(-100, 101) : probe.probe_at_point(probePos, raise_after, verbose_level); + abl.measured_z = faux ? 0.001f * random(-100, 101) : probe.probe_at_point(abl.probePos, raise_after, abl.verbose_level); - if (isnan(measured_z)) { - set_bed_leveling_enabled(abl_should_enable); + if (isnan(abl.measured_z)) { + set_bed_leveling_enabled(abl.reenable); break; // Breaks out of both loops } - #if ENABLED(PROBE_TEMP_COMPENSATION) - temp_comp.compensate_measurement(TSI_BED, thermalManager.degBed(), measured_z); - temp_comp.compensate_measurement(TSI_PROBE, thermalManager.degProbe(), measured_z); - TERN_(USE_TEMP_EXT_COMPENSATION, temp_comp.compensate_measurement(TSI_EXT, thermalManager.degHotend(), measured_z)); - #endif + TERN_(PTC_BED, ptc.compensate_measurement(TSI_BED, thermalManager.degBed(), abl.measured_z)); + TERN_(PTC_PROBE, ptc.compensate_measurement(TSI_PROBE, thermalManager.degProbe(), abl.measured_z)); + TERN_(PTC_HOTEND, ptc.compensate_measurement(TSI_EXT, thermalManager.degHotend(0), abl.measured_z)); #if ENABLED(AUTO_BED_LEVELING_LINEAR) - mean += measured_z; - eqnBVector[abl_probe_index] = measured_z; - eqnAMatrix[abl_probe_index + 0 * abl_points] = probePos.x; - eqnAMatrix[abl_probe_index + 1 * abl_points] = probePos.y; - eqnAMatrix[abl_probe_index + 2 * abl_points] = 1; + abl.mean += abl.measured_z; + abl.eqnBVector[abl.abl_probe_index] = abl.measured_z; + abl.eqnAMatrix[abl.abl_probe_index + 0 * abl.abl_points] = abl.probePos.x; + abl.eqnAMatrix[abl.abl_probe_index + 1 * abl.abl_points] = abl.probePos.y; + abl.eqnAMatrix[abl.abl_probe_index + 2 * abl.abl_points] = 1; - incremental_LSF(&lsf_results, probePos, measured_z); + incremental_LSF(&lsf_results, abl.probePos, abl.measured_z); #elif ENABLED(AUTO_BED_LEVELING_BILINEAR) - const float z = measured_z + zoffset; - z_values[meshCount.x][meshCount.y] = z; - TERN_(EXTENSIBLE_UI, ExtUI::onMeshUpdate(meshCount, z)); + const float z = abl.measured_z + abl.Z_offset; + z_values[abl.meshCount.x][abl.meshCount.y] = z PLUS_TERN0(X_AXIS_TWIST_COMPENSATION, xatc.compensation(abl.probePos)); + TERN_(EXTENSIBLE_UI, ExtUI::onMeshUpdate(abl.meshCount, z)); #endif - abl_should_enable = false; + abl.reenable = false; idle_no_sleep(); } // inner @@ -682,36 +691,36 @@ G29_TYPE GcodeSuite::G29() { // Probe at 3 arbitrary points LOOP_L_N(i, 3) { - if (verbose_level) SERIAL_ECHOLNPAIR("Probing point ", i + 1, "/3."); - TERN_(HAS_DISPLAY, ui.status_printf_P(0, PSTR(S_FMT " %i/3"), GET_TEXT(MSG_PROBING_MESH), int(i + 1))); + if (abl.verbose_level) SERIAL_ECHOLNPGM("Probing point ", i + 1, "/3."); + TERN_(HAS_STATUS_MESSAGE, ui.status_printf(0, F(S_FMT " %i/3"), GET_TEXT(MSG_PROBING_POINT), int(i + 1))); // Retain the last probe position - probePos = points[i]; - measured_z = faux ? 0.001 * random(-100, 101) : probe.probe_at_point(probePos, raise_after, verbose_level); - if (isnan(measured_z)) { - set_bed_leveling_enabled(abl_should_enable); + abl.probePos = xy_pos_t(points[i]); + abl.measured_z = faux ? 0.001 * random(-100, 101) : probe.probe_at_point(abl.probePos, raise_after, abl.verbose_level); + if (isnan(abl.measured_z)) { + set_bed_leveling_enabled(abl.reenable); break; } - points[i].z = measured_z; + points[i].z = abl.measured_z; } - if (!dryrun && !isnan(measured_z)) { + if (!abl.dryrun && !isnan(abl.measured_z)) { vector_3 planeNormal = vector_3::cross(points[0] - points[1], points[2] - points[1]).get_normal(); if (planeNormal.z < 0) planeNormal *= -1; planner.bed_level_matrix = matrix_3x3::create_look_at(planeNormal); // Can't re-enable (on error) until the new grid is written - abl_should_enable = false; + abl.reenable = false; } #endif // AUTO_BED_LEVELING_3POINT - TERN_(HAS_DISPLAY, ui.reset_status()); + TERN_(HAS_STATUS_MESSAGE, ui.reset_status()); // Stow the probe. No raise for FIX_MOUNTED_PROBE. if (probe.stow()) { - set_bed_leveling_enabled(abl_should_enable); - measured_z = NAN; + set_bed_leveling_enabled(abl.reenable); + abl.measured_z = NAN; } } #endif // !PROBE_MANUALLY @@ -734,10 +743,10 @@ G29_TYPE GcodeSuite::G29() { #endif // Calculate leveling, print reports, correct the position - if (!isnan(measured_z)) { + if (!isnan(abl.measured_z)) { #if ENABLED(AUTO_BED_LEVELING_BILINEAR) - if (!dryrun) extrapolate_unprobed_bed_level(); + if (!abl.dryrun) extrapolate_unprobed_bed_level(); print_bilinear_leveling_grid(); refresh_bed_level(); @@ -763,39 +772,39 @@ G29_TYPE GcodeSuite::G29() { plane_equation_coefficients.b = -lsf_results.B; // but that is not yet tested. plane_equation_coefficients.d = -lsf_results.D; - mean /= abl_points; + abl.mean /= abl.abl_points; - if (verbose_level) { + if (abl.verbose_level) { SERIAL_ECHOPAIR_F("Eqn coefficients: a: ", plane_equation_coefficients.a, 8); SERIAL_ECHOPAIR_F(" b: ", plane_equation_coefficients.b, 8); SERIAL_ECHOPAIR_F(" d: ", plane_equation_coefficients.d, 8); - if (verbose_level > 2) - SERIAL_ECHOPAIR_F("\nMean of sampled points: ", mean, 8); + if (abl.verbose_level > 2) + SERIAL_ECHOPAIR_F("\nMean of sampled points: ", abl.mean, 8); SERIAL_EOL(); } // Create the matrix but don't correct the position yet - if (!dryrun) + if (!abl.dryrun) planner.bed_level_matrix = matrix_3x3::create_look_at( vector_3(-plane_equation_coefficients.a, -plane_equation_coefficients.b, 1) // We can eliminate the '-' here and up above ); // Show the Topography map if enabled - if (do_topography_map) { + if (abl.topography_map) { float min_diff = 999; - auto print_topo_map = [&](PGM_P const title, const bool get_min) { - SERIAL_ECHOPGM_P(title); - for (int8_t yy = abl_grid_points.y - 1; yy >= 0; yy--) { - LOOP_L_N(xx, abl_grid_points.x) { - const int ind = indexIntoAB[xx][yy]; - xyz_float_t tmp = { eqnAMatrix[ind + 0 * abl_points], - eqnAMatrix[ind + 1 * abl_points], 0 }; - apply_rotation_xyz(planner.bed_level_matrix, tmp); - if (get_min) NOMORE(min_diff, eqnBVector[ind] - tmp.z); - const float subval = get_min ? mean : tmp.z + min_diff, - diff = eqnBVector[ind] - subval; + auto print_topo_map = [&](FSTR_P const title, const bool get_min) { + SERIAL_ECHOF(title); + for (int8_t yy = abl.grid_points.y - 1; yy >= 0; yy--) { + LOOP_L_N(xx, abl.grid_points.x) { + const int ind = abl.indexIntoAB[xx][yy]; + xyz_float_t tmp = { abl.eqnAMatrix[ind + 0 * abl.abl_points], + abl.eqnAMatrix[ind + 1 * abl.abl_points], 0 }; + planner.bed_level_matrix.apply_rotation_xyz(tmp.x, tmp.y, tmp.z); + if (get_min) NOMORE(min_diff, abl.eqnBVector[ind] - tmp.z); + const float subval = get_min ? abl.mean : tmp.z + min_diff, + diff = abl.eqnBVector[ind] - subval; SERIAL_CHAR(' '); if (diff >= 0.0) SERIAL_CHAR('+'); // Include + for column alignment SERIAL_ECHO_F(diff, 5); } // xx @@ -804,21 +813,21 @@ G29_TYPE GcodeSuite::G29() { SERIAL_EOL(); }; - print_topo_map(PSTR("\nBed Height Topography:\n" - " +--- BACK --+\n" - " | |\n" - " L | (+) | R\n" - " E | | I\n" - " F | (-) N (+) | G\n" - " T | | H\n" - " | (-) | T\n" - " | |\n" - " O-- FRONT --+\n" - " (0,0)\n"), true); - if (verbose_level > 3) - print_topo_map(PSTR("\nCorrected Bed Height vs. Bed Topology:\n"), false); - - } //do_topography_map + print_topo_map(F("\nBed Height Topography:\n" + " +--- BACK --+\n" + " | |\n" + " L | (+) | R\n" + " E | | I\n" + " F | (-) N (+) | G\n" + " T | | H\n" + " | (-) | T\n" + " | |\n" + " O-- FRONT --+\n" + " (0,0)\n"), true); + if (abl.verbose_level > 3) + print_topo_map(F("\nCorrected Bed Height vs. Bed Topology:\n"), false); + + } // abl.topography_map #endif // AUTO_BED_LEVELING_LINEAR @@ -826,10 +835,10 @@ G29_TYPE GcodeSuite::G29() { // For LINEAR and 3POINT leveling correct the current position - if (verbose_level > 0) - planner.bed_level_matrix.debug(PSTR("\n\nBed Level Correction Matrix:")); + if (abl.verbose_level > 0) + planner.bed_level_matrix.debug(F("\n\nBed Level Correction Matrix:")); - if (!dryrun) { + if (!abl.dryrun) { // // Correct the current XYZ position based on the tilted plane. // @@ -840,11 +849,11 @@ G29_TYPE GcodeSuite::G29() { planner.force_unapply_leveling(converted); // use conversion machinery // Use the last measured distance to the bed, if possible - if ( NEAR(current_position.x, probePos.x - probe.offset_xy.x) - && NEAR(current_position.y, probePos.y - probe.offset_xy.y) + if ( NEAR(current_position.x, abl.probePos.x - probe.offset_xy.x) + && NEAR(current_position.y, abl.probePos.y - probe.offset_xy.y) ) { - const float simple_z = current_position.z - measured_z; - if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPAIR("Probed Z", simple_z, " Matrix Z", converted.z, " Discrepancy ", simple_z - converted.z); + const float simple_z = current_position.z - abl.measured_z; + if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPGM("Probed Z", simple_z, " Matrix Z", converted.z, " Discrepancy ", simple_z - converted.z); converted.z = simple_z; } @@ -856,22 +865,22 @@ G29_TYPE GcodeSuite::G29() { #elif ENABLED(AUTO_BED_LEVELING_BILINEAR) - if (!dryrun) { - if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPAIR("G29 uncorrected Z:", current_position.z); + if (!abl.dryrun) { + if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPGM("G29 uncorrected Z:", current_position.z); // Unapply the offset because it is going to be immediately applied // and cause compensation movement in Z const float fade_scaling_factor = TERN(ENABLE_LEVELING_FADE_HEIGHT, planner.fade_scaling_factor_for_z(current_position.z), 1); current_position.z -= fade_scaling_factor * bilinear_z_offset(current_position); - if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPAIR(" corrected Z:", current_position.z); + if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPGM(" corrected Z:", current_position.z); } #endif // ABL_PLANAR // Auto Bed Leveling is complete! Enable if possible. - planner.leveling_active = dryrun ? abl_should_enable : true; - } // !isnan(measured_z) + planner.leveling_active = !abl.dryrun || abl.reenable; + } // !isnan(abl.measured_z) // Restore state after probing if (!faux) restore_feedrate_and_scaling(); @@ -879,23 +888,24 @@ G29_TYPE GcodeSuite::G29() { // Sync the planner from the current_position if (planner.leveling_active) sync_plan_position(); - #if HAS_BED_PROBE - probe.move_z_after_probing(); - #endif + TERN_(HAS_BED_PROBE, probe.move_z_after_probing()); #ifdef Z_PROBE_END_SCRIPT - if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPAIR("Z Probe End Script: ", Z_PROBE_END_SCRIPT); + if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPGM("Z Probe End Script: ", Z_PROBE_END_SCRIPT); planner.synchronize(); - process_subcommands_now_P(PSTR(Z_PROBE_END_SCRIPT)); + process_subcommands_now(F(Z_PROBE_END_SCRIPT)); #endif - #if ENABLED(DWIN_CREALITY_LCD) - DWIN_CompletedLeveling(); - #endif + TERN_(HAS_DWIN_E3V2_BASIC, DWIN_CompletedLeveling()); + + TERN_(HAS_MULTI_HOTEND, if (abl.tool_index != 0) tool_change(abl.tool_index)); report_current_position(); - G29_RETURN(isnan(measured_z)); + TERN_(FULL_REPORT_TO_HOST_FEATURE, set_and_report_grblstate(M_IDLE)); + + G29_RETURN(isnan(abl.measured_z)); + } #endif // HAS_ABL_NOT_UBL diff --git a/Marlin/src/gcode/bedlevel/mbl/G29.cpp b/Marlin/src/gcode/bedlevel/mbl/G29.cpp index c16338a6928f..090c15b0578c 100644 --- a/Marlin/src/gcode/bedlevel/mbl/G29.cpp +++ b/Marlin/src/gcode/bedlevel/mbl/G29.cpp @@ -40,8 +40,13 @@ #if ENABLED(EXTENSIBLE_UI) #include "../../../lcd/extui/ui_api.h" +#elif ENABLED(DWIN_CREALITY_LCD_ENHANCED) + #include "../../../lcd/e3v2/proui/dwin.h" #endif +#define DEBUG_OUT ENABLED(DEBUG_LEVELING_FEATURE) +#include "../../../core/debug_out.h" + // Save 130 bytes with non-duplication of PSTR inline void echo_not_entered(const char c) { SERIAL_CHAR(c); SERIAL_ECHOLNPGM(" not entered."); } @@ -59,6 +64,18 @@ inline void echo_not_entered(const char c) { SERIAL_CHAR(c); SERIAL_ECHOLNPGM(" * S5 Reset and disable mesh */ void GcodeSuite::G29() { + DEBUG_SECTION(log_G29, "G29", true); + + // G29 Q is also available if debugging + #if ENABLED(DEBUG_LEVELING_FEATURE) + const bool seenQ = parser.seen_test('Q'); + if (seenQ || DEBUGGING(LEVELING)) { + log_machine_info(); + if (seenQ) return; + } + #endif + + TERN_(FULL_REPORT_TO_HOST_FEATURE, set_and_report_grblstate(M_PROBE)); static int mbl_probe_index = -1; @@ -69,6 +86,7 @@ void GcodeSuite::G29() { } int8_t ix, iy; + ix = iy = 0; switch (state) { case MeshReport: @@ -85,7 +103,8 @@ void GcodeSuite::G29() { mbl.reset(); mbl_probe_index = 0; if (!ui.wait_for_move) { - queue.inject_P(parser.seen('N') ? PSTR("G28" TERN(G28_L0_ENSURES_LEVELING_OFF, "L0", "") "\nG29S2") : PSTR("G29S2")); + queue.inject(parser.seen_test('N') ? F("G28" TERN(CAN_SET_LEVELING_AFTER_G28, "L0", "") "\nG29S2") : F("G29S2")); + TERN_(EXTENSIBLE_UI, ExtUI::onMeshLevelingStart()); return; } state = MeshNext; @@ -98,15 +117,20 @@ void GcodeSuite::G29() { // For each G29 S2... if (mbl_probe_index == 0) { // Move close to the bed before the first point - do_blocking_move_to_z(0); + do_blocking_move_to_z(0.4f + #ifdef MANUAL_PROBE_START_Z + + (MANUAL_PROBE_START_Z) - 0.4f + #endif + ); } else { // Save Z for the previous mesh position mbl.set_zigzag_z(mbl_probe_index - 1, current_position.z); + TERN_(EXTENSIBLE_UI, ExtUI::onMeshUpdate(ix, iy, current_position.z)); SET_SOFT_ENDSTOP_LOOSE(false); } // If there's another point to sample, move there with optional lift. - if (mbl_probe_index < GRID_MAX_POINTS) { + if (mbl_probe_index < (GRID_MAX_POINTS)) { // Disable software endstops to allow manual adjustment // If G29 is left hanging without completion they won't be re-enabled! SET_SOFT_ENDSTOP_LOOSE(true); @@ -114,14 +138,21 @@ void GcodeSuite::G29() { _manual_goto_xy({ mbl.index_to_xpos[ix], mbl.index_to_ypos[iy] }); } else { - // One last "return to the bed" (as originally coded) at completion - current_position.z = MANUAL_PROBE_HEIGHT; + // Move to the after probing position + current_position.z = ( + #ifdef Z_AFTER_PROBING + Z_AFTER_PROBING + #else + Z_CLEARANCE_BETWEEN_MANUAL_PROBES + #endif + ); line_to_current_position(); planner.synchronize(); // After recording the last point, activate home and activate mbl_probe_index = -1; SERIAL_ECHOLNPGM("Mesh probing done."); + TERN_(HAS_STATUS_MESSAGE, LCD_MESSAGE(MSG_MESH_DONE)); BUZZ(100, 659); BUZZ(100, 698); @@ -141,8 +172,8 @@ void GcodeSuite::G29() { case MeshSet: if (parser.seenval('I')) { ix = parser.value_int(); - if (!WITHIN(ix, 0, GRID_MAX_POINTS_X - 1)) { - SERIAL_ECHOLNPAIR("I out of range (0-", GRID_MAX_POINTS_X - 1, ")"); + if (!WITHIN(ix, 0, (GRID_MAX_POINTS_X) - 1)) { + SERIAL_ECHOLNPGM("I out of range (0-", (GRID_MAX_POINTS_X) - 1, ")"); return; } } @@ -151,8 +182,8 @@ void GcodeSuite::G29() { if (parser.seenval('J')) { iy = parser.value_int(); - if (!WITHIN(iy, 0, GRID_MAX_POINTS_Y - 1)) { - SERIAL_ECHOLNPAIR("J out of range (0-", GRID_MAX_POINTS_Y - 1, ")"); + if (!WITHIN(iy, 0, (GRID_MAX_POINTS_Y) - 1)) { + SERIAL_ECHOLNPGM("J out of range (0-", (GRID_MAX_POINTS_Y) - 1, ")"); return; } } @@ -162,6 +193,7 @@ void GcodeSuite::G29() { if (parser.seenval('Z')) { mbl.z_values[ix][iy] = parser.value_linear_units(); TERN_(EXTENSIBLE_UI, ExtUI::onMeshUpdate(ix, iy, mbl.z_values[ix][iy])); + TERN_(DWIN_CREALITY_LCD_ENHANCED, DWIN_MeshUpdate(ix, iy, mbl.z_values[ix][iy])); } else return echo_not_entered('Z'); @@ -180,10 +212,14 @@ void GcodeSuite::G29() { } // switch(state) - if (state == MeshNext) - SERIAL_ECHOLNPAIR("MBL G29 point ", _MIN(mbl_probe_index, GRID_MAX_POINTS), " of ", GRID_MAX_POINTS); + if (state == MeshNext) { + SERIAL_ECHOLNPGM("MBL G29 point ", _MIN(mbl_probe_index, GRID_MAX_POINTS), " of ", GRID_MAX_POINTS); + if (mbl_probe_index > 0) TERN_(HAS_STATUS_MESSAGE, ui.status_printf(0, F(S_FMT " %i/%i"), GET_TEXT(MSG_PROBING_POINT), _MIN(mbl_probe_index, GRID_MAX_POINTS), int(GRID_MAX_POINTS))); + } report_current_position(); + + TERN_(FULL_REPORT_TO_HOST_FEATURE, set_and_report_grblstate(M_IDLE)); } #endif // MESH_BED_LEVELING diff --git a/Marlin/src/gcode/bedlevel/ubl/G29.cpp b/Marlin/src/gcode/bedlevel/ubl/G29.cpp index 2ef3ab4ceca9..932503d72b97 100644 --- a/Marlin/src/gcode/bedlevel/ubl/G29.cpp +++ b/Marlin/src/gcode/bedlevel/ubl/G29.cpp @@ -31,6 +31,17 @@ #include "../../gcode.h" #include "../../../feature/bedlevel/bedlevel.h" -void GcodeSuite::G29() { ubl.G29(); } +#if ENABLED(FULL_REPORT_TO_HOST_FEATURE) + #include "../../../module/motion.h" +#endif + +void GcodeSuite::G29() { + + TERN_(FULL_REPORT_TO_HOST_FEATURE, set_and_report_grblstate(M_PROBE)); + + ubl.G29(); + + TERN_(FULL_REPORT_TO_HOST_FEATURE, set_and_report_grblstate(M_IDLE)); +} #endif // AUTO_BED_LEVELING_UBL diff --git a/Marlin/src/gcode/bedlevel/ubl/M421.cpp b/Marlin/src/gcode/bedlevel/ubl/M421.cpp index 600c1fc8ba51..ac6f97b00acb 100644 --- a/Marlin/src/gcode/bedlevel/ubl/M421.cpp +++ b/Marlin/src/gcode/bedlevel/ubl/M421.cpp @@ -21,7 +21,7 @@ */ /** - * unified.cpp - Unified Bed Leveling + * M421.cpp - Unified Bed Leveling */ #include "../../../inc/MarlinConfig.h" @@ -33,37 +33,43 @@ #if ENABLED(EXTENSIBLE_UI) #include "../../../lcd/extui/ui_api.h" +#elif ENABLED(DWIN_CREALITY_LCD_ENHANCED) + #include "../../../lcd/e3v2/proui/dwin.h" #endif /** * M421: Set a single Mesh Bed Leveling Z coordinate * * Usage: - * M421 I J Z - * M421 I J Q - * M421 I J N - * M421 C Z - * M421 C Q + * M421 I J Z : Set the Mesh Point IJ to the Z value + * M421 I J Q : Add the Q value to the Mesh Point IJ + * M421 I J N : Set the Mesh Point IJ to NAN (not set) + * M421 C Z : Set the closest Mesh Point to the Z value + * M421 C Q : Add the Q value to the closest Mesh Point */ void GcodeSuite::M421() { xy_int8_t ij = { int8_t(parser.intval('I', -1)), int8_t(parser.intval('J', -1)) }; const bool hasI = ij.x >= 0, hasJ = ij.y >= 0, - hasC = parser.seen('C'), - hasN = parser.seen('N'), + hasC = parser.seen_test('C'), + hasN = parser.seen_test('N'), hasZ = parser.seen('Z'), hasQ = !hasZ && parser.seen('Q'); - if (hasC) ij = ubl.find_closest_mesh_point_of_type(REAL, current_position); + if (hasC) ij = ubl.find_closest_mesh_point_of_type(CLOSEST, current_position); + // Test for bad parameter combinations if (int(hasC) + int(hasI && hasJ) != 1 || !(hasZ || hasQ || hasN)) SERIAL_ERROR_MSG(STR_ERR_M421_PARAMETERS); + + // Test for I J out of range else if (!WITHIN(ij.x, 0, GRID_MAX_POINTS_X - 1) || !WITHIN(ij.y, 0, GRID_MAX_POINTS_Y - 1)) SERIAL_ERROR_MSG(STR_ERR_MESH_XY); else { - float &zval = ubl.z_values[ij.x][ij.y]; - zval = hasN ? NAN : parser.value_linear_units() + (hasQ ? zval : 0); - TERN_(EXTENSIBLE_UI, ExtUI::onMeshUpdate(ij.x, ij.y, zval)); + float &zval = ubl.z_values[ij.x][ij.y]; // Altering this Mesh Point + zval = hasN ? NAN : parser.value_linear_units() + (hasQ ? zval : 0); // N=NAN, Z=NEWVAL, or Q=ADDVAL + TERN_(EXTENSIBLE_UI, ExtUI::onMeshUpdate(ij.x, ij.y, zval)); // Ping ExtUI in case it's showing the mesh + TERN_(DWIN_CREALITY_LCD_ENHANCED, DWIN_MeshUpdate(ij.x, ij.y, zval)); } } diff --git a/Marlin/src/gcode/calibrate/G28.cpp b/Marlin/src/gcode/calibrate/G28.cpp index 947067887237..a38c5679395e 100644 --- a/Marlin/src/gcode/calibrate/G28.cpp +++ b/Marlin/src/gcode/calibrate/G28.cpp @@ -46,12 +46,13 @@ #endif #include "../../lcd/marlinui.h" -#if ENABLED(DWIN_CREALITY_LCD) - #include "../../lcd/dwin/e3v2/dwin.h" -#endif #if ENABLED(EXTENSIBLE_UI) #include "../../lcd/extui/ui_api.h" +#elif ENABLED(DWIN_CREALITY_LCD) + #include "../../lcd/e3v2/creality/dwin.h" +#elif ENABLED(DWIN_CREALITY_LCD_ENHANCED) + #include "../../lcd/e3v2/proui/dwin.h" #endif #if HAS_L64XX // set L6470 absolute position registers to counts @@ -73,18 +74,15 @@ current_position.set(0.0, 0.0); sync_plan_position(); - const int x_axis_home_dir = x_home_dir(active_extruder); + const int x_axis_home_dir = TOOL_X_HOME_DIR(active_extruder); - const float mlx = max_length(X_AXIS), - mly = max_length(Y_AXIS), - mlratio = mlx > mly ? mly / mlx : mlx / mly, - fr_mm_s = _MIN(homing_feedrate(X_AXIS), homing_feedrate(Y_AXIS)) * SQRT(sq(mlratio) + 1.0); + // Use a higher diagonal feedrate so axes move at homing speed + const float minfr = _MIN(homing_feedrate(X_AXIS), homing_feedrate(Y_AXIS)), + fr_mm_s = HYPOT(minfr, minfr); #if ENABLED(SENSORLESS_HOMING) sensorless_t stealth_states { - tmc_enable_stallguard(stepperX) - , tmc_enable_stallguard(stepperY) - , false + LINEAR_AXIS_LIST(tmc_enable_stallguard(stepperX), tmc_enable_stallguard(stepperY), false, false, false, false) , false #if AXIS_HAS_STALLGUARD(X2) || tmc_enable_stallguard(stepperX2) @@ -96,13 +94,13 @@ }; #endif - do_blocking_move_to_xy(1.5 * mlx * x_axis_home_dir, 1.5 * mly * Y_HOME_DIR, fr_mm_s); + do_blocking_move_to_xy(1.5 * max_length(X_AXIS) * x_axis_home_dir, 1.5 * max_length(Y_AXIS) * Y_HOME_DIR, fr_mm_s); endstops.validate_homing_move(); current_position.set(0.0, 0.0); - #if ENABLED(SENSORLESS_HOMING) + #if ENABLED(SENSORLESS_HOMING) && DISABLED(ENDSTOPS_ALWAYS_ON_DEFAULT) tmc_disable_stallguard(stepperX, stealth_states.x); tmc_disable_stallguard(stepperY, stealth_states.y); #if AXIS_HAS_STALLGUARD(X2) @@ -155,7 +153,7 @@ homeaxis(Z_AXIS); } else { - LCD_MESSAGEPGM(MSG_ZPROBE_OUT); + LCD_MESSAGE(MSG_ZPROBE_OUT); SERIAL_ECHO_MSG(STR_ZPROBE_OUT_SER); } } @@ -164,24 +162,28 @@ #if ENABLED(IMPROVE_HOMING_RELIABILITY) - slow_homing_t begin_slow_homing() { - slow_homing_t slow_homing{0}; - slow_homing.acceleration.set(planner.settings.max_acceleration_mm_per_s2[X_AXIS], - planner.settings.max_acceleration_mm_per_s2[Y_AXIS]); + motion_state_t begin_slow_homing() { + motion_state_t motion_state{0}; + motion_state.acceleration.set(planner.settings.max_acceleration_mm_per_s2[X_AXIS], + planner.settings.max_acceleration_mm_per_s2[Y_AXIS] + OPTARG(DELTA, planner.settings.max_acceleration_mm_per_s2[Z_AXIS]) + ); planner.settings.max_acceleration_mm_per_s2[X_AXIS] = 100; planner.settings.max_acceleration_mm_per_s2[Y_AXIS] = 100; + TERN_(DELTA, planner.settings.max_acceleration_mm_per_s2[Z_AXIS] = 100); #if HAS_CLASSIC_JERK - slow_homing.jerk_xy = planner.max_jerk; - planner.max_jerk.set(0, 0); + motion_state.jerk_state = planner.max_jerk; + planner.max_jerk.set(0, 0 OPTARG(DELTA, 0)); #endif planner.reset_acceleration_rates(); - return slow_homing; + return motion_state; } - void end_slow_homing(const slow_homing_t &slow_homing) { - planner.settings.max_acceleration_mm_per_s2[X_AXIS] = slow_homing.acceleration.x; - planner.settings.max_acceleration_mm_per_s2[Y_AXIS] = slow_homing.acceleration.y; - TERN_(HAS_CLASSIC_JERK, planner.max_jerk = slow_homing.jerk_xy); + void end_slow_homing(const motion_state_t &motion_state) { + planner.settings.max_acceleration_mm_per_s2[X_AXIS] = motion_state.acceleration.x; + planner.settings.max_acceleration_mm_per_s2[Y_AXIS] = motion_state.acceleration.y; + TERN_(DELTA, planner.settings.max_acceleration_mm_per_s2[Z_AXIS] = motion_state.acceleration.z); + TERN_(HAS_CLASSIC_JERK, planner.max_jerk = motion_state.jerk_state); planner.reset_acceleration_rates(); } @@ -195,9 +197,9 @@ * None Home to all axes with no parameters. * With QUICK_HOME enabled XY will home together, then Z. * - * O Home only if position is unknown - * - * Rn Raise by n mm/inches before homing + * L Force leveling state ON (if possible) or OFF after homing (Requires RESTORE_LEVELING_AFTER_G28 or ENABLE_LEVELING_AFTER_G28) + * O Home only if the position is not known and trusted + * R Raise by n mm/inches before homing * * Cartesian/SCARA parameters * @@ -211,14 +213,16 @@ void GcodeSuite::G28() { TERN_(LASER_MOVE_G28_OFF, cutter.set_inline_enabled(false)); // turn off laser + TERN_(FULL_REPORT_TO_HOST_FEATURE, set_and_report_grblstate(M_HOMING)); + #if ENABLED(DUAL_X_CARRIAGE) bool IDEX_saved_duplication_state = extruder_duplication_enabled; DualXMode IDEX_saved_mode = dual_x_carriage_mode; #endif #if ENABLED(MARLIN_DEV_MODE) - if (parser.seen('S')) { - LOOP_XYZ(a) set_axis_is_at_home((AxisEnum)a); + if (parser.seen_test('S')) { + LOOP_LINEAR_AXES(a) set_axis_is_at_home((AxisEnum)a); sync_plan_position(); SERIAL_ECHOLNPGM("Simulated Homing"); report_current_position(); @@ -227,12 +231,12 @@ void GcodeSuite::G28() { #endif // Home (O)nly if position is unknown - if (!axes_should_home() && parser.boolval('O')) { + if (!axes_should_home() && parser.seen_test('O')) { if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPGM("> homing not needed, skip"); return; } - TERN_(DWIN_CREALITY_LCD, DWIN_StartHoming()); + TERN_(HAS_DWIN_E3V2_BASIC, DWIN_StartHoming()); TERN_(EXTENSIBLE_UI, ExtUI::onHomingStart()); planner.synchronize(); // Wait for planner moves to finish! @@ -240,12 +244,16 @@ void GcodeSuite::G28() { SET_SOFT_ENDSTOP_LOOSE(false); // Reset a leftover 'loose' motion state // Disable the leveling matrix before homing - #if HAS_LEVELING - const bool leveling_restore_state = parser.boolval('L', TERN(RESTORE_LEVELING_AFTER_G28, planner.leveling_active, ENABLED(ENABLE_LEVELING_AFTER_G28))); - IF_ENABLED(PROBE_MANUALLY, g29_in_progress = false); // Cancel the active G29 session - set_bed_leveling_enabled(false); + #if CAN_SET_LEVELING_AFTER_G28 + const bool leveling_restore_state = parser.boolval('L', TERN1(RESTORE_LEVELING_AFTER_G28, planner.leveling_active)); #endif + // Cancel any prior G29 session + TERN_(PROBE_MANUALLY, g29_in_progress = false); + + // Disable leveling before homing + TERN_(HAS_LEVELING, set_bed_leveling_enabled(false)); + // Reset to the XY plane TERN_(CNC_WORKSPACE_PLANES, workspace_plane = PLANE_XY); @@ -253,37 +261,74 @@ void GcodeSuite::G28() { reset_stepper_timeout(); #define HAS_CURRENT_HOME(N) (defined(N##_CURRENT_HOME) && N##_CURRENT_HOME != N##_CURRENT) - #if HAS_CURRENT_HOME(X) || HAS_CURRENT_HOME(X2) || HAS_CURRENT_HOME(Y) || HAS_CURRENT_HOME(Y2) + #if HAS_CURRENT_HOME(X) || HAS_CURRENT_HOME(X2) || HAS_CURRENT_HOME(Y) || HAS_CURRENT_HOME(Y2) || (ENABLED(DELTA) && HAS_CURRENT_HOME(Z)) || HAS_CURRENT_HOME(I) || HAS_CURRENT_HOME(J) || HAS_CURRENT_HOME(K) #define HAS_HOMING_CURRENT 1 #endif #if HAS_HOMING_CURRENT - auto debug_current = [](PGM_P const s, const int16_t a, const int16_t b){ - DEBUG_ECHOPGM_P(s); DEBUG_ECHOLNPAIR(" current: ", a, " -> ", b); + auto debug_current = [](FSTR_P const s, const int16_t a, const int16_t b) { + DEBUG_ECHOF(s); DEBUG_ECHOLNPGM(" current: ", a, " -> ", b); }; #if HAS_CURRENT_HOME(X) const int16_t tmc_save_current_X = stepperX.getMilliamps(); stepperX.rms_current(X_CURRENT_HOME); - if (DEBUGGING(LEVELING)) debug_current(PSTR("X"), tmc_save_current_X, X_CURRENT_HOME); + if (DEBUGGING(LEVELING)) debug_current(F(STR_X), tmc_save_current_X, X_CURRENT_HOME); #endif #if HAS_CURRENT_HOME(X2) const int16_t tmc_save_current_X2 = stepperX2.getMilliamps(); stepperX2.rms_current(X2_CURRENT_HOME); - if (DEBUGGING(LEVELING)) debug_current(PSTR("X2"), tmc_save_current_X2, X2_CURRENT_HOME); + if (DEBUGGING(LEVELING)) debug_current(F(STR_X2), tmc_save_current_X2, X2_CURRENT_HOME); #endif #if HAS_CURRENT_HOME(Y) const int16_t tmc_save_current_Y = stepperY.getMilliamps(); stepperY.rms_current(Y_CURRENT_HOME); - if (DEBUGGING(LEVELING)) debug_current(PSTR("Y"), tmc_save_current_Y, Y_CURRENT_HOME); + if (DEBUGGING(LEVELING)) debug_current(F(STR_Y), tmc_save_current_Y, Y_CURRENT_HOME); #endif #if HAS_CURRENT_HOME(Y2) const int16_t tmc_save_current_Y2 = stepperY2.getMilliamps(); stepperY2.rms_current(Y2_CURRENT_HOME); - if (DEBUGGING(LEVELING)) debug_current(PSTR("Y2"), tmc_save_current_Y2, Y2_CURRENT_HOME); + if (DEBUGGING(LEVELING)) debug_current(F(STR_Y2), tmc_save_current_Y2, Y2_CURRENT_HOME); + #endif + #if HAS_CURRENT_HOME(I) + const int16_t tmc_save_current_I = stepperI.getMilliamps(); + stepperI.rms_current(I_CURRENT_HOME); + if (DEBUGGING(LEVELING)) debug_current(F(STR_I), tmc_save_current_I, I_CURRENT_HOME); + #endif + #if HAS_CURRENT_HOME(J) + const int16_t tmc_save_current_J = stepperJ.getMilliamps(); + stepperJ.rms_current(J_CURRENT_HOME); + if (DEBUGGING(LEVELING)) debug_current(F(STR_J), tmc_save_current_J, J_CURRENT_HOME); + #endif + #if HAS_CURRENT_HOME(K) + const int16_t tmc_save_current_K = stepperK.getMilliamps(); + stepperK.rms_current(K_CURRENT_HOME); + if (DEBUGGING(LEVELING)) debug_current(F(STR_K), tmc_save_current_K, K_CURRENT_HOME); + #endif + #if HAS_CURRENT_HOME(Z) && ENABLED(DELTA) + const int16_t tmc_save_current_Z = stepperZ.getMilliamps(); + stepperZ.rms_current(Z_CURRENT_HOME); + if (DEBUGGING(LEVELING)) debug_current(F(STR_Z), tmc_save_current_Z, Z_CURRENT_HOME); + #endif + #if HAS_CURRENT_HOME(I) + const int16_t tmc_save_current_I = stepperI.getMilliamps(); + stepperI.rms_current(I_CURRENT_HOME); + if (DEBUGGING(LEVELING)) debug_current(F(STR_I), tmc_save_current_I, I_CURRENT_HOME); + #endif + #if HAS_CURRENT_HOME(J) + const int16_t tmc_save_current_J = stepperJ.getMilliamps(); + stepperJ.rms_current(J_CURRENT_HOME); + if (DEBUGGING(LEVELING)) debug_current(F(STR_J), tmc_save_current_J, J_CURRENT_HOME); + #endif + #if HAS_CURRENT_HOME(K) + const int16_t tmc_save_current_K = stepperK.getMilliamps(); + stepperK.rms_current(K_CURRENT_HOME); + if (DEBUGGING(LEVELING)) debug_current(F(STR_K), tmc_save_current_K, K_CURRENT_HOME); #endif #endif - TERN_(IMPROVE_HOMING_RELIABILITY, slow_homing_t slow_homing = begin_slow_homing()); + #if ENABLED(IMPROVE_HOMING_RELIABILITY) + motion_state_t saved_motion_state = begin_slow_homing(); + #endif // Always home with tool 0 active #if HAS_MULTI_HOTEND @@ -309,7 +354,7 @@ void GcodeSuite::G28() { home_delta(); - TERN_(IMPROVE_HOMING_RELIABILITY, end_slow_homing(slow_homing)); + TERN_(IMPROVE_HOMING_RELIABILITY, end_slow_homing(saved_motion_state)); #elif ENABLED(AXEL_TPARA) @@ -319,33 +364,48 @@ void GcodeSuite::G28() { #else - const bool homeZ = parser.seen('Z'), - needX = homeZ && TERN0(Z_SAFE_HOMING, axes_should_home(_BV(X_AXIS))), - needY = homeZ && TERN0(Z_SAFE_HOMING, axes_should_home(_BV(Y_AXIS))), - homeX = needX || parser.seen('X'), homeY = needY || parser.seen('Y'), - home_all = homeX == homeY && homeX == homeZ, // All or None - doX = home_all || homeX, doY = home_all || homeY, doZ = home_all || homeZ; - - #if ENABLED(HOME_Z_FIRST) - - if (doZ) homeaxis(Z_AXIS); - + #define _UNSAFE(A) (homeZ && TERN0(Z_SAFE_HOMING, axes_should_home(_BV(A##_AXIS)))) + + const bool homeZ = TERN0(HAS_Z_AXIS, parser.seen_test('Z')), + LINEAR_AXIS_LIST( // Other axes should be homed before Z safe-homing + needX = _UNSAFE(X), needY = _UNSAFE(Y), needZ = false, // UNUSED + needI = _UNSAFE(I), needJ = _UNSAFE(J), needK = _UNSAFE(K) + ), + LINEAR_AXIS_LIST( // Home each axis if needed or flagged + homeX = needX || parser.seen_test('X'), + homeY = needY || parser.seen_test('Y'), + homeZZ = homeZ, + homeI = needI || parser.seen_test(AXIS4_NAME), homeJ = needJ || parser.seen_test(AXIS5_NAME), homeK = needK || parser.seen_test(AXIS6_NAME) + ), + home_all = LINEAR_AXIS_GANG( // Home-all if all or none are flagged + homeX == homeX, && homeY == homeX, && homeZ == homeX, + && homeI == homeX, && homeJ == homeX, && homeK == homeX + ), + LINEAR_AXIS_LIST( + doX = home_all || homeX, doY = home_all || homeY, doZ = home_all || homeZ, + doI = home_all || homeI, doJ = home_all || homeJ, doK = home_all || homeK + ); + + #if HAS_Z_AXIS + UNUSED(needZ); UNUSED(homeZZ); + #else + constexpr bool doZ = false; #endif - const float z_homing_height = parser.seenval('R') ? parser.value_linear_units() : Z_HOMING_HEIGHT; + TERN_(HOME_Z_FIRST, if (doZ) homeaxis(Z_AXIS)); + + const bool seenR = parser.seenval('R'); + const float z_homing_height = seenR ? parser.value_linear_units() : Z_HOMING_HEIGHT; - if (z_homing_height && (doX || doY || TERN0(Z_SAFE_HOMING, doZ))) { + if (z_homing_height && (seenR || LINEAR_AXIS_GANG(doX, || doY, || TERN0(Z_SAFE_HOMING, doZ), || doI, || doJ, || doK))) { // Raise Z before homing any other axes and z is not already high enough (never lower z) - if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPAIR("Raise Z (before homing) by ", z_homing_height); + if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPGM("Raise Z (before homing) by ", z_homing_height); do_z_clearance(z_homing_height); TERN_(BLTOUCH, bltouch.init()); } - #if ENABLED(QUICK_HOME) - - if (doX && doY) quick_home_xy(); - - #endif + // Diagonal move first if both are homing + TERN_(QUICK_HOME, if (doX && doY) quick_home_xy()); // Home Y (before X) if (ENABLED(HOME_Y_BEFORE_X) && (doY || TERN0(CODEPENDENT_XY_HOMING, doX))) @@ -381,21 +441,29 @@ void GcodeSuite::G28() { if (DISABLED(HOME_Y_BEFORE_X) && doY) homeaxis(Y_AXIS); - TERN_(IMPROVE_HOMING_RELIABILITY, end_slow_homing(slow_homing)); + TERN_(IMPROVE_HOMING_RELIABILITY, end_slow_homing(saved_motion_state)); // Home Z last if homing towards the bed - #if DISABLED(HOME_Z_FIRST) + #if HAS_Z_AXIS && DISABLED(HOME_Z_FIRST) if (doZ) { #if EITHER(Z_MULTI_ENDSTOPS, Z_STEPPER_AUTO_ALIGN) stepper.set_all_z_lock(false); stepper.set_separate_multi_axis(false); #endif - TERN(Z_SAFE_HOMING, home_z_safely(), homeaxis(Z_AXIS)); + #if ENABLED(Z_SAFE_HOMING) + if (TERN1(POWER_LOSS_RECOVERY, !parser.seen_test('H'))) home_z_safely(); else homeaxis(Z_AXIS); + #else + homeaxis(Z_AXIS); + #endif probe.move_z_after_homing(); } #endif + TERN_(HAS_I_AXIS, if (doI) homeaxis(I_AXIS)); + TERN_(HAS_J_AXIS, if (doJ) homeaxis(J_AXIS)); + TERN_(HAS_K_AXIS, if (doK) homeaxis(K_AXIS)); + sync_plan_position(); #endif @@ -410,7 +478,7 @@ void GcodeSuite::G28() { if (idex_is_duplicating()) { - TERN_(IMPROVE_HOMING_RELIABILITY, slow_homing = begin_slow_homing()); + TERN_(IMPROVE_HOMING_RELIABILITY, saved_motion_state = begin_slow_homing()); // Always home the 2nd (right) extruder first active_extruder = 1; @@ -429,7 +497,7 @@ void GcodeSuite::G28() { dual_x_carriage_mode = IDEX_saved_mode; set_duplication_enabled(IDEX_saved_duplication_state); - TERN_(IMPROVE_HOMING_RELIABILITY, end_slow_homing(slow_homing)); + TERN_(IMPROVE_HOMING_RELIABILITY, end_slow_homing(saved_motion_state)); } #endif // DUAL_X_CARRIAGE @@ -439,12 +507,10 @@ void GcodeSuite::G28() { // Clear endstop state for polled stallGuard endstops TERN_(SPI_ENDSTOPS, endstops.clear_endstop_state()); - #if BOTH(DELTA, DELTA_HOME_TO_SAFE_ZONE) - // move to a height where we can use the full xy-area - do_blocking_move_to_z(delta_clip_start_height); - #endif + // Move to a height where we can use the full xy-area + TERN_(DELTA_HOME_TO_SAFE_ZONE, do_blocking_move_to_z(delta_clip_start_height)); - TERN_(HAS_LEVELING, set_bed_leveling_enabled(leveling_restore_state)); + TERN_(CAN_SET_LEVELING_AFTER_G28, if (leveling_restore_state) set_bed_leveling_enabled()); restore_feedrate_and_scaling(); @@ -467,11 +533,23 @@ void GcodeSuite::G28() { #if HAS_CURRENT_HOME(Y2) stepperY2.rms_current(tmc_save_current_Y2); #endif - #endif + #if HAS_CURRENT_HOME(Z) && ENABLED(DELTA) + stepperZ.rms_current(tmc_save_current_Z); + #endif + #if HAS_CURRENT_HOME(I) + stepperI.rms_current(tmc_save_current_I); + #endif + #if HAS_CURRENT_HOME(J) + stepperJ.rms_current(tmc_save_current_J); + #endif + #if HAS_CURRENT_HOME(K) + stepperK.rms_current(tmc_save_current_K); + #endif + #endif // HAS_HOMING_CURRENT ui.refresh(); - TERN_(DWIN_CREALITY_LCD, DWIN_CompletedHoming()); + TERN_(HAS_DWIN_E3V2_BASIC, DWIN_CompletedHoming()); TERN_(EXTENSIBLE_UI, ExtUI::onHomingComplete()); report_current_position(); @@ -479,15 +557,19 @@ void GcodeSuite::G28() { if (ENABLED(NANODLP_Z_SYNC) && (doZ || ENABLED(NANODLP_ALL_AXIS))) SERIAL_ECHOLNPGM(STR_Z_MOVE_COMP); + TERN_(FULL_REPORT_TO_HOST_FEATURE, set_and_report_grblstate(M_IDLE)); + #if HAS_L64XX // Set L6470 absolute position registers to counts // constexpr *might* move this to PROGMEM. // If not, this will need a PROGMEM directive and an accessor. + #define _EN_ITEM(N) , E_AXIS static constexpr AxisEnum L64XX_axis_xref[MAX_L64XX] = { - X_AXIS, Y_AXIS, Z_AXIS, - X_AXIS, Y_AXIS, Z_AXIS, Z_AXIS, - E_AXIS, E_AXIS, E_AXIS, E_AXIS, E_AXIS, E_AXIS + LINEAR_AXIS_LIST(X_AXIS, Y_AXIS, Z_AXIS, I_AXIS, J_AXIS, K_AXIS), + X_AXIS, Y_AXIS, Z_AXIS, Z_AXIS, Z_AXIS + REPEAT(E_STEPPERS, _EN_ITEM) }; + #undef _EN_ITEM for (uint8_t j = 1; j <= L64XX::chain[0]; j++) { const uint8_t cv = L64XX::chain[j]; L64xxManager.set_param((L64XX_axis_t)cv, L6470_ABS_POS, stepper.position(L64XX_axis_xref[cv])); diff --git a/Marlin/src/gcode/calibrate/G33.cpp b/Marlin/src/gcode/calibrate/G33.cpp index 5530bc708971..a4b9aec01b67 100644 --- a/Marlin/src/gcode/calibrate/G33.cpp +++ b/Marlin/src/gcode/calibrate/G33.cpp @@ -63,13 +63,17 @@ enum CalEnum : char { // the 7 main calibration points - #define LOOP_CAL_RAD(VAR) LOOP_CAL_PT(VAR, __A, _7P_STEP) #define LOOP_CAL_ACT(VAR, _4P, _OP) LOOP_CAL_PT(VAR, _OP ? _AB : __A, _4P ? _4P_STEP : _7P_STEP) -TERN_(HAS_MULTI_HOTEND, const uint8_t old_tool_index = active_extruder); +#if HAS_MULTI_HOTEND + const uint8_t old_tool_index = active_extruder; +#endif float lcd_probe_pt(const xy_pos_t &xy); void ac_home() { endstops.enable(true); + TERN_(HAS_DELTA_SENSORLESS_PROBING, probe.set_homing_current(true)); home_delta(); + TERN_(HAS_DELTA_SENSORLESS_PROBING, probe.set_homing_current(false)); endstops.not_homing(); } @@ -91,10 +95,9 @@ void ac_cleanup(TERN_(HAS_MULTI_HOTEND, const uint8_t old_tool_index)) { TERN_(HAS_MULTI_HOTEND, tool_change(old_tool_index, true)); } -void print_signed_float(PGM_P const prefix, const float &f) { +void print_signed_float(FSTR_P const prefix, const_float_t f) { SERIAL_ECHOPGM(" "); - SERIAL_ECHOPGM_P(prefix); - SERIAL_CHAR(':'); + SERIAL_ECHOF(prefix, AS_CHAR(':')); if (f >= 0) SERIAL_CHAR('+'); SERIAL_ECHO_F(f, 2); } @@ -103,26 +106,25 @@ void print_signed_float(PGM_P const prefix, const float &f) { * - Print the delta settings */ static void print_calibration_settings(const bool end_stops, const bool tower_angles) { - SERIAL_ECHOPAIR(".Height:", delta_height); + SERIAL_ECHOPGM(".Height:", delta_height); if (end_stops) { - print_signed_float(PSTR("Ex"), delta_endstop_adj.a); - print_signed_float(PSTR("Ey"), delta_endstop_adj.b); - print_signed_float(PSTR("Ez"), delta_endstop_adj.c); + print_signed_float(F("Ex"), delta_endstop_adj.a); + print_signed_float(F("Ey"), delta_endstop_adj.b); + print_signed_float(F("Ez"), delta_endstop_adj.c); } if (end_stops && tower_angles) { - SERIAL_ECHOPAIR(" Radius:", delta_radius); - SERIAL_EOL(); + SERIAL_ECHOLNPGM(" Radius:", delta_radius); SERIAL_CHAR('.'); SERIAL_ECHO_SP(13); } if (tower_angles) { - print_signed_float(PSTR("Tx"), delta_tower_angle_trim.a); - print_signed_float(PSTR("Ty"), delta_tower_angle_trim.b); - print_signed_float(PSTR("Tz"), delta_tower_angle_trim.c); - } - if ((!end_stops && tower_angles) || (end_stops && !tower_angles)) { // XOR - SERIAL_ECHOPAIR(" Radius:", delta_radius); + print_signed_float(F("Tx"), delta_tower_angle_trim.a); + print_signed_float(F("Ty"), delta_tower_angle_trim.b); + print_signed_float(F("Tz"), delta_tower_angle_trim.c); } + if (end_stops != tower_angles) + SERIAL_ECHOPGM(" Radius:", delta_radius); + SERIAL_EOL(); } @@ -131,11 +133,11 @@ static void print_calibration_settings(const bool end_stops, const bool tower_an */ static void print_calibration_results(const float z_pt[NPP + 1], const bool tower_points, const bool opposite_points) { SERIAL_ECHOPGM(". "); - print_signed_float(PSTR("c"), z_pt[CEN]); + print_signed_float(F("c"), z_pt[CEN]); if (tower_points) { - print_signed_float(PSTR(" x"), z_pt[__A]); - print_signed_float(PSTR(" y"), z_pt[__B]); - print_signed_float(PSTR(" z"), z_pt[__C]); + print_signed_float(F(" x"), z_pt[__A]); + print_signed_float(F(" y"), z_pt[__B]); + print_signed_float(F(" z"), z_pt[__C]); } if (tower_points && opposite_points) { SERIAL_EOL(); @@ -143,9 +145,9 @@ static void print_calibration_results(const float z_pt[NPP + 1], const bool towe SERIAL_ECHO_SP(13); } if (opposite_points) { - print_signed_float(PSTR("yz"), z_pt[_BC]); - print_signed_float(PSTR("zx"), z_pt[_CA]); - print_signed_float(PSTR("xy"), z_pt[_AB]); + print_signed_float(F("yz"), z_pt[_BC]); + print_signed_float(F("zx"), z_pt[_CA]); + print_signed_float(F("xy"), z_pt[_AB]); } SERIAL_EOL(); } @@ -171,9 +173,9 @@ static float std_dev_points(float z_pt[NPP + 1], const bool _0p_cal, const bool /** * - Probe a point */ -static float calibration_probe(const xy_pos_t &xy, const bool stow) { +static float calibration_probe(const xy_pos_t &xy, const bool stow, const bool probe_at_offset) { #if HAS_BED_PROBE - return probe.probe_at_point(xy, stow ? PROBE_PT_STOW : PROBE_PT_RAISE, 0, true, false); + return probe.probe_at_point(xy, stow ? PROBE_PT_STOW : PROBE_PT_RAISE, 0, probe_at_offset, false); #else UNUSED(stow); return lcd_probe_pt(xy); @@ -183,7 +185,7 @@ static float calibration_probe(const xy_pos_t &xy, const bool stow) { /** * - Probe a grid */ -static bool probe_calibration_points(float z_pt[NPP + 1], const int8_t probe_points, const bool towers_set, const bool stow_after_each) { +static bool probe_calibration_points(float z_pt[NPP + 1], const int8_t probe_points, const float dcr, const bool towers_set, const bool stow_after_each, const bool probe_at_offset) { const bool _0p_calibration = probe_points == 0, _1p_calibration = probe_points == 1 || probe_points == -1, _4p_calibration = probe_points == 2, @@ -205,11 +207,9 @@ static bool probe_calibration_points(float z_pt[NPP + 1], const int8_t probe_poi if (!_0p_calibration) { - const float dcr = delta_calibration_radius(); - if (!_7p_no_intermediates && !_7p_4_intermediates && !_7p_11_intermediates) { // probe the center const xy_pos_t center{0}; - z_pt[CEN] += calibration_probe(center, stow_after_each); + z_pt[CEN] += calibration_probe(center, stow_after_each, probe_at_offset); if (isnan(z_pt[CEN])) return false; } @@ -220,7 +220,7 @@ static bool probe_calibration_points(float z_pt[NPP + 1], const int8_t probe_poi const float a = RADIANS(210 + (360 / NPP) * (rad - 1)), r = dcr * 0.1; const xy_pos_t vec = { cos(a), sin(a) }; - z_pt[CEN] += calibration_probe(vec * r, stow_after_each); + z_pt[CEN] += calibration_probe(vec * r, stow_after_each, probe_at_offset); if (isnan(z_pt[CEN])) return false; } z_pt[CEN] /= float(_7p_2_intermediates ? 7 : probe_points); @@ -245,7 +245,7 @@ static bool probe_calibration_points(float z_pt[NPP + 1], const int8_t probe_poi r = dcr * (1 - 0.1 * (zig_zag ? offset - circle : circle)), interpol = FMOD(rad, 1); const xy_pos_t vec = { cos(a), sin(a) }; - const float z_temp = calibration_probe(vec * r, stow_after_each); + const float z_temp = calibration_probe(vec * r, stow_after_each, probe_at_offset); if (isnan(z_temp)) return false; // split probe point to neighbouring calibration points z_pt[uint8_t(LROUND(rad - interpol + NPP - 1)) % NPP + 1] += z_temp * sq(cos(RADIANS(interpol * 90))); @@ -269,10 +269,9 @@ static bool probe_calibration_points(float z_pt[NPP + 1], const int8_t probe_poi * - formulae for approximative forward kinematics in the end-stop displacement matrix * - definition of the matrix scaling parameters */ -static void reverse_kinematics_probe_points(float z_pt[NPP + 1], abc_float_t mm_at_pt_axis[NPP + 1]) { +static void reverse_kinematics_probe_points(float z_pt[NPP + 1], abc_float_t mm_at_pt_axis[NPP + 1], const float dcr) { xyz_pos_t pos{0}; - const float dcr = delta_calibration_radius(); LOOP_CAL_ALL(rad) { const float a = RADIANS(210 + (360 / NPP) * (rad - 1)), r = (rad == CEN ? 0.0f : dcr); @@ -282,8 +281,8 @@ static void reverse_kinematics_probe_points(float z_pt[NPP + 1], abc_float_t mm_ } } -static void forward_kinematics_probe_points(abc_float_t mm_at_pt_axis[NPP + 1], float z_pt[NPP + 1]) { - const float r_quot = delta_calibration_radius() / delta_radius; +static void forward_kinematics_probe_points(abc_float_t mm_at_pt_axis[NPP + 1], float z_pt[NPP + 1], const float dcr) { + const float r_quot = dcr / delta_radius; #define ZPP(N,I,A) (((1.0f + r_quot * (N)) / 3.0f) * mm_at_pt_axis[I].A) #define Z00(I, A) ZPP( 0, I, A) @@ -301,19 +300,19 @@ static void forward_kinematics_probe_points(abc_float_t mm_at_pt_axis[NPP + 1], z_pt[_AB] = Zp1(_AB, a) + Zp1(_AB, b) + Zm2(_AB, c); } -static void calc_kinematics_diff_probe_points(float z_pt[NPP + 1], abc_float_t delta_e, const float delta_r, abc_float_t delta_t) { +static void calc_kinematics_diff_probe_points(float z_pt[NPP + 1], const float dcr, abc_float_t delta_e, const float delta_r, abc_float_t delta_t) { const float z_center = z_pt[CEN]; abc_float_t diff_mm_at_pt_axis[NPP + 1], new_mm_at_pt_axis[NPP + 1]; - reverse_kinematics_probe_points(z_pt, diff_mm_at_pt_axis); + reverse_kinematics_probe_points(z_pt, diff_mm_at_pt_axis, dcr); delta_radius += delta_r; delta_tower_angle_trim += delta_t; recalc_delta_settings(); - reverse_kinematics_probe_points(z_pt, new_mm_at_pt_axis); + reverse_kinematics_probe_points(z_pt, new_mm_at_pt_axis, dcr); LOOP_CAL_ALL(rad) diff_mm_at_pt_axis[rad] -= new_mm_at_pt_axis[rad] + delta_e; - forward_kinematics_probe_points(diff_mm_at_pt_axis, z_pt); + forward_kinematics_probe_points(diff_mm_at_pt_axis, z_pt, dcr); LOOP_CAL_RAD(rad) z_pt[rad] -= z_pt[CEN] - z_center; z_pt[CEN] = z_center; @@ -323,31 +322,31 @@ static void calc_kinematics_diff_probe_points(float z_pt[NPP + 1], abc_float_t d recalc_delta_settings(); } -static float auto_tune_h() { - const float r_quot = delta_calibration_radius() / delta_radius; +static float auto_tune_h(const float dcr) { + const float r_quot = dcr / delta_radius; return RECIPROCAL(r_quot / (2.0f / 3.0f)); // (2/3)/CR } -static float auto_tune_r() { +static float auto_tune_r(const float dcr) { constexpr float diff = 0.01f, delta_r = diff; float r_fac = 0.0f, z_pt[NPP + 1] = { 0.0f }; abc_float_t delta_e = { 0.0f }, delta_t = { 0.0f }; - calc_kinematics_diff_probe_points(z_pt, delta_e, delta_r, delta_t); + calc_kinematics_diff_probe_points(z_pt, dcr, delta_e, delta_r, delta_t); r_fac = -(z_pt[__A] + z_pt[__B] + z_pt[__C] + z_pt[_BC] + z_pt[_CA] + z_pt[_AB]) / 6.0f; r_fac = diff / r_fac / 3.0f; // 1/(3*delta_Z) return r_fac; } -static float auto_tune_a() { +static float auto_tune_a(const float dcr) { constexpr float diff = 0.01f, delta_r = 0.0f; float a_fac = 0.0f, z_pt[NPP + 1] = { 0.0f }; abc_float_t delta_e = { 0.0f }, delta_t = { 0.0f }; delta_t.reset(); - LOOP_XYZ(axis) { + LOOP_LINEAR_AXES(axis) { delta_t[axis] = diff; - calc_kinematics_diff_probe_points(z_pt, delta_e, delta_r, delta_t); + calc_kinematics_diff_probe_points(z_pt, dcr, delta_e, delta_r, delta_t); delta_t[axis] = 0; a_fac += z_pt[uint8_t((axis * _4P_STEP) - _7P_STEP + NPP) % NPP + 1] / 6.0f; a_fac -= z_pt[uint8_t((axis * _4P_STEP) + 1 + _7P_STEP)] / 6.0f; @@ -369,6 +368,8 @@ static float auto_tune_a() { * P3 Probe all positions: center, towers and opposite towers. Calibrate all. * P4-P10 Probe all positions at different intermediate locations and average them. * + * Rn.nn Temporary reduce the probe grid by the specified amount (mm) + * * T Don't calibrate tower angle corrections * * Cn.nn Calibration precision; when omitted calibrates to maximum precision @@ -382,16 +383,36 @@ static float auto_tune_a() { * V3 Report settings and probe results * * E Engage the probe for each point + * + * O Probe at offsetted probe positions (this is wrong but it seems to work) + * + * With SENSORLESS_PROBING: + * Use these flags to calibrate stall sensitivity: (e.g., `G33 P1 Y Z` to calibrate X only.) + * X Don't activate stallguard on X. + * Y Don't activate stallguard on Y. + * Z Don't activate stallguard on Z. */ void GcodeSuite::G33() { + TERN_(FULL_REPORT_TO_HOST_FEATURE, set_and_report_grblstate(M_PROBE)); + const int8_t probe_points = parser.intval('P', DELTA_CALIBRATION_DEFAULT_POINTS); if (!WITHIN(probe_points, 0, 10)) { SERIAL_ECHOLNPGM("?(P)oints implausible (0-10)."); return; } - const bool towers_set = !parser.seen('T'); + const bool probe_at_offset = TERN0(HAS_PROBE_XY_OFFSET, parser.seen_test('O')), + towers_set = !parser.seen_test('T'); + + // The calibration radius is set to a calculated value + float dcr = probe_at_offset ? DELTA_PRINTABLE_RADIUS : DELTA_PRINTABLE_RADIUS - PROBING_MARGIN; + #if HAS_PROBE_XY_OFFSET + const float total_offset = HYPOT(probe.offset_xy.x, probe.offset_xy.y); + dcr -= probe_at_offset ? _MAX(total_offset, PROBING_MARGIN) : total_offset; + #endif + NOMORE(dcr, DELTA_PRINTABLE_RADIUS); + if (parser.seenval('R')) dcr -= _MAX(parser.value_float(),0); const float calibration_precision = parser.floatval('C', 0.0f); if (calibration_precision < 0) { @@ -411,7 +432,13 @@ void GcodeSuite::G33() { return; } - const bool stow_after_each = parser.seen('E'); + const bool stow_after_each = parser.seen_test('E'); + + #if HAS_DELTA_SENSORLESS_PROBING + probe.test_sensitivity.x = !parser.seen_test('X'); + TERN_(HAS_Y_AXIS, probe.test_sensitivity.y = !parser.seen_test('Y')); + TERN_(HAS_Z_AXIS, probe.test_sensitivity.z = !parser.seen_test('Z')); + #endif const bool _0p_calibration = probe_points == 0, _1p_calibration = probe_points == 1 || probe_points == -1, @@ -435,24 +462,13 @@ void GcodeSuite::G33() { SERIAL_ECHOLNPGM("G33 Auto Calibrate"); - const float dcr = delta_calibration_radius(); - - if (!_1p_calibration && !_0p_calibration) { // test if the outer radius is reachable - LOOP_CAL_RAD(axis) { - const float a = RADIANS(210 + (360 / NPP) * (axis - 1)); - if (!position_is_reachable(cos(a) * dcr, sin(a) * dcr)) { - SERIAL_ECHOLNPGM("?Bed calibration radius implausible."); - return; - } - } - } - // Report settings PGM_P const checkingac = PSTR("Checking... AC"); SERIAL_ECHOPGM_P(checkingac); + SERIAL_ECHOPGM(" at radius:", dcr); if (verbose_level == 0) SERIAL_ECHOPGM(" (DRY-RUN)"); SERIAL_EOL(); - ui.set_status_P(checkingac); + ui.set_status(checkingac); print_calibration_settings(_endstop_results, _angle_results); @@ -469,7 +485,7 @@ void GcodeSuite::G33() { // Probe the points zero_std_dev_old = zero_std_dev; - if (!probe_calibration_points(z_at_pt, probe_points, towers_set, stow_after_each)) { + if (!probe_calibration_points(z_at_pt, probe_points, dcr, towers_set, stow_after_each, probe_at_offset)) { SERIAL_ECHOLNPGM("Correct delta settings with M665 and M666"); return ac_cleanup(TERN_(HAS_MULTI_HOTEND, old_tool_index)); } @@ -508,11 +524,11 @@ void GcodeSuite::G33() { #define Z0(I) ZP(0, I) // calculate factors - if (_7p_9_center) calibration_radius_factor = 0.9f; - h_factor = auto_tune_h(); - r_factor = auto_tune_r(); - a_factor = auto_tune_a(); - calibration_radius_factor = 1.0f; + if (_7p_9_center) dcr *= 0.9f; + h_factor = auto_tune_h(dcr); + r_factor = auto_tune_r(dcr); + a_factor = auto_tune_a(dcr); + if (_7p_9_center) dcr /= 0.9f; switch (probe_points) { case 0: @@ -521,7 +537,7 @@ void GcodeSuite::G33() { case 1: test_precision = 0.0f; // forced end - LOOP_XYZ(axis) e_delta[axis] = +Z4(CEN); + LOOP_LINEAR_AXES(axis) e_delta[axis] = +Z4(CEN); break; case 2: @@ -569,21 +585,21 @@ void GcodeSuite::G33() { // Normalize angles to least-squares if (_angle_results) { float a_sum = 0.0f; - LOOP_XYZ(axis) a_sum += delta_tower_angle_trim[axis]; - LOOP_XYZ(axis) delta_tower_angle_trim[axis] -= a_sum / 3.0f; + LOOP_LINEAR_AXES(axis) a_sum += delta_tower_angle_trim[axis]; + LOOP_LINEAR_AXES(axis) delta_tower_angle_trim[axis] -= a_sum / 3.0f; } // adjust delta_height and endstops by the max amount const float z_temp = _MAX(delta_endstop_adj.a, delta_endstop_adj.b, delta_endstop_adj.c); delta_height -= z_temp; - LOOP_XYZ(axis) delta_endstop_adj[axis] -= z_temp; + LOOP_LINEAR_AXES(axis) delta_endstop_adj[axis] -= z_temp; } recalc_delta_settings(); NOMORE(zero_std_dev_min, zero_std_dev); // print report - if (verbose_level == 3) + if (verbose_level == 3 || verbose_level == 0) print_calibration_results(z_at_pt, _tower_results, _opposite_results); if (verbose_level != 0) { // !dry run @@ -624,13 +640,13 @@ void GcodeSuite::G33() { } } else { // dry run - PGM_P const enddryrun = PSTR("End DRY-RUN"); - SERIAL_ECHOPGM_P(enddryrun); + FSTR_P const enddryrun = F("End DRY-RUN"); + SERIAL_ECHOF(enddryrun); SERIAL_ECHO_SP(35); SERIAL_ECHOLNPAIR_F("std dev:", zero_std_dev, 3); char mess[21]; - strcpy_P(mess, enddryrun); + strcpy_P(mess, FTOP(enddryrun)); strcpy_P(&mess[11], PSTR(" sd:")); if (zero_std_dev < 1) sprintf_P(&mess[15], PSTR("0.%03i"), (int)LROUND(zero_std_dev * 1000.0f)); @@ -643,6 +659,8 @@ void GcodeSuite::G33() { while (((zero_std_dev < test_precision && iterations < 31) || iterations <= force_iterations) && zero_std_dev > calibration_precision); ac_cleanup(TERN_(HAS_MULTI_HOTEND, old_tool_index)); + + TERN_(FULL_REPORT_TO_HOST_FEATURE, set_and_report_grblstate(M_IDLE)); } #endif // DELTA_AUTO_CALIBRATION diff --git a/Marlin/src/gcode/calibrate/G34.cpp b/Marlin/src/gcode/calibrate/G34.cpp index bcca00dd42b3..ea5d5fa150cf 100644 --- a/Marlin/src/gcode/calibrate/G34.cpp +++ b/Marlin/src/gcode/calibrate/G34.cpp @@ -39,7 +39,7 @@ void GcodeSuite::G34() { // Home before the alignment procedure - if (!all_axes_trusted()) home_all_axes(); + home_if_needed(); TERN_(HAS_LEVELING, TEMPORARY_BED_LEVELING_STATE(false)); @@ -47,7 +47,7 @@ void GcodeSuite::G34() { TemporaryGlobalEndstopsState unlock_z(false); #ifdef GANTRY_CALIBRATION_COMMANDS_PRE - gcode.process_subcommands_now_P(PSTR(GANTRY_CALIBRATION_COMMANDS_PRE)); + process_subcommands_now(F(GANTRY_CALIBRATION_COMMANDS_PRE)); if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPGM("Sub Commands Processed"); #endif @@ -81,11 +81,11 @@ void GcodeSuite::G34() { const uint16_t target_current = parser.intval('S', GANTRY_CALIBRATION_CURRENT); const uint32_t previous_current = stepper.motor_current_setting[Z_AXIS]; stepper.set_digipot_current(1, target_current); - #elif ENABLED(HAS_MOTOR_CURRENT_DAC) + #elif HAS_MOTOR_CURRENT_DAC const float target_current = parser.floatval('S', GANTRY_CALIBRATION_CURRENT); const float previous_current = dac_amps(Z_AXIS, target_current); stepper_dac.set_current_value(Z_AXIS, target_current); - #elif ENABLED(HAS_MOTOR_CURRENT_I2C) + #elif HAS_MOTOR_CURRENT_I2C const uint16_t target_current = parser.intval('S', GANTRY_CALIBRATION_CURRENT); previous_current = dac_amps(Z_AXIS); digipot_i2c.set_current(Z_AXIS, target_current) @@ -114,10 +114,6 @@ void GcodeSuite::G34() { if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPGM("Final Z Move"); do_blocking_move_to_z(zgrind, MMM_TO_MMS(GANTRY_CALIBRATION_FEEDRATE)); - // Back off end plate, back to normal motion range - if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPGM("Z Backoff"); - do_blocking_move_to_z(zpounce, MMM_TO_MMS(GANTRY_CALIBRATION_FEEDRATE)); - #if _REDUCE_CURRENT // Reset current to original values if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPGM("Restore Current"); @@ -127,9 +123,9 @@ void GcodeSuite::G34() { stepper.set_digipot_current(Z_AXIS, previous_current); #elif HAS_MOTOR_CURRENT_PWM stepper.set_digipot_current(1, previous_current); - #elif ENABLED(HAS_MOTOR_CURRENT_DAC) + #elif HAS_MOTOR_CURRENT_DAC stepper_dac.set_current_value(Z_AXIS, previous_current); - #elif ENABLED(HAS_MOTOR_CURRENT_I2C) + #elif HAS_MOTOR_CURRENT_I2C digipot_i2c.set_current(Z_AXIS, previous_current) #elif HAS_TRINAMIC_CONFIG #if AXIS_IS_TMC(Z) @@ -146,9 +142,13 @@ void GcodeSuite::G34() { #endif #endif + // Back off end plate, back to normal motion range + if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPGM("Z Backoff"); + do_blocking_move_to_z(zpounce, MMM_TO_MMS(GANTRY_CALIBRATION_FEEDRATE)); + #ifdef GANTRY_CALIBRATION_COMMANDS_POST if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPGM("Running Post Commands"); - gcode.process_subcommands_now_P(PSTR(GANTRY_CALIBRATION_COMMANDS_POST)); + process_subcommands_now(F(GANTRY_CALIBRATION_COMMANDS_POST)); #endif SET_SOFT_ENDSTOP_LOOSE(false); diff --git a/Marlin/src/gcode/calibrate/G34_M422.cpp b/Marlin/src/gcode/calibrate/G34_M422.cpp index 50476e8e7c25..328a40dbb46a 100644 --- a/Marlin/src/gcode/calibrate/G34_M422.cpp +++ b/Marlin/src/gcode/calibrate/G34_M422.cpp @@ -31,7 +31,7 @@ #include "../../module/stepper.h" #include "../../module/planner.h" #include "../../module/probe.h" -#include "../../lcd/marlinui.h" // for LCD_MESSAGEPGM +#include "../../lcd/marlinui.h" // for LCD_MESSAGE #if HAS_LEVELING #include "../../feature/bedlevel/bedlevel.h" @@ -45,9 +45,20 @@ #include "../../libs/least_squares_fit.h" #endif +#if ENABLED(BLTOUCH) + #include "../../feature/bltouch.h" +#endif + #define DEBUG_OUT ENABLED(DEBUG_LEVELING_FEATURE) #include "../../core/debug_out.h" +#if NUM_Z_STEPPER_DRIVERS >= 3 + #define TRIPLE_Z 1 + #if NUM_Z_STEPPER_DRIVERS >= 4 + #define QUAD_Z 1 + #endif +#endif + /** * G34: Z-Stepper automatic alignment * @@ -82,9 +93,9 @@ void GcodeSuite::G34() { switch (parser.intval('Z')) { case 1: stepper.set_z1_lock(state); break; case 2: stepper.set_z2_lock(state); break; - #if NUM_Z_STEPPER_DRIVERS >= 3 + #if TRIPLE_Z case 3: stepper.set_z3_lock(state); break; - #if NUM_Z_STEPPER_DRIVERS >= 4 + #if QUAD_Z case 4: stepper.set_z4_lock(state); break; #endif #endif @@ -99,13 +110,6 @@ void GcodeSuite::G34() { #if ENABLED(Z_STEPPER_AUTO_ALIGN) do { // break out on error - #if NUM_Z_STEPPER_DRIVERS == 4 - SERIAL_ECHOLNPGM("Alignment for 4 steppers is Experimental!"); - #elif NUM_Z_STEPPER_DRIVERS > 4 - SERIAL_ECHOLNPGM("Alignment not supported for over 4 steppers"); - break; - #endif - const int8_t z_auto_align_iterations = parser.intval('I', Z_STEPPER_ALIGN_ITERATIONS); if (!WITHIN(z_auto_align_iterations, 1, 30)) { SERIAL_ECHOLNPGM("?(I)teration out of bounds (1-30)."); @@ -130,7 +134,9 @@ void GcodeSuite::G34() { // Disable the leveling matrix before auto-aligning #if HAS_LEVELING - TERN_(RESTORE_LEVELING_AFTER_G34, const bool leveling_was_active = planner.leveling_active); + #if ENABLED(RESTORE_LEVELING_AFTER_G34) + const bool leveling_was_active = planner.leveling_active; + #endif set_bed_leveling_enabled(false); #endif @@ -147,7 +153,7 @@ void GcodeSuite::G34() { // In BLTOUCH HS mode, the probe travels in a deployed state. // Users of G34 might have a badly misaligned bed, so raise Z by the // length of the deployed pin (BLTOUCH stroke < 7mm) - #define Z_BASIC_CLEARANCE (Z_CLEARANCE_BETWEEN_PROBES + 7.0f * BOTH(BLTOUCH, BLTOUCH_HS_MODE)) + #define Z_BASIC_CLEARANCE (Z_CLEARANCE_BETWEEN_PROBES + TERN0(BLTOUCH, bltouch.z_extra_clearance())) // Compute a worst-case clearance height to probe from. After the first // iteration this will be re-calculated based on the actual bed position @@ -155,19 +161,17 @@ void GcodeSuite::G34() { const xy_pos_t diff = z_stepper_align.xy[i] - z_stepper_align.xy[j]; return HYPOT2(diff.x, diff.y); }; - float z_probe = Z_BASIC_CLEARANCE + (G34_MAX_GRADE) * 0.01f * SQRT( - #if NUM_Z_STEPPER_DRIVERS == 3 - _MAX(magnitude2(0, 1), magnitude2(1, 2), magnitude2(2, 0)) - #elif NUM_Z_STEPPER_DRIVERS == 4 - _MAX(magnitude2(0, 1), magnitude2(1, 2), magnitude2(2, 3), - magnitude2(3, 0), magnitude2(0, 2), magnitude2(1, 3)) - #else - magnitude2(0, 1) + float z_probe = Z_BASIC_CLEARANCE + (G34_MAX_GRADE) * 0.01f * SQRT(_MAX(0, magnitude2(0, 1) + #if TRIPLE_Z + , magnitude2(2, 1), magnitude2(2, 0) + #if QUAD_Z + , magnitude2(3, 2), magnitude2(3, 1), magnitude2(3, 0) + #endif #endif - ); + )); // Home before the alignment procedure - if (!all_axes_trusted()) home_all_axes(); + home_if_needed(); // Move the Z coordinate realm towards the positive - dirty trick current_position.z += z_probe * 0.5f; @@ -176,7 +180,7 @@ void GcodeSuite::G34() { // This hack is un-done at the end of G34 - either by re-homing, or by using the probed heights of the last iteration. #if DISABLED(Z_STEPPER_ALIGN_KNOWN_STEPPER_POSITIONS) - float last_z_align_move[NUM_Z_STEPPER_DRIVERS] = ARRAY_N(NUM_Z_STEPPER_DRIVERS, 10000.0f, 10000.0f, 10000.0f, 10000.0f); + float last_z_align_move[NUM_Z_STEPPER_DRIVERS] = ARRAY_N_1(NUM_Z_STEPPER_DRIVERS, 10000.0f); #else float last_z_align_level_indicator = 10000.0f; #endif @@ -188,7 +192,7 @@ void GcodeSuite::G34() { bool adjustment_reverse = false; #endif - #if HAS_DISPLAY + #if HAS_STATUS_MESSAGE PGM_P const msg_iteration = GET_TEXT(MSG_ITERATION); const uint8_t iter_str_len = strlen_P(msg_iteration); #endif @@ -201,8 +205,8 @@ void GcodeSuite::G34() { if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPGM("> probing all positions."); const int iter = iteration + 1; - SERIAL_ECHOLNPAIR("\nG34 Iteration: ", iter); - #if HAS_DISPLAY + SERIAL_ECHOLNPGM("\nG34 Iteration: ", iter); + #if HAS_STATUS_MESSAGE char str[iter_str_len + 2 + 1]; sprintf_P(str, msg_iteration, iter); ui.set_status(str); @@ -221,7 +225,7 @@ void GcodeSuite::G34() { if ((iteration == 0 || i > 0) && z_probe > current_position.z) do_blocking_move_to_z(z_probe); if (DEBUGGING(LEVELING)) - DEBUG_ECHOLNPAIR_P(PSTR("Probing X"), z_stepper_align.xy[iprobe].x, SP_Y_STR, z_stepper_align.xy[iprobe].y); + DEBUG_ECHOLNPGM_P(PSTR("Probing X"), z_stepper_align.xy[iprobe].x, SP_Y_STR, z_stepper_align.xy[iprobe].y); // Probe a Z height for each stepper. // Probing sanity check is disabled, as it would trigger even in normal cases because @@ -229,7 +233,7 @@ void GcodeSuite::G34() { const float z_probed_height = probe.probe_at_point(z_stepper_align.xy[iprobe], raise_after, 0, true, false); if (isnan(z_probed_height)) { SERIAL_ECHOLNPGM("Probing failed"); - LCD_MESSAGEPGM(MSG_LCD_PROBING_FAILED); + LCD_MESSAGE(MSG_LCD_PROBING_FAILED); err_break = true; break; } @@ -238,7 +242,7 @@ void GcodeSuite::G34() { // the next iteration of probing. This allows adjustments to be made away from the bed. z_measured[iprobe] = z_probed_height + Z_CLEARANCE_BETWEEN_PROBES; - if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPAIR("> Z", iprobe + 1, " measured position is ", z_measured[iprobe]); + if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPGM("> Z", iprobe + 1, " measured position is ", z_measured[iprobe]); // Remember the minimum measurement to calculate the correction later on z_measured_min = _MIN(z_measured_min, z_measured[iprobe]); @@ -267,7 +271,7 @@ void GcodeSuite::G34() { linear_fit_data lfd; incremental_LSF_reset(&lfd); LOOP_L_N(i, NUM_Z_STEPPER_DRIVERS) { - SERIAL_ECHOLNPAIR("PROBEPT_", i, ": ", z_measured[i]); + SERIAL_ECHOLNPGM("PROBEPT_", i, ": ", z_measured[i]); incremental_LSF(&lfd, z_stepper_align.xy[i], z_measured[i]); } finish_incremental_LSF(&lfd); @@ -278,42 +282,57 @@ void GcodeSuite::G34() { z_measured_min = _MIN(z_measured_min, z_measured[i]); } - SERIAL_ECHOLNPAIR("CALCULATED STEPPER POSITIONS: Z1=", z_measured[0], " Z2=", z_measured[1], " Z3=", z_measured[2]); + SERIAL_ECHOLNPGM( + LIST_N(DOUBLE(NUM_Z_STEPPER_DRIVERS), + "Calculated Z1=", z_measured[0], + " Z2=", z_measured[1], + " Z3=", z_measured[2], + " Z4=", z_measured[3] + ) + ); #endif - SERIAL_ECHOLNPAIR("\n" - "DIFFERENCE Z1-Z2=", ABS(z_measured[0] - z_measured[1]) - #if NUM_Z_STEPPER_DRIVERS == 3 - , " Z2-Z3=", ABS(z_measured[1] - z_measured[2]) + SERIAL_ECHOLNPGM("\n" + "Z2-Z1=", ABS(z_measured[1] - z_measured[0]) + #if TRIPLE_Z + , " Z3-Z2=", ABS(z_measured[2] - z_measured[1]) , " Z3-Z1=", ABS(z_measured[2] - z_measured[0]) + #if QUAD_Z + , " Z4-Z3=", ABS(z_measured[3] - z_measured[2]) + , " Z4-Z2=", ABS(z_measured[3] - z_measured[1]) + , " Z4-Z1=", ABS(z_measured[3] - z_measured[0]) + #endif #endif ); - #if HAS_DISPLAY + + #if HAS_STATUS_MESSAGE char fstr1[10]; - #if NUM_Z_STEPPER_DRIVERS == 2 - char msg[6 + (6 + 5) * 1 + 1]; - #else - char msg[6 + (6 + 5) * 3 + 1], fstr2[10], fstr3[10]; - #endif - sprintf_P(msg, - PSTR("Diffs Z1-Z2=%s" - #if NUM_Z_STEPPER_DRIVERS == 3 - " Z2-Z3=%s" - " Z3-Z1=%s" + char msg[6 + (6 + 5) * NUM_Z_STEPPER_DRIVERS + 1] + #if TRIPLE_Z + , fstr2[10], fstr3[10] + #if QUAD_Z + , fstr4[10], fstr5[10], fstr6[10] #endif - ), dtostrf(ABS(z_measured[0] - z_measured[1]), 1, 3, fstr1) - #if NUM_Z_STEPPER_DRIVERS == 3 - , dtostrf(ABS(z_measured[1] - z_measured[2]), 1, 3, fstr2) - , dtostrf(ABS(z_measured[2] - z_measured[0]), 1, 3, fstr3) #endif + ; + sprintf_P(msg, + PSTR("1:2=%s" TERN_(TRIPLE_Z, " 3-2=%s 3-1=%s") TERN_(QUAD_Z, " 4-3=%s 4-2=%s 4-1=%s")), + dtostrf(ABS(z_measured[1] - z_measured[0]), 1, 3, fstr1) + OPTARG(TRIPLE_Z, + dtostrf(ABS(z_measured[2] - z_measured[1]), 1, 3, fstr2), + dtostrf(ABS(z_measured[2] - z_measured[0]), 1, 3, fstr3)) + OPTARG(QUAD_Z, + dtostrf(ABS(z_measured[3] - z_measured[2]), 1, 3, fstr4), + dtostrf(ABS(z_measured[3] - z_measured[1]), 1, 3, fstr5), + dtostrf(ABS(z_measured[3] - z_measured[0]), 1, 3, fstr6)) ); ui.set_status(msg); #endif - auto decreasing_accuracy = [](const float &v1, const float &v2){ + auto decreasing_accuracy = [](const_float_t v1, const_float_t v2) { if (v1 < v2 * 0.7f) { SERIAL_ECHOLNPGM("Decreasing Accuracy Detected."); - LCD_MESSAGEPGM(MSG_DECREASING_ACCURACY); + LCD_MESSAGE(MSG_DECREASING_ACCURACY); return true; } return false; @@ -357,8 +376,8 @@ void GcodeSuite::G34() { // Check for less accuracy compared to last move if (decreasing_accuracy(last_z_align_move[zstepper], z_align_abs)) { - if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPAIR("> Z", zstepper + 1, " last_z_align_move = ", last_z_align_move[zstepper]); - if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPAIR("> Z", zstepper + 1, " z_align_abs = ", z_align_abs); + if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPGM("> Z", zstepper + 1, " last_z_align_move = ", last_z_align_move[zstepper]); + if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPGM("> Z", zstepper + 1, " z_align_abs = ", z_align_abs); adjustment_reverse = !adjustment_reverse; } @@ -370,7 +389,7 @@ void GcodeSuite::G34() { // Stop early if all measured points achieve accuracy target if (z_align_abs > z_auto_align_accuracy) success_break = false; - if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPAIR("> Z", zstepper + 1, " corrected by ", z_align_move); + if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPGM("> Z", zstepper + 1, " corrected by ", z_align_move); // Lock all steppers except one stepper.set_all_z_lock(true, zstepper); @@ -380,7 +399,7 @@ void GcodeSuite::G34() { // Will match reversed Z steppers on dual steppers. Triple will need more work to map. if (adjustment_reverse) { z_align_move = -z_align_move; - if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPAIR("> Z", zstepper + 1, " correction reversed to ", z_align_move); + if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPGM("> Z", zstepper + 1, " correction reversed to ", z_align_move); } #endif @@ -396,7 +415,7 @@ void GcodeSuite::G34() { if (success_break) { SERIAL_ECHOLNPGM("Target accuracy achieved."); - LCD_MESSAGEPGM(MSG_ACCURACY_ACHIEVED); + LCD_MESSAGE(MSG_ACCURACY_ACHIEVED); break; } @@ -406,19 +425,19 @@ void GcodeSuite::G34() { if (err_break) SERIAL_ECHOLNPGM("G34 aborted."); else { - SERIAL_ECHOLNPAIR("Did ", iteration + (iteration != z_auto_align_iterations), " of ", z_auto_align_iterations); + SERIAL_ECHOLNPGM("Did ", iteration + (iteration != z_auto_align_iterations), " of ", z_auto_align_iterations); SERIAL_ECHOLNPAIR_F("Accuracy: ", z_maxdiff); } - // Stow the probe, as the last call to probe.probe_at_point(...) left - // the probe deployed if it was successful. - probe.stow(); + // Stow the probe because the last call to probe.probe_at_point(...) + // leaves the probe deployed when it's successful. + IF_DISABLED(TOUCH_MI_PROBE, probe.stow()); #if ENABLED(HOME_AFTER_G34) // After this operation the z position needs correction set_axis_never_homed(Z_AXIS); // Home Z after the alignment procedure - process_subcommands_now_P(PSTR("G28Z")); + process_subcommands_now(F("G28Z")); #else // Use the probed height from the last iteration to determine the Z height. // z_measured_min is used, because all steppers are aligned to z_measured_min. @@ -435,7 +454,7 @@ void GcodeSuite::G34() { #endif }while(0); - #endif + #endif // Z_STEPPER_AUTO_ALIGN } #endif // Z_MULTI_ENDSTOPS || Z_STEPPER_AUTO_ALIGN @@ -460,21 +479,13 @@ void GcodeSuite::G34() { */ void GcodeSuite::M422() { + if (!parser.seen_any()) return M422_report(); + if (parser.seen('R')) { z_stepper_align.reset_to_default(); return; } - if (!parser.seen_any()) { - LOOP_L_N(i, NUM_Z_STEPPER_DRIVERS) - SERIAL_ECHOLNPAIR_P(PSTR("M422 S"), i + 1, SP_X_STR, z_stepper_align.xy[i].x, SP_Y_STR, z_stepper_align.xy[i].y); - #if ENABLED(Z_STEPPER_ALIGN_KNOWN_STEPPER_POSITIONS) - LOOP_L_N(i, NUM_Z_STEPPER_DRIVERS) - SERIAL_ECHOLNPAIR_P(PSTR("M422 W"), i + 1, SP_X_STR, z_stepper_align.stepper_xy[i].x, SP_Y_STR, z_stepper_align.stepper_xy[i].y); - #endif - return; - } - const bool is_probe_point = parser.seen('S'); if (TERN0(Z_STEPPER_ALIGN_KNOWN_STEPPER_POSITIONS, is_probe_point && parser.seen('W'))) { @@ -530,4 +541,26 @@ void GcodeSuite::M422() { pos_dest[position_index] = pos; } +void GcodeSuite::M422_report(const bool forReplay/*=true*/) { + report_heading(forReplay, F(STR_Z_AUTO_ALIGN)); + LOOP_L_N(i, NUM_Z_STEPPER_DRIVERS) { + report_echo_start(forReplay); + SERIAL_ECHOLNPGM_P( + PSTR(" M422 S"), i + 1, + SP_X_STR, z_stepper_align.xy[i].x, + SP_Y_STR, z_stepper_align.xy[i].y + ); + } + #if ENABLED(Z_STEPPER_ALIGN_KNOWN_STEPPER_POSITIONS) + LOOP_L_N(i, NUM_Z_STEPPER_DRIVERS) { + report_echo_start(forReplay); + SERIAL_ECHOLNPGM_P( + PSTR(" M422 W"), i + 1, + SP_X_STR, z_stepper_align.stepper_xy[i].x, + SP_Y_STR, z_stepper_align.stepper_xy[i].y + ); + } + #endif +} + #endif // Z_STEPPER_AUTO_ALIGN diff --git a/Marlin/src/gcode/calibrate/G425.cpp b/Marlin/src/gcode/calibrate/G425.cpp index 0918bc9d4fec..906f8cc4194a 100644 --- a/Marlin/src/gcode/calibrate/G425.cpp +++ b/Marlin/src/gcode/calibrate/G425.cpp @@ -73,11 +73,23 @@ #if BOTH(CALIBRATION_MEASURE_LEFT, CALIBRATION_MEASURE_RIGHT) #define HAS_X_CENTER 1 #endif -#if BOTH(CALIBRATION_MEASURE_FRONT, CALIBRATION_MEASURE_BACK) +#if ALL(HAS_Y_AXIS, CALIBRATION_MEASURE_FRONT, CALIBRATION_MEASURE_BACK) #define HAS_Y_CENTER 1 #endif +#if ALL(HAS_I_AXIS, CALIBRATION_MEASURE_IMIN, CALIBRATION_MEASURE_IMAX) + #define HAS_I_CENTER 1 +#endif +#if ALL(HAS_J_AXIS, CALIBRATION_MEASURE_JMIN, CALIBRATION_MEASURE_JMAX) + #define HAS_J_CENTER 1 +#endif +#if ALL(HAS_K_AXIS, CALIBRATION_MEASURE_KMIN, CALIBRATION_MEASURE_KMAX) + #define HAS_K_CENTER 1 +#endif -enum side_t : uint8_t { TOP, RIGHT, FRONT, LEFT, BACK, NUM_SIDES }; +enum side_t : uint8_t { + TOP, RIGHT, FRONT, LEFT, BACK, NUM_SIDES, + LIST_N(DOUBLE(SUB3(LINEAR_AXES)), IMINIMUM, IMAXIMUM, JMINIMUM, JMAXIMUM, KMINIMUM, KMAXIMUM) +}; static constexpr xyz_pos_t true_center CALIBRATION_OBJECT_CENTER; static constexpr xyz_float_t dimensions CALIBRATION_OBJECT_DIMENSIONS; @@ -105,7 +117,7 @@ struct measurements_t { #endif inline void calibration_move() { - do_blocking_move_to(current_position, MMM_TO_MMS(CALIBRATION_FEEDRATE_TRAVEL)); + do_blocking_move_to((xyz_pos_t)current_position, MMM_TO_MMS(CALIBRATION_FEEDRATE_TRAVEL)); } /** @@ -174,7 +186,7 @@ float measuring_movement(const AxisEnum axis, const int dir, const bool stop_sta destination = current_position; for (float travel = 0; travel < limit; travel += step) { destination[axis] += dir * step; - do_blocking_move_to(destination, mms); + do_blocking_move_to((xyz_pos_t)destination, mms); planner.synchronize(); if (read_calibration_pin() == stop_state) break; } @@ -194,18 +206,22 @@ float measuring_movement(const AxisEnum axis, const int dir, const bool stop_sta inline float measure(const AxisEnum axis, const int dir, const bool stop_state, float * const backlash_ptr, const float uncertainty) { const bool fast = uncertainty == CALIBRATION_MEASUREMENT_UNKNOWN; - // Save position - destination = current_position; - const float start_pos = destination[axis]; + // Save the current position of the specified axis + const float start_pos = current_position[axis]; + + // Take a measurement. Only the specified axis will be affected. const float measured_pos = measuring_movement(axis, dir, stop_state, fast); + // Measure backlash if (backlash_ptr && !fast) { const float release_pos = measuring_movement(axis, -dir, !stop_state, fast); *backlash_ptr = ABS(release_pos - measured_pos); } - // Return to starting position + + // Move back to the starting position + destination = current_position; destination[axis] = start_pos; - do_blocking_move_to(destination, MMM_TO_MMS(CALIBRATION_FEEDRATE_TRAVEL)); + do_blocking_move_to((xyz_pos_t)destination, MMM_TO_MMS(CALIBRATION_FEEDRATE_TRAVEL)); return measured_pos; } @@ -225,8 +241,17 @@ inline void probe_side(measurements_t &m, const float uncertainty, const side_t park_above_object(m, uncertainty); + #define _ACASE(N,A,B) case A: dir = -1; case B: axis = N##_AXIS; break + #define _PCASE(N) _ACASE(N, N##MINIMUM, N##MAXIMUM) + switch (side) { - #if AXIS_CAN_CALIBRATE(Z) + #if AXIS_CAN_CALIBRATE(X) + _ACASE(X, RIGHT, LEFT); + #endif + #if HAS_Y_AXIS && AXIS_CAN_CALIBRATE(Y) + _ACASE(Y, BACK, FRONT); + #endif + #if HAS_Z_AXIS && AXIS_CAN_CALIBRATE(Z) case TOP: { const float measurement = measure(Z_AXIS, -1, true, &m.backlash[TOP], uncertainty); m.obj_center.z = measurement - dimensions.z / 2; @@ -234,13 +259,14 @@ inline void probe_side(measurements_t &m, const float uncertainty, const side_t return; } #endif - #if AXIS_CAN_CALIBRATE(X) - case LEFT: axis = X_AXIS; break; - case RIGHT: axis = X_AXIS; dir = -1; break; + #if HAS_I_AXIS && AXIS_CAN_CALIBRATE(I) + _PCASE(I); #endif - #if AXIS_CAN_CALIBRATE(Y) - case FRONT: axis = Y_AXIS; break; - case BACK: axis = Y_AXIS; dir = -1; break; + #if HAS_J_AXIS && AXIS_CAN_CALIBRATE(J) + _PCASE(J); + #endif + #if HAS_K_AXIS && AXIS_CAN_CALIBRATE(K) + _PCASE(K); #endif default: return; } @@ -285,14 +311,23 @@ inline void probe_sides(measurements_t &m, const float uncertainty) { probe_side(m, uncertainty, TOP); #endif - TERN_(CALIBRATION_MEASURE_RIGHT, probe_side(m, uncertainty, RIGHT, probe_top_at_edge)); - TERN_(CALIBRATION_MEASURE_FRONT, probe_side(m, uncertainty, FRONT, probe_top_at_edge)); - TERN_(CALIBRATION_MEASURE_LEFT, probe_side(m, uncertainty, LEFT, probe_top_at_edge)); - TERN_(CALIBRATION_MEASURE_BACK, probe_side(m, uncertainty, BACK, probe_top_at_edge)); + TERN_(CALIBRATION_MEASURE_RIGHT, probe_side(m, uncertainty, RIGHT, probe_top_at_edge)); + TERN_(CALIBRATION_MEASURE_FRONT, probe_side(m, uncertainty, FRONT, probe_top_at_edge)); + TERN_(CALIBRATION_MEASURE_LEFT, probe_side(m, uncertainty, LEFT, probe_top_at_edge)); + TERN_(CALIBRATION_MEASURE_BACK, probe_side(m, uncertainty, BACK, probe_top_at_edge)); + TERN_(CALIBRATION_MEASURE_IMIN, probe_side(m, uncertainty, IMINIMUM, probe_top_at_edge)); + TERN_(CALIBRATION_MEASURE_IMAX, probe_side(m, uncertainty, IMAXIMUM, probe_top_at_edge)); + TERN_(CALIBRATION_MEASURE_JMIN, probe_side(m, uncertainty, JMINIMUM, probe_top_at_edge)); + TERN_(CALIBRATION_MEASURE_JMAX, probe_side(m, uncertainty, JMAXIMUM, probe_top_at_edge)); + TERN_(CALIBRATION_MEASURE_KMIN, probe_side(m, uncertainty, KMINIMUM, probe_top_at_edge)); + TERN_(CALIBRATION_MEASURE_KMAX, probe_side(m, uncertainty, KMAXIMUM, probe_top_at_edge)); // Compute the measured center of the calibration object. - TERN_(HAS_X_CENTER, m.obj_center.x = (m.obj_side[LEFT] + m.obj_side[RIGHT]) / 2); - TERN_(HAS_Y_CENTER, m.obj_center.y = (m.obj_side[FRONT] + m.obj_side[BACK]) / 2); + TERN_(HAS_X_CENTER, m.obj_center.x = (m.obj_side[LEFT] + m.obj_side[RIGHT]) / 2); + TERN_(HAS_Y_CENTER, m.obj_center.y = (m.obj_side[FRONT] + m.obj_side[BACK]) / 2); + TERN_(HAS_I_CENTER, m.obj_center.i = (m.obj_side[IMINIMUM] + m.obj_side[IMAXIMUM]) / 2); + TERN_(HAS_J_CENTER, m.obj_center.j = (m.obj_side[JMINIMUM] + m.obj_side[JMAXIMUM]) / 2); + TERN_(HAS_K_CENTER, m.obj_center.k = (m.obj_side[KMINIMUM] + m.obj_side[KMAXIMUM]) / 2); // Compute the outside diameter of the nozzle at the height // at which it makes contact with the calibration object @@ -303,36 +338,59 @@ inline void probe_sides(measurements_t &m, const float uncertainty) { // The difference between the known and the measured location // of the calibration object is the positional error - m.pos_error.x = (0 - #if HAS_X_CENTER - + true_center.x - m.obj_center.x - #endif + LINEAR_AXIS_CODE( + m.pos_error.x = TERN0(HAS_X_CENTER, true_center.x - m.obj_center.x), + m.pos_error.y = TERN0(HAS_Y_CENTER, true_center.y - m.obj_center.y), + m.pos_error.z = true_center.z - m.obj_center.z, + m.pos_error.i = TERN0(HAS_I_CENTER, true_center.i - m.obj_center.i), + m.pos_error.j = TERN0(HAS_J_CENTER, true_center.j - m.obj_center.j), + m.pos_error.k = TERN0(HAS_K_CENTER, true_center.k - m.obj_center.k) ); - m.pos_error.y = (0 - #if HAS_Y_CENTER - + true_center.y - m.obj_center.y - #endif - ); - m.pos_error.z = true_center.z - m.obj_center.z; } #if ENABLED(CALIBRATION_REPORTING) inline void report_measured_faces(const measurements_t &m) { SERIAL_ECHOLNPGM("Sides:"); - #if AXIS_CAN_CALIBRATE(Z) - SERIAL_ECHOLNPAIR(" Top: ", m.obj_side[TOP]); + #if HAS_Z_AXIS && AXIS_CAN_CALIBRATE(Z) + SERIAL_ECHOLNPGM(" Top: ", m.obj_side[TOP]); #endif #if ENABLED(CALIBRATION_MEASURE_LEFT) - SERIAL_ECHOLNPAIR(" Left: ", m.obj_side[LEFT]); + SERIAL_ECHOLNPGM(" Left: ", m.obj_side[LEFT]); #endif #if ENABLED(CALIBRATION_MEASURE_RIGHT) - SERIAL_ECHOLNPAIR(" Right: ", m.obj_side[RIGHT]); + SERIAL_ECHOLNPGM(" Right: ", m.obj_side[RIGHT]); + #endif + #if HAS_Y_AXIS + #if ENABLED(CALIBRATION_MEASURE_FRONT) + SERIAL_ECHOLNPGM(" Front: ", m.obj_side[FRONT]); + #endif + #if ENABLED(CALIBRATION_MEASURE_BACK) + SERIAL_ECHOLNPGM(" Back: ", m.obj_side[BACK]); + #endif + #endif + #if HAS_I_AXIS + #if ENABLED(CALIBRATION_MEASURE_IMIN) + SERIAL_ECHOLNPGM(" " STR_I_MIN ": ", m.obj_side[IMINIMUM]); + #endif + #if ENABLED(CALIBRATION_MEASURE_IMAX) + SERIAL_ECHOLNPGM(" " STR_I_MAX ": ", m.obj_side[IMAXIMUM]); + #endif #endif - #if ENABLED(CALIBRATION_MEASURE_FRONT) - SERIAL_ECHOLNPAIR(" Front: ", m.obj_side[FRONT]); + #if HAS_J_AXIS + #if ENABLED(CALIBRATION_MEASURE_JMIN) + SERIAL_ECHOLNPGM(" " STR_J_MIN ": ", m.obj_side[JMINIMUM]); + #endif + #if ENABLED(CALIBRATION_MEASURE_JMAX) + SERIAL_ECHOLNPGM(" " STR_J_MAX ": ", m.obj_side[JMAXIMUM]); + #endif #endif - #if ENABLED(CALIBRATION_MEASURE_BACK) - SERIAL_ECHOLNPAIR(" Back: ", m.obj_side[BACK]); + #if HAS_K_AXIS + #if ENABLED(CALIBRATION_MEASURE_KMIN) + SERIAL_ECHOLNPGM(" " STR_K_MIN ": ", m.obj_side[KMINIMUM]); + #endif + #if ENABLED(CALIBRATION_MEASURE_KMAX) + SERIAL_ECHOLNPGM(" " STR_K_MAX ": ", m.obj_side[KMAXIMUM]); + #endif #endif SERIAL_EOL(); } @@ -340,12 +398,21 @@ inline void probe_sides(measurements_t &m, const float uncertainty) { inline void report_measured_center(const measurements_t &m) { SERIAL_ECHOLNPGM("Center:"); #if HAS_X_CENTER - SERIAL_ECHOLNPAIR_P(SP_X_STR, m.obj_center.x); + SERIAL_ECHOLNPGM_P(SP_X_STR, m.obj_center.x); #endif #if HAS_Y_CENTER - SERIAL_ECHOLNPAIR_P(SP_Y_STR, m.obj_center.y); + SERIAL_ECHOLNPGM_P(SP_Y_STR, m.obj_center.y); + #endif + SERIAL_ECHOLNPGM_P(SP_Z_STR, m.obj_center.z); + #if HAS_I_CENTER + SERIAL_ECHOLNPGM_P(SP_I_STR, m.obj_center.i); + #endif + #if HAS_J_CENTER + SERIAL_ECHOLNPGM_P(SP_J_STR, m.obj_center.j); + #endif + #if HAS_K_CENTER + SERIAL_ECHOLNPGM_P(SP_K_STR, m.obj_center.k); #endif - SERIAL_ECHOLNPAIR_P(SP_Z_STR, m.obj_center.z); SERIAL_EOL(); } @@ -353,22 +420,46 @@ inline void probe_sides(measurements_t &m, const float uncertainty) { SERIAL_ECHOLNPGM("Backlash:"); #if AXIS_CAN_CALIBRATE(X) #if ENABLED(CALIBRATION_MEASURE_LEFT) - SERIAL_ECHOLNPAIR(" Left: ", m.backlash[LEFT]); + SERIAL_ECHOLNPGM(" Left: ", m.backlash[LEFT]); #endif #if ENABLED(CALIBRATION_MEASURE_RIGHT) - SERIAL_ECHOLNPAIR(" Right: ", m.backlash[RIGHT]); + SERIAL_ECHOLNPGM(" Right: ", m.backlash[RIGHT]); #endif #endif - #if AXIS_CAN_CALIBRATE(Y) + #if HAS_Y_AXIS && AXIS_CAN_CALIBRATE(Y) #if ENABLED(CALIBRATION_MEASURE_FRONT) - SERIAL_ECHOLNPAIR(" Front: ", m.backlash[FRONT]); + SERIAL_ECHOLNPGM(" Front: ", m.backlash[FRONT]); #endif #if ENABLED(CALIBRATION_MEASURE_BACK) - SERIAL_ECHOLNPAIR(" Back: ", m.backlash[BACK]); + SERIAL_ECHOLNPGM(" Back: ", m.backlash[BACK]); #endif #endif - #if AXIS_CAN_CALIBRATE(Z) - SERIAL_ECHOLNPAIR(" Top: ", m.backlash[TOP]); + #if HAS_Z_AXIS && AXIS_CAN_CALIBRATE(Z) + SERIAL_ECHOLNPGM(" Top: ", m.backlash[TOP]); + #endif + #if HAS_I_AXIS && AXIS_CAN_CALIBRATE(I) + #if ENABLED(CALIBRATION_MEASURE_IMIN) + SERIAL_ECHOLNPGM(" " STR_I_MIN ": ", m.backlash[IMINIMUM]); + #endif + #if ENABLED(CALIBRATION_MEASURE_IMAX) + SERIAL_ECHOLNPGM(" " STR_I_MAX ": ", m.backlash[IMAXIMUM]); + #endif + #endif + #if HAS_J_AXIS && AXIS_CAN_CALIBRATE(J) + #if ENABLED(CALIBRATION_MEASURE_JMIN) + SERIAL_ECHOLNPGM(" " STR_J_MIN ": ", m.backlash[JMINIMUM]); + #endif + #if ENABLED(CALIBRATION_MEASURE_JMAX) + SERIAL_ECHOLNPGM(" " STR_J_MAX ": ", m.backlash[JMAXIMUM]); + #endif + #endif + #if HAS_K_AXIS && AXIS_CAN_CALIBRATE(K) + #if ENABLED(CALIBRATION_MEASURE_KMIN) + SERIAL_ECHOLNPGM(" " STR_K_MIN ": ", m.backlash[KMINIMUM]); + #endif + #if ENABLED(CALIBRATION_MEASURE_KMAX) + SERIAL_ECHOLNPGM(" " STR_K_MAX ": ", m.backlash[KMAXIMUM]); + #endif #endif SERIAL_EOL(); } @@ -377,29 +468,37 @@ inline void probe_sides(measurements_t &m, const float uncertainty) { SERIAL_CHAR('T'); SERIAL_ECHO(active_extruder); SERIAL_ECHOLNPGM(" Positional Error:"); - #if HAS_X_CENTER - SERIAL_ECHOLNPAIR_P(SP_X_STR, m.pos_error.x); + #if HAS_X_CENTER && AXIS_CAN_CALIBRATE(X) + SERIAL_ECHOLNPGM_P(SP_X_STR, m.pos_error.x); #endif - #if HAS_Y_CENTER - SERIAL_ECHOLNPAIR_P(SP_Y_STR, m.pos_error.y); + #if HAS_Y_CENTER && AXIS_CAN_CALIBRATE(Y) + SERIAL_ECHOLNPGM_P(SP_Y_STR, m.pos_error.y); + #endif + #if HAS_Z_AXIS && AXIS_CAN_CALIBRATE(Z) + SERIAL_ECHOLNPGM_P(SP_Z_STR, m.pos_error.z); + #endif + #if HAS_I_CENTER && AXIS_CAN_CALIBRATE(I) + SERIAL_ECHOLNPGM_P(SP_I_STR, m.pos_error.i); + #endif + #if HAS_J_CENTER && AXIS_CAN_CALIBRATE(J) + SERIAL_ECHOLNPGM_P(SP_J_STR, m.pos_error.j); + #endif + #if HAS_K_CENTER && AXIS_CAN_CALIBRATE(K) + SERIAL_ECHOLNPGM_P(SP_Z_STR, m.pos_error.z); #endif - if (AXIS_CAN_CALIBRATE(Z)) SERIAL_ECHOLNPAIR_P(SP_Z_STR, m.pos_error.z); SERIAL_EOL(); } inline void report_measured_nozzle_dimensions(const measurements_t &m) { SERIAL_ECHOLNPGM("Nozzle Tip Outer Dimensions:"); - #if HAS_X_CENTER || HAS_Y_CENTER - #if HAS_X_CENTER - SERIAL_ECHOLNPAIR_P(SP_X_STR, m.nozzle_outer_dimension.x); - #endif - #if HAS_Y_CENTER - SERIAL_ECHOLNPAIR_P(SP_Y_STR, m.nozzle_outer_dimension.y); - #endif - #else - UNUSED(m); + #if HAS_X_CENTER + SERIAL_ECHOLNPGM_P(SP_X_STR, m.nozzle_outer_dimension.x); + #endif + #if HAS_Y_CENTER + SERIAL_ECHOLNPGM_P(SP_Y_STR, m.nozzle_outer_dimension.y); #endif SERIAL_EOL(); + UNUSED(m); } #if HAS_HOTEND_OFFSET @@ -408,7 +507,7 @@ inline void probe_sides(measurements_t &m, const float uncertainty) { // inline void report_hotend_offsets() { LOOP_S_L_N(e, 1, HOTENDS) - SERIAL_ECHOLNPAIR_P(PSTR("T"), e, PSTR(" Hotend Offset X"), hotend_offset[e].x, SP_Y_STR, hotend_offset[e].y, SP_Z_STR, hotend_offset[e].z); + SERIAL_ECHOLNPGM_P(PSTR("T"), e, PSTR(" Hotend Offset X"), hotend_offset[e].x, SP_Y_STR, hotend_offset[e].y, SP_Z_STR, hotend_offset[e].z); } #endif @@ -448,8 +547,33 @@ inline void calibrate_backlash(measurements_t &m, const float uncertainty) { backlash.distance_mm.y = m.backlash[BACK]; #endif - if (AXIS_CAN_CALIBRATE(Z)) backlash.distance_mm.z = m.backlash[TOP]; - #endif + TERN_(HAS_Z_AXIS, if (AXIS_CAN_CALIBRATE(Z)) backlash.distance_mm.z = m.backlash[TOP]); + + #if HAS_I_CENTER + backlash.distance_mm.i = (m.backlash[IMINIMUM] + m.backlash[IMAXIMUM]) / 2; + #elif ENABLED(CALIBRATION_MEASURE_IMIN) + backlash.distance_mm.i = m.backlash[IMINIMUM]; + #elif ENABLED(CALIBRATION_MEASURE_IMAX) + backlash.distance_mm.i = m.backlash[IMAXIMUM]; + #endif + + #if HAS_J_CENTER + backlash.distance_mm.j = (m.backlash[JMINIMUM] + m.backlash[JMAXIMUM]) / 2; + #elif ENABLED(CALIBRATION_MEASURE_JMIN) + backlash.distance_mm.j = m.backlash[JMINIMUM]; + #elif ENABLED(CALIBRATION_MEASURE_JMAX) + backlash.distance_mm.j = m.backlash[JMAXIMUM]; + #endif + + #if HAS_K_CENTER + backlash.distance_mm.k = (m.backlash[KMINIMUM] + m.backlash[KMAXIMUM]) / 2; + #elif ENABLED(CALIBRATION_MEASURE_KMIN) + backlash.distance_mm.k = m.backlash[KMINIMUM]; + #elif ENABLED(CALIBRATION_MEASURE_KMAX) + backlash.distance_mm.k = m.backlash[KMAXIMUM]; + #endif + + #endif // BACKLASH_GCODE } #if ENABLED(BACKLASH_GCODE) @@ -459,7 +583,10 @@ inline void calibrate_backlash(measurements_t &m, const float uncertainty) { // New scope for TEMPORARY_BACKLASH_CORRECTION TEMPORARY_BACKLASH_CORRECTION(all_on); TEMPORARY_BACKLASH_SMOOTHING(0.0f); - const xyz_float_t move = { AXIS_CAN_CALIBRATE(X) * 3, AXIS_CAN_CALIBRATE(Y) * 3, AXIS_CAN_CALIBRATE(Z) * 3 }; + const xyz_float_t move = LINEAR_AXIS_ARRAY( + AXIS_CAN_CALIBRATE(X) * 3, AXIS_CAN_CALIBRATE(Y) * 3, AXIS_CAN_CALIBRATE(Z) * 3, + AXIS_CAN_CALIBRATE(I) * 3, AXIS_CAN_CALIBRATE(J) * 3, AXIS_CAN_CALIBRATE(K) * 3 + ); current_position += move; calibration_move(); current_position -= move; calibration_move(); } @@ -487,11 +614,7 @@ inline void calibrate_toolhead(measurements_t &m, const float uncertainty, const TEMPORARY_BACKLASH_CORRECTION(all_on); TEMPORARY_BACKLASH_SMOOTHING(0.0f); - #if HAS_MULTI_HOTEND - set_nozzle(m, extruder); - #else - UNUSED(extruder); - #endif + TERN(HAS_MULTI_HOTEND, set_nozzle(m, extruder), UNUSED(extruder)); probe_sides(m, uncertainty); @@ -510,6 +633,10 @@ inline void calibrate_toolhead(measurements_t &m, const float uncertainty, const if (ENABLED(HAS_Y_CENTER) && AXIS_CAN_CALIBRATE(Y)) update_measurements(m, Y_AXIS); if (AXIS_CAN_CALIBRATE(Z)) update_measurements(m, Z_AXIS); + TERN_(HAS_I_CENTER, update_measurements(m, I_AXIS)); + TERN_(HAS_J_CENTER, update_measurements(m, J_AXIS)); + TERN_(HAS_K_CENTER, update_measurements(m, K_AXIS)); + sync_plan_position(); } @@ -537,7 +664,7 @@ inline void calibrate_all_toolheads(measurements_t &m, const float uncertainty) * 1) For each nozzle, touch top and sides of object to determine object position and * nozzle offsets. Do a fast but rough search over a wider area. * 2) With the first nozzle, touch top and sides of object to determine backlash values - * for all axis (if BACKLASH_GCODE is enabled) + * for all axes (if BACKLASH_GCODE is enabled) * 3) For each nozzle, touch top and sides of object slowly to determine precise * position of object. Adjust coordinate system and nozzle offsets so probed object * location corresponds to known object location with a high degree of precision. @@ -580,7 +707,7 @@ inline void calibrate_all() { void GcodeSuite::G425() { #ifdef CALIBRATION_SCRIPT_PRE - GcodeSuite::process_subcommands_now_P(PSTR(CALIBRATION_SCRIPT_PRE)); + process_subcommands_now(F(CALIBRATION_SCRIPT_PRE)); #endif if (homing_needed_error()) return; @@ -589,12 +716,12 @@ void GcodeSuite::G425() { SET_SOFT_ENDSTOP_LOOSE(true); measurements_t m; - float uncertainty = parser.seenval('U') ? parser.value_float() : CALIBRATION_MEASUREMENT_UNCERTAIN; + const float uncertainty = parser.floatval('U', CALIBRATION_MEASUREMENT_UNCERTAIN); - if (parser.seen('B')) + if (parser.seen_test('B')) calibrate_backlash(m, uncertainty); - else if (parser.seen('T')) - calibrate_toolhead(m, uncertainty, parser.has_value() ? parser.value_int() : active_extruder); + else if (parser.seen_test('T')) + calibrate_toolhead(m, uncertainty, parser.intval('T', active_extruder)); #if ENABLED(CALIBRATION_REPORTING) else if (parser.seen('V')) { probe_sides(m, uncertainty); @@ -616,7 +743,7 @@ void GcodeSuite::G425() { SET_SOFT_ENDSTOP_LOOSE(false); #ifdef CALIBRATION_SCRIPT_POST - GcodeSuite::process_subcommands_now_P(PSTR(CALIBRATION_SCRIPT_POST)); + process_subcommands_now(F(CALIBRATION_SCRIPT_POST)); #endif } diff --git a/Marlin/src/gcode/calibrate/G76_M192_M871.cpp b/Marlin/src/gcode/calibrate/G76_M192_M871.cpp deleted file mode 100644 index 5d0bb0dc1e0d..000000000000 --- a/Marlin/src/gcode/calibrate/G76_M192_M871.cpp +++ /dev/null @@ -1,369 +0,0 @@ -/** - * Marlin 3D Printer Firmware - * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] - * - * Based on Sprinter and grbl. - * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm - * - * This program is free software: you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation, either version 3 of the License, or - * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with this program. If not, see . - * - */ - -/** - * G76_M871.cpp - Temperature calibration/compensation for z-probing - */ - -#include "../../inc/MarlinConfig.h" - -#if ENABLED(PROBE_TEMP_COMPENSATION) - -#include "../gcode.h" -#include "../../module/motion.h" -#include "../../module/planner.h" -#include "../../module/probe.h" -#include "../../feature/bedlevel/bedlevel.h" -#include "../../module/temperature.h" -#include "../../module/probe.h" -#include "../../feature/probe_temp_comp.h" - -#include "../../lcd/marlinui.h" -#include "../../MarlinCore.h" // for wait_for_heatup, idle() - -#if ENABLED(PRINTJOB_TIMER_AUTOSTART) - #include "../../module/printcounter.h" -#endif - -#if ENABLED(PRINTER_EVENTS_LEDS) - #include "../../feature/leds/leds.h" -#endif - -/** - * G76: calibrate probe and/or bed temperature offsets - * Notes: - * - When calibrating probe, bed temperature is held constant. - * Compensation values are deltas to first probe measurement at probe temp. = 30°C. - * - When calibrating bed, probe temperature is held constant. - * Compensation values are deltas to first probe measurement at bed temp. = 60°C. - * - The hotend will not be heated at any time. - * - On my Průša MK3S clone I put a piece of paper between the probe and the hotend - * so the hotend fan would not cool my probe constantly. Alternativly you could just - * make sure the fan is not running while running the calibration process. - * - * Probe calibration: - * - Moves probe to cooldown point. - * - Heats up bed to 100°C. - * - Moves probe to probing point (1mm above heatbed). - * - Waits until probe reaches target temperature (30°C). - * - Does a z-probing (=base value) and increases target temperature by 5°C. - * - Waits until probe reaches increased target temperature. - * - Does a z-probing (delta to base value will be a compensation value) and increases target temperature by 5°C. - * - Repeats last two steps until max. temperature reached or timeout (i.e. probe does not heat up any further). - * - Compensation values of higher temperatures will be extrapolated (using linear regression first). - * While this is not exact by any means it is still better than simply using the last compensation value. - * - * Bed calibration: - * - Moves probe to cooldown point. - * - Heats up bed to 60°C. - * - Moves probe to probing point (1mm above heatbed). - * - Waits until probe reaches target temperature (30°C). - * - Does a z-probing (=base value) and increases bed temperature by 5°C. - * - Moves probe to cooldown point. - * - Waits until probe is below 30°C and bed has reached target temperature. - * - Moves probe to probing point and waits until it reaches target temperature (30°C). - * - Does a z-probing (delta to base value will be a compensation value) and increases bed temperature by 5°C. - * - Repeats last four points until max. bed temperature reached (110°C) or timeout. - * - Compensation values of higher temperatures will be extrapolated (using linear regression first). - * While this is not exact by any means it is still better than simply using the last compensation value. - * - * G76 [B | P] - * - no flag - Both calibration procedures will be run. - * - `B` - Run bed temperature calibration. - * - `P` - Run probe temperature calibration. - */ - -static void say_waiting_for() { SERIAL_ECHOPGM("Waiting for "); } -static void say_waiting_for_probe_heating() { say_waiting_for(); SERIAL_ECHOLNPGM("probe heating."); } -static void say_successfully_calibrated() { SERIAL_ECHOPGM("Successfully calibrated"); } -static void say_failed_to_calibrate() { SERIAL_ECHOPGM("!Failed to calibrate"); } - -void GcodeSuite::G76() { - // Check if heated bed is available and z-homing is done with probe - #if TEMP_SENSOR_BED == 0 || !(HOMING_Z_WITH_PROBE) - return; - #endif - - auto report_temps = [](millis_t &ntr, millis_t timeout=0) { - idle_no_sleep(); - const millis_t ms = millis(); - if (ELAPSED(ms, ntr)) { - ntr = ms + 1000; - thermalManager.print_heater_states(active_extruder); - } - return (timeout && ELAPSED(ms, timeout)); - }; - - auto wait_for_temps = [&](const float tb, const float tp, millis_t &ntr, const millis_t timeout=0) { - say_waiting_for(); SERIAL_ECHOLNPGM("bed and probe temperature."); - while (fabs(thermalManager.degBed() - tb) > 0.1f || thermalManager.degProbe() > tp) - if (report_temps(ntr, timeout)) return true; - return false; - }; - - auto g76_probe = [](const TempSensorID sid, uint16_t &targ, const xy_pos_t &nozpos) { - do_z_clearance(5.0); // Raise nozzle before probing - const float measured_z = probe.probe_at_point(nozpos, PROBE_PT_STOW, 0, false); // verbose=0, probe_relative=false - if (isnan(measured_z)) - SERIAL_ECHOLNPGM("!Received NAN. Aborting."); - else { - SERIAL_ECHOLNPAIR_F("Measured: ", measured_z); - if (targ == cali_info_init[sid].start_temp) - temp_comp.prepare_new_calibration(measured_z); - else - temp_comp.push_back_new_measurement(sid, measured_z); - targ += cali_info_init[sid].temp_res; - } - return measured_z; - }; - - #if ENABLED(BLTOUCH) - // Make sure any BLTouch error condition is cleared - bltouch_command(BLTOUCH_RESET, BLTOUCH_RESET_DELAY); - set_bltouch_deployed(false); - #endif - - bool do_bed_cal = parser.boolval('B'), do_probe_cal = parser.boolval('P'); - if (!do_bed_cal && !do_probe_cal) do_bed_cal = do_probe_cal = true; - - // Synchronize with planner - planner.synchronize(); - - const xyz_pos_t parkpos = temp_comp.park_point, - probe_pos_xyz = xyz_pos_t(temp_comp.measure_point) + xyz_pos_t({ 0.0f, 0.0f, PTC_PROBE_HEATING_OFFSET }), - noz_pos_xyz = probe_pos_xyz - probe.offset_xy; // Nozzle position based on probe position - - if (do_bed_cal || do_probe_cal) { - // Ensure park position is reachable - bool reachable = position_is_reachable(parkpos) || WITHIN(parkpos.z, Z_MIN_POS - fslop, Z_MAX_POS + fslop); - if (!reachable) - SERIAL_ECHOLNPGM("!Park"); - else { - // Ensure probe position is reachable - reachable = probe.can_reach(probe_pos_xyz); - if (!reachable) SERIAL_ECHOLNPGM("!Probe"); - } - - if (!reachable) { - SERIAL_ECHOLNPGM(" position unreachable - aborting."); - return; - } - - process_subcommands_now_P(G28_STR); - } - - remember_feedrate_scaling_off(); - - - /****************************************** - * Calibrate bed temperature offsets - ******************************************/ - - // Report temperatures every second and handle heating timeouts - millis_t next_temp_report = millis() + 1000; - - auto report_targets = [&](const uint16_t tb, const uint16_t tp) { - SERIAL_ECHOLNPAIR("Target Bed:", tb, " Probe:", tp); - }; - - if (do_bed_cal) { - - uint16_t target_bed = cali_info_init[TSI_BED].start_temp, - target_probe = temp_comp.bed_calib_probe_temp; - - say_waiting_for(); SERIAL_ECHOLNPGM(" cooling."); - while (thermalManager.degBed() > target_bed || thermalManager.degProbe() > target_probe) - report_temps(next_temp_report); - - // Disable leveling so it won't mess with us - TERN_(HAS_LEVELING, set_bed_leveling_enabled(false)); - - for (;;) { - thermalManager.setTargetBed(target_bed); - - report_targets(target_bed, target_probe); - - // Park nozzle - do_blocking_move_to(parkpos); - - // Wait for heatbed to reach target temp and probe to cool below target temp - if (wait_for_temps(target_bed, target_probe, next_temp_report, millis() + MIN_TO_MS(15))) { - SERIAL_ECHOLNPGM("!Bed heating timeout."); - break; - } - - // Move the nozzle to the probing point and wait for the probe to reach target temp - do_blocking_move_to(noz_pos_xyz); - say_waiting_for_probe_heating(); - SERIAL_EOL(); - while (thermalManager.degProbe() < target_probe) - report_temps(next_temp_report); - - const float measured_z = g76_probe(TSI_BED, target_bed, noz_pos_xyz); - if (isnan(measured_z) || target_bed > BED_MAX_TARGET) break; - } - - SERIAL_ECHOLNPAIR("Retrieved measurements: ", temp_comp.get_index()); - if (temp_comp.finish_calibration(TSI_BED)) { - say_successfully_calibrated(); - SERIAL_ECHOLNPGM(" bed."); - } - else { - say_failed_to_calibrate(); - SERIAL_ECHOLNPGM(" bed. Values reset."); - } - - // Cleanup - thermalManager.setTargetBed(0); - TERN_(HAS_LEVELING, set_bed_leveling_enabled(true)); - } // do_bed_cal - - /******************************************** - * Calibrate probe temperature offsets - ********************************************/ - - if (do_probe_cal) { - - // Park nozzle - do_blocking_move_to(parkpos); - - // Initialize temperatures - const uint16_t target_bed = temp_comp.probe_calib_bed_temp; - thermalManager.setTargetBed(target_bed); - - uint16_t target_probe = cali_info_init[TSI_PROBE].start_temp; - - report_targets(target_bed, target_probe); - - // Wait for heatbed to reach target temp and probe to cool below target temp - wait_for_temps(target_bed, target_probe, next_temp_report); - - // Disable leveling so it won't mess with us - TERN_(HAS_LEVELING, set_bed_leveling_enabled(false)); - - bool timeout = false; - for (;;) { - // Move probe to probing point and wait for it to reach target temperature - do_blocking_move_to(noz_pos_xyz); - - say_waiting_for_probe_heating(); - SERIAL_ECHOLNPAIR(" Bed:", target_bed, " Probe:", target_probe); - const millis_t probe_timeout_ms = millis() + SEC_TO_MS(900UL); - while (thermalManager.degProbe() < target_probe) { - if (report_temps(next_temp_report, probe_timeout_ms)) { - SERIAL_ECHOLNPGM("!Probe heating timed out."); - timeout = true; - break; - } - } - if (timeout) break; - - const float measured_z = g76_probe(TSI_PROBE, target_probe, noz_pos_xyz); - if (isnan(measured_z) || target_probe > cali_info_init[TSI_PROBE].end_temp) break; - } - - SERIAL_ECHOLNPAIR("Retrieved measurements: ", temp_comp.get_index()); - if (temp_comp.finish_calibration(TSI_PROBE)) - say_successfully_calibrated(); - else - say_failed_to_calibrate(); - SERIAL_ECHOLNPGM(" probe."); - - // Cleanup - thermalManager.setTargetBed(0); - TERN_(HAS_LEVELING, set_bed_leveling_enabled(true)); - - SERIAL_ECHOLNPGM("Final compensation values:"); - temp_comp.print_offsets(); - } // do_probe_cal - - restore_feedrate_and_scaling(); -} - -/** - * M871: Report / reset temperature compensation offsets. - * Note: This does not affect values in EEPROM until M500. - * - * M871 [ R | B | P | E ] - * - * No Parameters - Print current offset values. - * - * Select only one of these flags: - * R - Reset all offsets to zero (i.e., disable compensation). - * B - Manually set offset for bed - * P - Manually set offset for probe - * E - Manually set offset for extruder - * - * With B, P, or E: - * I[index] - Index in the array - * V[value] - Adjustment in µm - */ -void GcodeSuite::M871() { - - if (parser.seen('R')) { - // Reset z-probe offsets to factory defaults - temp_comp.clear_all_offsets(); - SERIAL_ECHOLNPGM("Offsets reset to default."); - } - else if (parser.seen("BPE")) { - if (!parser.seenval('V')) return; - const int16_t offset_val = parser.value_int(); - if (!parser.seenval('I')) return; - const int16_t idx = parser.value_int(); - const TempSensorID mod = (parser.seen('B') ? TSI_BED : - #if ENABLED(USE_TEMP_EXT_COMPENSATION) - parser.seen('E') ? TSI_EXT : - #endif - TSI_PROBE - ); - if (idx > 0 && temp_comp.set_offset(mod, idx - 1, offset_val)) - SERIAL_ECHOLNPAIR("Set value: ", offset_val); - else - SERIAL_ECHOLNPGM("!Invalid index. Failed to set value (note: value at index 0 is constant)."); - - } - else // Print current Z-probe adjustments. Note: Values in EEPROM might differ. - temp_comp.print_offsets(); -} - -/** - * M192: Wait for probe temperature sensor to reach a target - * - * Select only one of these flags: - * R - Wait for heating or cooling - * S - Wait only for heating - */ -void GcodeSuite::M192() { - if (DEBUGGING(DRYRUN)) return; - - const bool no_wait_for_cooling = parser.seenval('S'); - if (!no_wait_for_cooling && ! parser.seenval('R')) { - SERIAL_ERROR_MSG("No target temperature set."); - return; - } - - const float target_temp = parser.value_celsius(); - ui.set_status_P(thermalManager.isProbeBelowTemp(target_temp) ? GET_TEXT(MSG_PROBE_HEATING) : GET_TEXT(MSG_PROBE_COOLING)); - thermalManager.wait_for_probe(target_temp, no_wait_for_cooling); -} - -#endif // PROBE_TEMP_COMPENSATION diff --git a/Marlin/src/gcode/calibrate/G76_M871.cpp b/Marlin/src/gcode/calibrate/G76_M871.cpp new file mode 100644 index 000000000000..21bb2c759006 --- /dev/null +++ b/Marlin/src/gcode/calibrate/G76_M871.cpp @@ -0,0 +1,337 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + * + */ + +/** + * G76_M871.cpp - Temperature calibration/compensation for z-probing + */ + +#include "../../inc/MarlinConfig.h" + +#if HAS_PTC + +#include "../gcode.h" +#include "../../module/motion.h" +#include "../../module/planner.h" +#include "../../module/probe.h" +#include "../../feature/bedlevel/bedlevel.h" +#include "../../module/temperature.h" +#include "../../module/probe.h" +#include "../../feature/probe_temp_comp.h" +#include "../../lcd/marlinui.h" + +/** + * G76: calibrate probe and/or bed temperature offsets + * Notes: + * - When calibrating probe, bed temperature is held constant. + * Compensation values are deltas to first probe measurement at probe temp. = 30°C. + * - When calibrating bed, probe temperature is held constant. + * Compensation values are deltas to first probe measurement at bed temp. = 60°C. + * - The hotend will not be heated at any time. + * - On my Průša MK3S clone I put a piece of paper between the probe and the hotend + * so the hotend fan would not cool my probe constantly. Alternatively you could just + * make sure the fan is not running while running the calibration process. + * + * Probe calibration: + * - Moves probe to cooldown point. + * - Heats up bed to 100°C. + * - Moves probe to probing point (1mm above heatbed). + * - Waits until probe reaches target temperature (30°C). + * - Does a z-probing (=base value) and increases target temperature by 5°C. + * - Waits until probe reaches increased target temperature. + * - Does a z-probing (delta to base value will be a compensation value) and increases target temperature by 5°C. + * - Repeats last two steps until max. temperature reached or timeout (i.e. probe does not heat up any further). + * - Compensation values of higher temperatures will be extrapolated (using linear regression first). + * While this is not exact by any means it is still better than simply using the last compensation value. + * + * Bed calibration: + * - Moves probe to cooldown point. + * - Heats up bed to 60°C. + * - Moves probe to probing point (1mm above heatbed). + * - Waits until probe reaches target temperature (30°C). + * - Does a z-probing (=base value) and increases bed temperature by 5°C. + * - Moves probe to cooldown point. + * - Waits until probe is below 30°C and bed has reached target temperature. + * - Moves probe to probing point and waits until it reaches target temperature (30°C). + * - Does a z-probing (delta to base value will be a compensation value) and increases bed temperature by 5°C. + * - Repeats last four points until max. bed temperature reached (110°C) or timeout. + * - Compensation values of higher temperatures will be extrapolated (using linear regression first). + * While this is not exact by any means it is still better than simply using the last compensation value. + * + * G76 [B | P] + * - no flag - Both calibration procedures will be run. + * - `B` - Run bed temperature calibration. + * - `P` - Run probe temperature calibration. + */ + +static void say_waiting_for() { SERIAL_ECHOPGM("Waiting for "); } +static void say_waiting_for_probe_heating() { say_waiting_for(); SERIAL_ECHOLNPGM("probe heating."); } +static void say_successfully_calibrated() { SERIAL_ECHOPGM("Successfully calibrated"); } +static void say_failed_to_calibrate() { SERIAL_ECHOPGM("!Failed to calibrate"); } + +#if BOTH(PTC_PROBE, PTC_BED) + + void GcodeSuite::G76() { + auto report_temps = [](millis_t &ntr, millis_t timeout=0) { + idle_no_sleep(); + const millis_t ms = millis(); + if (ELAPSED(ms, ntr)) { + ntr = ms + 1000; + thermalManager.print_heater_states(active_extruder); + } + return (timeout && ELAPSED(ms, timeout)); + }; + + auto wait_for_temps = [&](const celsius_t tb, const celsius_t tp, millis_t &ntr, const millis_t timeout=0) { + say_waiting_for(); SERIAL_ECHOLNPGM("bed and probe temperature."); + while (thermalManager.wholeDegBed() != tb || thermalManager.wholeDegProbe() > tp) + if (report_temps(ntr, timeout)) return true; + return false; + }; + + auto g76_probe = [](const TempSensorID sid, celsius_t &targ, const xy_pos_t &nozpos) { + do_z_clearance(5.0); // Raise nozzle before probing + const float measured_z = probe.probe_at_point(nozpos, PROBE_PT_STOW, 0, false); // verbose=0, probe_relative=false + if (isnan(measured_z)) + SERIAL_ECHOLNPGM("!Received NAN. Aborting."); + else { + SERIAL_ECHOLNPAIR_F("Measured: ", measured_z); + if (targ == ProbeTempComp::cali_info[sid].start_temp) + ptc.prepare_new_calibration(measured_z); + else + ptc.push_back_new_measurement(sid, measured_z); + targ += ProbeTempComp::cali_info[sid].temp_resolution; + } + return measured_z; + }; + + #if ENABLED(BLTOUCH) + // Make sure any BLTouch error condition is cleared + bltouch_command(BLTOUCH_RESET, BLTOUCH_RESET_DELAY); + set_bltouch_deployed(false); + #endif + + bool do_bed_cal = parser.boolval('B'), do_probe_cal = parser.boolval('P'); + if (!do_bed_cal && !do_probe_cal) do_bed_cal = do_probe_cal = true; + + // Synchronize with planner + planner.synchronize(); + + #ifndef PTC_PROBE_HEATING_OFFSET + #define PTC_PROBE_HEATING_OFFSET 0 + #endif + const xyz_pos_t parkpos = PTC_PARK_POS, + probe_pos_xyz = xyz_pos_t(PTC_PROBE_POS) + xyz_pos_t({ 0.0f, 0.0f, PTC_PROBE_HEATING_OFFSET }), + noz_pos_xyz = probe_pos_xyz - probe.offset_xy; // Nozzle position based on probe position + + if (do_bed_cal || do_probe_cal) { + // Ensure park position is reachable + bool reachable = position_is_reachable(parkpos) || WITHIN(parkpos.z, Z_MIN_POS - fslop, Z_MAX_POS + fslop); + if (!reachable) + SERIAL_ECHOLNPGM("!Park"); + else { + // Ensure probe position is reachable + reachable = probe.can_reach(probe_pos_xyz); + if (!reachable) SERIAL_ECHOLNPGM("!Probe"); + } + + if (!reachable) { + SERIAL_ECHOLNPGM(" position unreachable - aborting."); + return; + } + + process_subcommands_now(FPSTR(G28_STR)); + } + + remember_feedrate_scaling_off(); + + /****************************************** + * Calibrate bed temperature offsets + ******************************************/ + + // Report temperatures every second and handle heating timeouts + millis_t next_temp_report = millis() + 1000; + + auto report_targets = [&](const celsius_t tb, const celsius_t tp) { + SERIAL_ECHOLNPGM("Target Bed:", tb, " Probe:", tp); + }; + + if (do_bed_cal) { + + celsius_t target_bed = PTC_BED_START, + target_probe = PTC_PROBE_TEMP; + + say_waiting_for(); SERIAL_ECHOLNPGM(" cooling."); + while (thermalManager.wholeDegBed() > target_bed || thermalManager.wholeDegProbe() > target_probe) + report_temps(next_temp_report); + + // Disable leveling so it won't mess with us + TERN_(HAS_LEVELING, set_bed_leveling_enabled(false)); + + for (uint8_t idx = 0; idx <= PTC_BED_COUNT; idx++) { + thermalManager.setTargetBed(target_bed); + + report_targets(target_bed, target_probe); + + // Park nozzle + do_blocking_move_to(parkpos); + + // Wait for heatbed to reach target temp and probe to cool below target temp + if (wait_for_temps(target_bed, target_probe, next_temp_report, millis() + MIN_TO_MS(15))) { + SERIAL_ECHOLNPGM("!Bed heating timeout."); + break; + } + + // Move the nozzle to the probing point and wait for the probe to reach target temp + do_blocking_move_to(noz_pos_xyz); + say_waiting_for_probe_heating(); + SERIAL_EOL(); + while (thermalManager.wholeDegProbe() < target_probe) + report_temps(next_temp_report); + + const float measured_z = g76_probe(TSI_BED, target_bed, noz_pos_xyz); + if (isnan(measured_z) || target_bed > (BED_MAX_TARGET)) break; + } + + SERIAL_ECHOLNPGM("Retrieved measurements: ", ptc.get_index()); + if (ptc.finish_calibration(TSI_BED)) { + say_successfully_calibrated(); + SERIAL_ECHOLNPGM(" bed."); + } + else { + say_failed_to_calibrate(); + SERIAL_ECHOLNPGM(" bed. Values reset."); + } + + // Cleanup + thermalManager.setTargetBed(0); + TERN_(HAS_LEVELING, set_bed_leveling_enabled(true)); + } // do_bed_cal + + /******************************************** + * Calibrate probe temperature offsets + ********************************************/ + + if (do_probe_cal) { + + // Park nozzle + do_blocking_move_to(parkpos); + + // Initialize temperatures + const celsius_t target_bed = BED_MAX_TARGET; + thermalManager.setTargetBed(target_bed); + + celsius_t target_probe = PTC_PROBE_START; + + report_targets(target_bed, target_probe); + + // Wait for heatbed to reach target temp and probe to cool below target temp + wait_for_temps(target_bed, target_probe, next_temp_report); + + // Disable leveling so it won't mess with us + TERN_(HAS_LEVELING, set_bed_leveling_enabled(false)); + + bool timeout = false; + for (uint8_t idx = 0; idx <= PTC_PROBE_COUNT; idx++) { + // Move probe to probing point and wait for it to reach target temperature + do_blocking_move_to(noz_pos_xyz); + + say_waiting_for_probe_heating(); + SERIAL_ECHOLNPGM(" Bed:", target_bed, " Probe:", target_probe); + const millis_t probe_timeout_ms = millis() + SEC_TO_MS(900UL); + while (thermalManager.degProbe() < target_probe) { + if (report_temps(next_temp_report, probe_timeout_ms)) { + SERIAL_ECHOLNPGM("!Probe heating timed out."); + timeout = true; + break; + } + } + if (timeout) break; + + const float measured_z = g76_probe(TSI_PROBE, target_probe, noz_pos_xyz); + if (isnan(measured_z)) break; + } + + SERIAL_ECHOLNPGM("Retrieved measurements: ", ptc.get_index()); + if (ptc.finish_calibration(TSI_PROBE)) + say_successfully_calibrated(); + else + say_failed_to_calibrate(); + SERIAL_ECHOLNPGM(" probe."); + + // Cleanup + thermalManager.setTargetBed(0); + TERN_(HAS_LEVELING, set_bed_leveling_enabled(true)); + + SERIAL_ECHOLNPGM("Final compensation values:"); + ptc.print_offsets(); + } // do_probe_cal + + restore_feedrate_and_scaling(); + } + +#endif // PTC_PROBE && PTC_BED + +/** + * M871: Report / reset temperature compensation offsets. + * Note: This does not affect values in EEPROM until M500. + * + * M871 [ R | B | P | E ] + * + * No Parameters - Print current offset values. + * + * Select only one of these flags: + * R - Reset all offsets to zero (i.e., disable compensation). + * B - Manually set offset for bed + * P - Manually set offset for probe + * E - Manually set offset for extruder + * + * With B, P, or E: + * I[index] - Index in the array + * V[value] - Adjustment in µm + */ +void GcodeSuite::M871() { + + if (parser.seen('R')) { + // Reset z-probe offsets to factory defaults + ptc.clear_all_offsets(); + SERIAL_ECHOLNPGM("Offsets reset to default."); + } + else if (parser.seen("BPE")) { + if (!parser.seenval('V')) return; + const int16_t offset_val = parser.value_int(); + if (!parser.seenval('I')) return; + const int16_t idx = parser.value_int(); + const TempSensorID mod = TERN_(PTC_BED, parser.seen_test('B') ? TSI_BED :) + TERN_(PTC_HOTEND, parser.seen_test('E') ? TSI_EXT :) + TERN_(PTC_PROBE, parser.seen_test('P') ? TSI_PROBE :) TSI_COUNT; + if (mod == TSI_COUNT) + SERIAL_ECHOLNPGM("!Invalid sensor."); + else if (idx > 0 && ptc.set_offset(mod, idx - 1, offset_val)) + SERIAL_ECHOLNPGM("Set value: ", offset_val); + else + SERIAL_ECHOLNPGM("!Invalid index. Failed to set value (note: value at index 0 is constant)."); + } + else // Print current Z-probe adjustments. Note: Values in EEPROM might differ. + ptc.print_offsets(); +} + +#endif // HAS_PTC diff --git a/Marlin/src/gcode/calibrate/M100.cpp b/Marlin/src/gcode/calibrate/M100.cpp index ee572e033d75..338392b59746 100644 --- a/Marlin/src/gcode/calibrate/M100.cpp +++ b/Marlin/src/gcode/calibrate/M100.cpp @@ -51,7 +51,7 @@ * Also, there are two support functions that can be called from a developer's C code. * * uint16_t check_for_free_memory_corruption(PGM_P const free_memory_start); - * void M100_dump_routine(PGM_P const title, const char * const start, const uintptr_t size); + * void M100_dump_routine(FSTR_P const title, const char * const start, const uintptr_t size); * * Initial version by Roxy-3D */ @@ -182,8 +182,8 @@ inline int32_t count_test_bytes(const char * const start_free_memory) { } } - void M100_dump_routine(PGM_P const title, const char * const start, const uintptr_t size) { - SERIAL_ECHOLNPGM_P(title); + void M100_dump_routine(FSTR_P const title, const char * const start, const uintptr_t size) { + SERIAL_ECHOLNF(title); // // Round the start and end locations to produce full lines of output // @@ -196,13 +196,13 @@ inline int32_t count_test_bytes(const char * const start_free_memory) { #endif // M100_FREE_MEMORY_DUMPER -inline int check_for_free_memory_corruption(PGM_P const title) { - SERIAL_ECHOPGM_P(title); +inline int check_for_free_memory_corruption(FSTR_P const title) { + SERIAL_ECHOF(title); char *start_free_memory = free_memory_start, *end_free_memory = free_memory_end; int n = end_free_memory - start_free_memory; - SERIAL_ECHOLNPAIR("\nfmc() n=", n, + SERIAL_ECHOLNPGM("\nfmc() n=", n, "\nfree_memory_start=", hex_address(free_memory_start), " end=", hex_address(end_free_memory)); @@ -217,7 +217,7 @@ inline int check_for_free_memory_corruption(PGM_P const title) { // idle(); serial_delay(20); #if ENABLED(M100_FREE_MEMORY_DUMPER) - M100_dump_routine(PSTR(" Memory corruption detected with end_free_memory 8) { - //SERIAL_ECHOPAIR("Found ", j); - //SERIAL_ECHOLNPAIR(" bytes free at ", hex_address(start_free_memory + i)); + //SERIAL_ECHOPGM("Found ", j); + //SERIAL_ECHOLNPGM(" bytes free at ", hex_address(start_free_memory + i)); i += j; block_cnt++; - SERIAL_ECHOLNPAIR(" (", block_cnt, ") found=", j); + SERIAL_ECHOLNPGM(" (", block_cnt, ") found=", j); } } } - SERIAL_ECHOPAIR(" block_found=", block_cnt); + SERIAL_ECHOPGM(" block_found=", block_cnt); if (block_cnt != 1) SERIAL_ECHOLNPGM("\nMemory Corruption detected in free memory area."); @@ -267,7 +267,7 @@ inline void free_memory_pool_report(char * const start_free_memory, const int32_ if (*addr == TEST_BYTE) { const int32_t j = count_test_bytes(addr); if (j > 8) { - SERIAL_ECHOLNPAIR("Found ", j, " bytes free at ", hex_address(addr)); + SERIAL_ECHOLNPGM("Found ", j, " bytes free at ", hex_address(addr)); if (j > max_cnt) { max_cnt = j; max_addr = addr; @@ -277,11 +277,11 @@ inline void free_memory_pool_report(char * const start_free_memory, const int32_ } } } - if (block_cnt > 1) SERIAL_ECHOLNPAIR( + if (block_cnt > 1) SERIAL_ECHOLNPGM( "\nMemory Corruption detected in free memory area." "\nLargest free block is ", max_cnt, " bytes at ", hex_address(max_addr) ); - SERIAL_ECHOLNPAIR("check_for_free_memory_corruption() = ", check_for_free_memory_corruption(PSTR("M100 F "))); + SERIAL_ECHOLNPGM("check_for_free_memory_corruption() = ", check_for_free_memory_corruption(F("M100 F "))); } #if ENABLED(M100_FREE_MEMORY_CORRUPTOR) @@ -299,7 +299,7 @@ inline void free_memory_pool_report(char * const start_free_memory, const int32_ for (uint32_t i = 1; i <= size; i++) { char * const addr = start_free_memory + i * j; *addr = i; - SERIAL_ECHOPAIR("\nCorrupting address: ", hex_address(addr)); + SERIAL_ECHOPGM("\nCorrupting address: ", hex_address(addr)); } SERIAL_EOL(); } @@ -327,8 +327,8 @@ inline void init_free_memory(char *start_free_memory, int32_t size) { for (int32_t i = 0; i < size; i++) { if (start_free_memory[i] != TEST_BYTE) { - SERIAL_ECHOPAIR("? address : ", hex_address(start_free_memory + i)); - SERIAL_ECHOLNPAIR("=", hex_byte(start_free_memory[i])); + SERIAL_ECHOPGM("? address : ", hex_address(start_free_memory + i)); + SERIAL_ECHOLNPGM("=", hex_byte(start_free_memory[i])); SERIAL_EOL(); } } @@ -340,14 +340,14 @@ inline void init_free_memory(char *start_free_memory, int32_t size) { void GcodeSuite::M100() { char *sp = top_of_stack(); if (!free_memory_end) free_memory_end = sp - MEMORY_END_CORRECTION; - SERIAL_ECHOPAIR("\nbss_end : ", hex_address(end_bss)); - if (heaplimit) SERIAL_ECHOPAIR("\n__heaplimit : ", hex_address(heaplimit)); - SERIAL_ECHOPAIR("\nfree_memory_start : ", hex_address(free_memory_start)); - if (stacklimit) SERIAL_ECHOPAIR("\n__stacklimit : ", hex_address(stacklimit)); - SERIAL_ECHOPAIR("\nfree_memory_end : ", hex_address(free_memory_end)); + SERIAL_ECHOPGM("\nbss_end : ", hex_address(end_bss)); + if (heaplimit) SERIAL_ECHOPGM("\n__heaplimit : ", hex_address(heaplimit)); + SERIAL_ECHOPGM("\nfree_memory_start : ", hex_address(free_memory_start)); + if (stacklimit) SERIAL_ECHOPGM("\n__stacklimit : ", hex_address(stacklimit)); + SERIAL_ECHOPGM("\nfree_memory_end : ", hex_address(free_memory_end)); if (MEMORY_END_CORRECTION) - SERIAL_ECHOPAIR("\nMEMORY_END_CORRECTION : ", MEMORY_END_CORRECTION); - SERIAL_ECHOLNPAIR("\nStack Pointer : ", hex_address(sp)); + SERIAL_ECHOPGM("\nMEMORY_END_CORRECTION : ", MEMORY_END_CORRECTION); + SERIAL_ECHOLNPGM("\nStack Pointer : ", hex_address(sp)); // Always init on the first invocation of M100 static bool m100_not_initialized = true; diff --git a/Marlin/src/gcode/calibrate/M12.cpp b/Marlin/src/gcode/calibrate/M12.cpp index da244543754d..191ff22d7dd9 100644 --- a/Marlin/src/gcode/calibrate/M12.cpp +++ b/Marlin/src/gcode/calibrate/M12.cpp @@ -19,6 +19,7 @@ * along with this program. If not, see . * */ + #include "../../inc/MarlinConfigPre.h" #if ENABLED(EXTERNAL_CLOSED_LOOP_CONTROLLER) diff --git a/Marlin/src/gcode/calibrate/M425.cpp b/Marlin/src/gcode/calibrate/M425.cpp index 40441ac08d16..2d36e0d410d5 100644 --- a/Marlin/src/gcode/calibrate/M425.cpp +++ b/Marlin/src/gcode/calibrate/M425.cpp @@ -48,15 +48,20 @@ void GcodeSuite::M425() { auto axis_can_calibrate = [](const uint8_t a) { switch (a) { - default: - case X_AXIS: return AXIS_CAN_CALIBRATE(X); - case Y_AXIS: return AXIS_CAN_CALIBRATE(Y); - case Z_AXIS: return AXIS_CAN_CALIBRATE(Z); + default: return false; + LINEAR_AXIS_CODE( + case X_AXIS: return AXIS_CAN_CALIBRATE(X), + case Y_AXIS: return AXIS_CAN_CALIBRATE(Y), + case Z_AXIS: return AXIS_CAN_CALIBRATE(Z), + case I_AXIS: return AXIS_CAN_CALIBRATE(I), + case J_AXIS: return AXIS_CAN_CALIBRATE(J), + case K_AXIS: return AXIS_CAN_CALIBRATE(K) + ); } }; - LOOP_XYZ(a) { - if (axis_can_calibrate(a) && parser.seen(XYZ_CHAR(a))) { + LOOP_LINEAR_AXES(a) { + if (axis_can_calibrate(a) && parser.seen(AXIS_CHAR(a))) { planner.synchronize(); backlash.distance_mm[a] = parser.has_value() ? parser.value_linear_units() : backlash.get_measurement(AxisEnum(a)); noArgs = false; @@ -81,23 +86,23 @@ void GcodeSuite::M425() { SERIAL_ECHOPGM("Backlash Correction "); if (!backlash.correction) SERIAL_ECHOPGM("in"); SERIAL_ECHOLNPGM("active:"); - SERIAL_ECHOLNPAIR(" Correction Amount/Fade-out: F", backlash.get_correction(), " (F1.0 = full, F0.0 = none)"); + SERIAL_ECHOLNPGM(" Correction Amount/Fade-out: F", backlash.get_correction(), " (F1.0 = full, F0.0 = none)"); SERIAL_ECHOPGM(" Backlash Distance (mm): "); - LOOP_XYZ(a) if (axis_can_calibrate(a)) { - SERIAL_CHAR(' ', XYZ_CHAR(a)); + LOOP_LINEAR_AXES(a) if (axis_can_calibrate(a)) { + SERIAL_CHAR(' ', AXIS_CHAR(a)); SERIAL_ECHO(backlash.distance_mm[a]); SERIAL_EOL(); } #ifdef BACKLASH_SMOOTHING_MM - SERIAL_ECHOLNPAIR(" Smoothing (mm): S", backlash.smoothing_mm); + SERIAL_ECHOLNPGM(" Smoothing (mm): S", backlash.smoothing_mm); #endif #if ENABLED(MEASURE_BACKLASH_WHEN_PROBING) SERIAL_ECHOPGM(" Average measured backlash (mm):"); if (backlash.has_any_measurement()) { - LOOP_XYZ(a) if (axis_can_calibrate(a) && backlash.has_measurement(AxisEnum(a))) { - SERIAL_CHAR(' ', XYZ_CHAR(a)); + LOOP_LINEAR_AXES(a) if (axis_can_calibrate(a) && backlash.has_measurement(AxisEnum(a))) { + SERIAL_CHAR(' ', AXIS_CHAR(a)); SERIAL_ECHO(backlash.get_measurement(AxisEnum(a))); } } @@ -108,4 +113,22 @@ void GcodeSuite::M425() { } } +void GcodeSuite::M425_report(const bool forReplay/*=true*/) { + report_heading_etc(forReplay, F(STR_BACKLASH_COMPENSATION)); + SERIAL_ECHOLNPGM_P( + PSTR(" M425 F"), backlash.get_correction() + #ifdef BACKLASH_SMOOTHING_MM + , PSTR(" S"), LINEAR_UNIT(backlash.smoothing_mm) + #endif + , LIST_N(DOUBLE(LINEAR_AXES), + SP_X_STR, LINEAR_UNIT(backlash.distance_mm.x), + SP_Y_STR, LINEAR_UNIT(backlash.distance_mm.y), + SP_Z_STR, LINEAR_UNIT(backlash.distance_mm.z), + SP_I_STR, LINEAR_UNIT(backlash.distance_mm.i), + SP_J_STR, LINEAR_UNIT(backlash.distance_mm.j), + SP_K_STR, LINEAR_UNIT(backlash.distance_mm.k) + ) + ); +} + #endif // BACKLASH_GCODE diff --git a/Marlin/src/gcode/calibrate/M48.cpp b/Marlin/src/gcode/calibrate/M48.cpp index 0c6176173c3e..913ffe30d478 100644 --- a/Marlin/src/gcode/calibrate/M48.cpp +++ b/Marlin/src/gcode/calibrate/M48.cpp @@ -79,7 +79,7 @@ void GcodeSuite::M48() { }; if (!probe.can_reach(test_position)) { - ui.set_status_P(GET_TEXT(MSG_M48_OUT_OF_BOUNDS), 99); + ui.set_status(GET_TEXT_F(MSG_M48_OUT_OF_BOUNDS), 99); SERIAL_ECHOLNPGM("? (X,Y) out of bounds."); return; } @@ -117,7 +117,7 @@ void GcodeSuite::M48() { max = -99999.9, // Largest value sampled so far sample_set[n_samples]; // Storage for sampled values - auto dev_report = [](const bool verbose, const float &mean, const float &sigma, const float &min, const float &max, const bool final=false) { + auto dev_report = [](const bool verbose, const_float_t mean, const_float_t sigma, const_float_t min, const_float_t max, const bool final=false) { if (verbose) { SERIAL_ECHOPAIR_F("Mean: ", mean, 6); if (!final) SERIAL_ECHOPAIR_F(" Sigma: ", sigma, 6); @@ -142,9 +142,9 @@ void GcodeSuite::M48() { float sample_sum = 0.0; LOOP_L_N(n, n_samples) { - #if HAS_WIRED_LCD + #if HAS_STATUS_MESSAGE // Display M48 progress in the status bar - ui.status_printf_P(0, PSTR(S_FMT ": %d/%d"), GET_TEXT(MSG_M48_POINT), int(n + 1), int(n_samples)); + ui.status_printf(0, F(S_FMT ": %d/%d"), GET_TEXT(MSG_M48_POINT), int(n + 1), int(n_samples)); #endif // When there are "legs" of movement move around the point before probing @@ -162,7 +162,7 @@ void GcodeSuite::M48() { #endif ); if (verbose_level > 3) { - SERIAL_ECHOPAIR("Start radius:", radius, " angle:", angle, " dir:"); + SERIAL_ECHOPGM("Start radius:", radius, " angle:", angle, " dir:"); if (dir > 0) SERIAL_CHAR('C'); SERIAL_ECHOLNPGM("CW"); } @@ -200,7 +200,7 @@ void GcodeSuite::M48() { while (!probe.can_reach(next_pos)) { next_pos *= 0.8f; if (verbose_level > 3) - SERIAL_ECHOLNPAIR_P(PSTR("Moving inward: X"), next_pos.x, SP_Y_STR, next_pos.y); + SERIAL_ECHOLNPGM_P(PSTR("Moving inward: X"), next_pos.x, SP_Y_STR, next_pos.y); } #elif HAS_ENDSTOPS // For a rectangular bed just keep the probe in bounds @@ -209,7 +209,7 @@ void GcodeSuite::M48() { #endif if (verbose_level > 3) - SERIAL_ECHOLNPAIR_P(PSTR("Going to: X"), next_pos.x, SP_Y_STR, next_pos.y); + SERIAL_ECHOLNPGM_P(PSTR("Going to: X"), next_pos.x, SP_Y_STR, next_pos.y); do_blocking_move_to_xy(next_pos); } // n_legs loop @@ -241,7 +241,7 @@ void GcodeSuite::M48() { if (verbose_level > 1) { SERIAL_ECHO(n + 1); - SERIAL_ECHOPAIR(" of ", n_samples); + SERIAL_ECHOPGM(" of ", n_samples); SERIAL_ECHOPAIR_F(": z: ", pz, 3); SERIAL_CHAR(' '); dev_report(verbose_level > 2, mean, sigma, min, max); @@ -257,10 +257,10 @@ void GcodeSuite::M48() { SERIAL_ECHOLNPGM("Finished!"); dev_report(verbose_level > 0, mean, sigma, min, max, true); - #if HAS_WIRED_LCD + #if HAS_STATUS_MESSAGE // Display M48 results in the status bar char sigma_str[8]; - ui.status_printf_P(0, PSTR(S_FMT ": %s"), GET_TEXT(MSG_M48_DEVIATION), dtostrf(sigma, 2, 6, sigma_str)); + ui.status_printf(0, F(S_FMT ": %s"), GET_TEXT(MSG_M48_DEVIATION), dtostrf(sigma, 2, 6, sigma_str)); #endif } diff --git a/Marlin/src/gcode/calibrate/M665.cpp b/Marlin/src/gcode/calibrate/M665.cpp index 0d0c4146d931..aa21471b6027 100644 --- a/Marlin/src/gcode/calibrate/M665.cpp +++ b/Marlin/src/gcode/calibrate/M665.cpp @@ -30,6 +30,7 @@ #if ENABLED(DELTA) #include "../../module/delta.h" + /** * M665: Set delta configurations * @@ -45,6 +46,8 @@ * C = Gamma (Tower 3) diagonal rod trim */ void GcodeSuite::M665() { + if (!parser.seen_any()) return M665_report(); + if (parser.seenval('H')) delta_height = parser.value_linear_units(); if (parser.seenval('L')) delta_diagonal_rod = parser.value_linear_units(); if (parser.seenval('R')) delta_radius = parser.value_linear_units(); @@ -58,6 +61,22 @@ recalc_delta_settings(); } + void GcodeSuite::M665_report(const bool forReplay/*=true*/) { + report_heading_etc(forReplay, F(STR_DELTA_SETTINGS)); + SERIAL_ECHOLNPGM_P( + PSTR(" M665 L"), LINEAR_UNIT(delta_diagonal_rod) + , PSTR(" R"), LINEAR_UNIT(delta_radius) + , PSTR(" H"), LINEAR_UNIT(delta_height) + , PSTR(" S"), segments_per_second + , SP_X_STR, LINEAR_UNIT(delta_tower_angle_trim.a) + , SP_Y_STR, LINEAR_UNIT(delta_tower_angle_trim.b) + , SP_Z_STR, LINEAR_UNIT(delta_tower_angle_trim.c) + , PSTR(" A"), LINEAR_UNIT(delta_diagonal_rod_trim.a) + , PSTR(" B"), LINEAR_UNIT(delta_diagonal_rod_trim.b) + , PSTR(" C"), LINEAR_UNIT(delta_diagonal_rod_trim.c) + ); + } + #elif IS_SCARA #include "../../module/scara.h" @@ -68,6 +87,9 @@ * Parameters: * * S[segments-per-second] - Segments-per-second + * + * Without NO_WORKSPACE_OFFSETS: + * * P[theta-psi-offset] - Theta-Psi offset, added to the shoulder (A/X) angle * T[theta-offset] - Theta offset, added to the elbow (B/Y) angle * Z[z-offset] - Z offset, added to Z @@ -76,6 +98,8 @@ * B, T, and Y are all aliases for the elbow angle */ void GcodeSuite::M665() { + if (!parser.seen_any()) return M665_report(); + if (parser.seenval('S')) segments_per_second = parser.value_float(); #if HAS_SCARA_OFFSET @@ -107,6 +131,41 @@ #endif // HAS_SCARA_OFFSET } + void GcodeSuite::M665_report(const bool forReplay/*=true*/) { + report_heading_etc(forReplay, F(STR_SCARA_SETTINGS " (" STR_S_SEG_PER_SEC TERN_(HAS_SCARA_OFFSET, " " STR_SCARA_P_T_Z) ")")); + SERIAL_ECHOLNPGM_P( + PSTR(" M665 S"), segments_per_second + #if HAS_SCARA_OFFSET + , SP_P_STR, scara_home_offset.a + , SP_T_STR, scara_home_offset.b + , SP_Z_STR, LINEAR_UNIT(scara_home_offset.z) + #endif + ); + } + +#elif ENABLED(POLARGRAPH) + + #include "../../module/polargraph.h" + + /** + * M665: Set POLARGRAPH settings + * + * Parameters: + * + * S[segments-per-second] - Segments-per-second + */ + void GcodeSuite::M665() { + if (parser.seenval('S')) + segments_per_second = parser.value_float(); + else + M665_report(); + } + + void GcodeSuite::M665_report(const bool forReplay/*=true*/) { + report_heading_etc(forReplay, F(STR_POLARGRAPH_SETTINGS " (" STR_S_SEG_PER_SEC ")")); + SERIAL_ECHOLNPGM(" M665 S", segments_per_second); + } + #endif #endif // IS_KINEMATIC diff --git a/Marlin/src/gcode/calibrate/M666.cpp b/Marlin/src/gcode/calibrate/M666.cpp index e915aa8ff7b7..15f8baf109eb 100644 --- a/Marlin/src/gcode/calibrate/M666.cpp +++ b/Marlin/src/gcode/calibrate/M666.cpp @@ -27,30 +27,49 @@ #include "../gcode.h" #if ENABLED(DELTA) - #include "../../module/delta.h" #include "../../module/motion.h" +#else + #include "../../module/endstops.h" +#endif + +#define DEBUG_OUT ENABLED(DEBUG_LEVELING_FEATURE) +#include "../../core/debug_out.h" - #define DEBUG_OUT ENABLED(DEBUG_LEVELING_FEATURE) - #include "../../core/debug_out.h" +#if ENABLED(DELTA) /** * M666: Set delta endstop adjustment */ void GcodeSuite::M666() { DEBUG_SECTION(log_M666, "M666", DEBUGGING(LEVELING)); - LOOP_XYZ(i) { - if (parser.seen(XYZ_CHAR(i))) { + bool is_err = false, is_set = false; + LOOP_LINEAR_AXES(i) { + if (parser.seen(AXIS_CHAR(i))) { + is_set = true; const float v = parser.value_linear_units(); - if (v * Z_HOME_DIR <= 0) delta_endstop_adj[i] = v; - if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPAIR("delta_endstop_adj[", XYZ_CHAR(i), "] = ", delta_endstop_adj[i]); + if (v > 0) + is_err = true; + else { + delta_endstop_adj[i] = v; + if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPGM("delta_endstop_adj[", AS_CHAR(AXIS_CHAR(i)), "] = ", v); + } } } + if (is_err) SERIAL_ECHOLNPGM("?M666 offsets must be <= 0"); + if (!is_set) M666_report(); } -#elif HAS_EXTRA_ENDSTOPS + void GcodeSuite::M666_report(const bool forReplay/*=true*/) { + report_heading_etc(forReplay, F(STR_ENDSTOP_ADJUSTMENT)); + SERIAL_ECHOLNPGM_P( + PSTR(" M666 X"), LINEAR_UNIT(delta_endstop_adj.a) + , SP_Y_STR, LINEAR_UNIT(delta_endstop_adj.b) + , SP_Z_STR, LINEAR_UNIT(delta_endstop_adj.c) + ); + } - #include "../../module/endstops.h" +#else /** * M666: Set Dual Endstops offsets for X, Y, and/or Z. @@ -63,6 +82,8 @@ * Set All: M666 Z */ void GcodeSuite::M666() { + if (!parser.seen_any()) return M666_report(); + #if ENABLED(X_DUAL_ENDSTOPS) if (parser.seenval('X')) endstops.x2_endstop_adj = parser.value_linear_units(); #endif @@ -71,33 +92,40 @@ #endif #if ENABLED(Z_MULTI_ENDSTOPS) if (parser.seenval('Z')) { - #if NUM_Z_STEPPER_DRIVERS >= 3 - const float z_adj = parser.value_linear_units(); - const int ind = parser.intval('S'); - if (!ind || ind == 2) endstops.z2_endstop_adj = z_adj; - if (!ind || ind == 3) endstops.z3_endstop_adj = z_adj; - #if NUM_Z_STEPPER_DRIVERS >= 4 - if (!ind || ind == 4) endstops.z4_endstop_adj = z_adj; - #endif + const float z_adj = parser.value_linear_units(); + #if NUM_Z_STEPPER_DRIVERS == 2 + endstops.z2_endstop_adj = z_adj; #else - endstops.z2_endstop_adj = parser.value_linear_units(); + const int ind = parser.intval('S'); + #define _SET_ZADJ(N) if (!ind || ind == N) endstops.z##N##_endstop_adj = z_adj; + REPEAT_S(2, INCREMENT(NUM_Z_STEPPER_DRIVERS), _SET_ZADJ) #endif } #endif - if (!parser.seen("XYZ")) { - SERIAL_ECHOPGM("Dual Endstop Adjustment (mm): "); - #if ENABLED(X_DUAL_ENDSTOPS) - SERIAL_ECHOPAIR(" X2:", endstops.x2_endstop_adj); - #endif - #if ENABLED(Y_DUAL_ENDSTOPS) - SERIAL_ECHOPAIR(" Y2:", endstops.y2_endstop_adj); - #endif - #if ENABLED(Z_MULTI_ENDSTOPS) - #define _ECHO_ZADJ(N) SERIAL_ECHOPAIR(" Z" STRINGIFY(N) ":", endstops.z##N##_endstop_adj); - REPEAT_S(2, INCREMENT(NUM_Z_STEPPER_DRIVERS), _ECHO_ZADJ) + } + + void GcodeSuite::M666_report(const bool forReplay/*=true*/) { + report_heading_etc(forReplay, F(STR_ENDSTOP_ADJUSTMENT)); + SERIAL_ECHOPGM(" M666"); + #if ENABLED(X_DUAL_ENDSTOPS) + SERIAL_ECHOLNPGM_P(SP_X_STR, LINEAR_UNIT(endstops.x2_endstop_adj)); + #endif + #if ENABLED(Y_DUAL_ENDSTOPS) + SERIAL_ECHOLNPGM_P(SP_Y_STR, LINEAR_UNIT(endstops.y2_endstop_adj)); + #endif + #if ENABLED(Z_MULTI_ENDSTOPS) + #if NUM_Z_STEPPER_DRIVERS >= 3 + SERIAL_ECHOPGM(" S2 Z", LINEAR_UNIT(endstops.z3_endstop_adj)); + report_echo_start(forReplay); + SERIAL_ECHOPGM(" M666 S3 Z", LINEAR_UNIT(endstops.z3_endstop_adj)); + #if NUM_Z_STEPPER_DRIVERS >= 4 + report_echo_start(forReplay); + SERIAL_ECHOPGM(" M666 S4 Z", LINEAR_UNIT(endstops.z4_endstop_adj)); + #endif + #else + SERIAL_ECHOLNPGM_P(SP_Z_STR, LINEAR_UNIT(endstops.z2_endstop_adj)); #endif - SERIAL_EOL(); - } + #endif } #endif // HAS_EXTRA_ENDSTOPS diff --git a/Marlin/src/gcode/calibrate/M852.cpp b/Marlin/src/gcode/calibrate/M852.cpp index e52f03b86c4e..6c661dcd61d9 100644 --- a/Marlin/src/gcode/calibrate/M852.cpp +++ b/Marlin/src/gcode/calibrate/M852.cpp @@ -36,10 +36,11 @@ * K[yz_factor] - New YZ skew factor */ void GcodeSuite::M852() { - uint8_t ijk = 0, badval = 0, setval = 0; + if (!parser.seen("SIJK")) return M852_report(); - if (parser.seen('I') || parser.seen('S')) { - ++ijk; + uint8_t badval = 0, setval = 0; + + if (parser.seenval('I') || parser.seenval('S')) { const float value = parser.value_linear_units(); if (WITHIN(value, SKEW_FACTOR_MIN, SKEW_FACTOR_MAX)) { if (planner.skew_factor.xy != value) { @@ -53,8 +54,7 @@ void GcodeSuite::M852() { #if ENABLED(SKEW_CORRECTION_FOR_Z) - if (parser.seen('J')) { - ++ijk; + if (parser.seenval('J')) { const float value = parser.value_linear_units(); if (WITHIN(value, SKEW_FACTOR_MIN, SKEW_FACTOR_MAX)) { if (planner.skew_factor.xz != value) { @@ -66,8 +66,7 @@ void GcodeSuite::M852() { ++badval; } - if (parser.seen('K')) { - ++ijk; + if (parser.seenval('K')) { const float value = parser.value_linear_units(); if (WITHIN(value, SKEW_FACTOR_MIN, SKEW_FACTOR_MAX)) { if (planner.skew_factor.yz != value) { @@ -86,21 +85,22 @@ void GcodeSuite::M852() { // When skew is changed the current position changes if (setval) { - set_current_from_steppers_for_axis(ALL_AXES); + set_current_from_steppers_for_axis(ALL_AXES_ENUM); sync_plan_position(); report_current_position(); } +} - if (!ijk) { - SERIAL_ECHO_START(); - SERIAL_ECHOPGM_P(GET_TEXT(MSG_SKEW_FACTOR)); - SERIAL_ECHOPAIR_F(" XY: ", planner.skew_factor.xy, 6); - #if ENABLED(SKEW_CORRECTION_FOR_Z) - SERIAL_ECHOPAIR_F(" XZ: ", planner.skew_factor.xz, 6); - SERIAL_ECHOPAIR_F(" YZ: ", planner.skew_factor.yz, 6); - #endif - SERIAL_EOL(); - } +void GcodeSuite::M852_report(const bool forReplay/*=true*/) { + report_heading_etc(forReplay, F(STR_SKEW_FACTOR)); + SERIAL_ECHOPAIR_F(" M852 I", planner.skew_factor.xy, 6); + #if ENABLED(SKEW_CORRECTION_FOR_Z) + SERIAL_ECHOPAIR_F(" J", planner.skew_factor.xz, 6); + SERIAL_ECHOPAIR_F(" K", planner.skew_factor.yz, 6); + SERIAL_ECHOLNPGM(" ; XY, XZ, YZ"); + #else + SERIAL_ECHOLNPGM(" ; XY"); + #endif } #endif // SKEW_CORRECTION_GCODE diff --git a/Marlin/src/gcode/config/M200-M205.cpp b/Marlin/src/gcode/config/M200-M205.cpp index cb17fc45a6e8..9490e3c625f3 100644 --- a/Marlin/src/gcode/config/M200-M205.cpp +++ b/Marlin/src/gcode/config/M200-M205.cpp @@ -32,9 +32,14 @@ * T - Optional extruder number. Current extruder if omitted. * D - Set filament diameter and enable. D0 disables volumetric. * S - Turn volumetric ON or OFF. + * + * With VOLUMETRIC_EXTRUDER_LIMIT: + * * L - Volumetric extruder limit (in mm^3/sec). L0 disables the limit. */ void GcodeSuite::M200() { + if (!parser.seen("DST" TERN_(VOLUMETRIC_EXTRUDER_LIMIT, "L"))) + return M200_report(); const int8_t target_extruder = get_target_extruder_from_command(); if (target_extruder < 0) return; @@ -69,6 +74,37 @@ planner.calculate_volumetric_multipliers(); } + void GcodeSuite::M200_report(const bool forReplay/*=true*/) { + if (!forReplay) { + report_heading(forReplay, F(STR_FILAMENT_SETTINGS), false); + if (!parser.volumetric_enabled) SERIAL_ECHOPGM(" (Disabled):"); + SERIAL_EOL(); + report_echo_start(forReplay); + } + + #if EXTRUDERS == 1 + { + SERIAL_ECHOLNPGM( + " M200 S", parser.volumetric_enabled, " D", LINEAR_UNIT(planner.filament_size[0]) + #if ENABLED(VOLUMETRIC_EXTRUDER_LIMIT) + , " L", LINEAR_UNIT(planner.volumetric_extruder_limit[0]) + #endif + ); + } + #else + SERIAL_ECHOLNPGM(" M200 S", parser.volumetric_enabled); + LOOP_L_N(i, EXTRUDERS) { + report_echo_start(forReplay); + SERIAL_ECHOLNPGM( + " M200 T", i, " D", LINEAR_UNIT(planner.filament_size[i]) + #if ENABLED(VOLUMETRIC_EXTRUDER_LIMIT) + , " L", LINEAR_UNIT(planner.volumetric_extruder_limit[i]) + #endif + ); + } + #endif + } + #endif // !NO_VOLUMETRICS /** @@ -77,6 +113,8 @@ * With multiple extruders use T to specify which one. */ void GcodeSuite::M201() { + if (!parser.seen("T" LOGICAL_AXES_STRING)) + return M201_report(); const int8_t target_extruder = get_target_extruder_from_command(); if (target_extruder < 0) return; @@ -86,31 +124,85 @@ void GcodeSuite::M201() { if (parser.seenval('G')) planner.xy_freq_min_speed_factor = constrain(parser.value_float(), 1, 100) / 100; #endif - LOOP_XYZE(i) { - if (parser.seen(axis_codes[i])) { - const uint8_t a = (i == E_AXIS ? uint8_t(E_AXIS_N(target_extruder)) : i); + LOOP_LOGICAL_AXES(i) { + if (parser.seenval(axis_codes[i])) { + const uint8_t a = TERN(HAS_EXTRUDERS, (i == E_AXIS ? uint8_t(E_AXIS_N(target_extruder)) : i), i); planner.set_max_acceleration(a, parser.value_axis_units((AxisEnum)a)); } } } +void GcodeSuite::M201_report(const bool forReplay/*=true*/) { + report_heading_etc(forReplay, F(STR_MAX_ACCELERATION)); + SERIAL_ECHOLNPGM_P( + LIST_N(DOUBLE(LINEAR_AXES), + PSTR(" M201 X"), LINEAR_UNIT(planner.settings.max_acceleration_mm_per_s2[X_AXIS]), + SP_Y_STR, LINEAR_UNIT(planner.settings.max_acceleration_mm_per_s2[Y_AXIS]), + SP_Z_STR, LINEAR_UNIT(planner.settings.max_acceleration_mm_per_s2[Z_AXIS]), + SP_I_STR, LINEAR_UNIT(planner.settings.max_acceleration_mm_per_s2[I_AXIS]), + SP_J_STR, LINEAR_UNIT(planner.settings.max_acceleration_mm_per_s2[J_AXIS]), + SP_K_STR, LINEAR_UNIT(planner.settings.max_acceleration_mm_per_s2[K_AXIS]) + ) + #if HAS_EXTRUDERS && DISABLED(DISTINCT_E_FACTORS) + , SP_E_STR, VOLUMETRIC_UNIT(planner.settings.max_acceleration_mm_per_s2[E_AXIS]) + #endif + ); + #if ENABLED(DISTINCT_E_FACTORS) + LOOP_L_N(i, E_STEPPERS) { + report_echo_start(forReplay); + SERIAL_ECHOLNPGM_P( + PSTR(" M201 T"), i + , SP_E_STR, VOLUMETRIC_UNIT(planner.settings.max_acceleration_mm_per_s2[E_AXIS_N(i)]) + ); + } + #endif +} + /** * M203: Set maximum feedrate that your machine can sustain (M203 X200 Y200 Z300 E10000) in units/sec * * With multiple extruders use T to specify which one. */ void GcodeSuite::M203() { + if (!parser.seen("T" LOGICAL_AXES_STRING)) + return M203_report(); const int8_t target_extruder = get_target_extruder_from_command(); if (target_extruder < 0) return; - LOOP_XYZE(i) - if (parser.seen(axis_codes[i])) { - const uint8_t a = (i == E_AXIS ? uint8_t(E_AXIS_N(target_extruder)) : i); + LOOP_LOGICAL_AXES(i) + if (parser.seenval(axis_codes[i])) { + const uint8_t a = TERN(HAS_EXTRUDERS, (i == E_AXIS ? uint8_t(E_AXIS_N(target_extruder)) : i), i); planner.set_max_feedrate(a, parser.value_axis_units((AxisEnum)a)); } } +void GcodeSuite::M203_report(const bool forReplay/*=true*/) { + report_heading_etc(forReplay, F(STR_MAX_FEEDRATES)); + SERIAL_ECHOLNPGM_P( + LIST_N(DOUBLE(LINEAR_AXES), + PSTR(" M203 X"), LINEAR_UNIT(planner.settings.max_feedrate_mm_s[X_AXIS]), + SP_Y_STR, LINEAR_UNIT(planner.settings.max_feedrate_mm_s[Y_AXIS]), + SP_Z_STR, LINEAR_UNIT(planner.settings.max_feedrate_mm_s[Z_AXIS]), + SP_I_STR, LINEAR_UNIT(planner.settings.max_feedrate_mm_s[I_AXIS]), + SP_J_STR, LINEAR_UNIT(planner.settings.max_feedrate_mm_s[J_AXIS]), + SP_K_STR, LINEAR_UNIT(planner.settings.max_feedrate_mm_s[K_AXIS]) + ) + #if HAS_EXTRUDERS && DISABLED(DISTINCT_E_FACTORS) + , SP_E_STR, VOLUMETRIC_UNIT(planner.settings.max_feedrate_mm_s[E_AXIS]) + #endif + ); + #if ENABLED(DISTINCT_E_FACTORS) + LOOP_L_N(i, E_STEPPERS) { + SERIAL_ECHO_START(); + SERIAL_ECHOLNPGM_P( + PSTR(" M203 T"), i + , SP_E_STR, VOLUMETRIC_UNIT(planner.settings.max_feedrate_mm_s[E_AXIS_N(i)]) + ); + } + #endif +} + /** * M204: Set Accelerations in units/sec^2 (M204 P1200 R3000 T3000) * @@ -119,11 +211,8 @@ void GcodeSuite::M203() { * T = Travel (non printing) moves */ void GcodeSuite::M204() { - if (!parser.seen("PRST")) { - SERIAL_ECHOPAIR("Acceleration: P", planner.settings.acceleration); - SERIAL_ECHOPAIR(" R", planner.settings.retract_acceleration); - SERIAL_ECHOLNPAIR_P(SP_T_STR, planner.settings.travel_acceleration); - } + if (!parser.seen("PRST")) + return M204_report(); else { //planner.synchronize(); // 'S' for legacy compatibility. Should NOT BE USED for new development @@ -134,6 +223,15 @@ void GcodeSuite::M204() { } } +void GcodeSuite::M204_report(const bool forReplay/*=true*/) { + report_heading_etc(forReplay, F(STR_ACCELERATION_P_R_T)); + SERIAL_ECHOLNPGM_P( + PSTR(" M204 P"), LINEAR_UNIT(planner.settings.acceleration) + , PSTR(" R"), LINEAR_UNIT(planner.settings.retract_acceleration) + , SP_T_STR, LINEAR_UNIT(planner.settings.travel_acceleration) + ); +} + /** * M205: Set Advanced Settings * @@ -147,24 +245,18 @@ void GcodeSuite::M204() { * J = Junction Deviation (mm) (If not using CLASSIC_JERK) */ void GcodeSuite::M205() { - #if HAS_JUNCTION_DEVIATION - #define J_PARAM "J" - #else - #define J_PARAM - #endif - #if HAS_CLASSIC_JERK - #define XYZE_PARAM "XYZE" - #else - #define XYZE_PARAM - #endif - if (!parser.seen("BST" J_PARAM XYZE_PARAM)) return; + if (!parser.seen("BST" TERN_(HAS_JUNCTION_DEVIATION, "J") TERN_(HAS_CLASSIC_JERK, "XYZE"))) + return M205_report(); //planner.synchronize(); - if (parser.seen('B')) planner.settings.min_segment_time_us = parser.value_ulong(); - if (parser.seen('S')) planner.settings.min_feedrate_mm_s = parser.value_linear_units(); - if (parser.seen('T')) planner.settings.min_travel_feedrate_mm_s = parser.value_linear_units(); + if (parser.seenval('B')) planner.settings.min_segment_time_us = parser.value_ulong(); + if (parser.seenval('S')) planner.settings.min_feedrate_mm_s = parser.value_linear_units(); + if (parser.seenval('T')) planner.settings.min_travel_feedrate_mm_s = parser.value_linear_units(); #if HAS_JUNCTION_DEVIATION - if (parser.seen('J')) { + #if HAS_CLASSIC_JERK && AXIS_COLLISION('J') + #error "Can't set_max_jerk for 'J' axis because 'J' is used for Junction Deviation." + #endif + if (parser.seenval('J')) { const float junc_dev = parser.value_linear_units(); if (WITHIN(junc_dev, 0.01f, 0.3f)) { planner.junction_deviation_mm = junc_dev; @@ -175,17 +267,55 @@ void GcodeSuite::M205() { } #endif #if HAS_CLASSIC_JERK - if (parser.seen('X')) planner.set_max_jerk(X_AXIS, parser.value_linear_units()); - if (parser.seen('Y')) planner.set_max_jerk(Y_AXIS, parser.value_linear_units()); - if (parser.seen('Z')) { - planner.set_max_jerk(Z_AXIS, parser.value_linear_units()); - #if HAS_MESH && DISABLED(LIMITED_JERK_EDITING) - if (planner.max_jerk.z <= 0.1f) - SERIAL_ECHOLNPGM("WARNING! Low Z Jerk may lead to unwanted pauses."); + bool seenZ = false; + LOGICAL_AXIS_CODE( + if (parser.seenval('E')) planner.set_max_jerk(E_AXIS, parser.value_linear_units()), + if (parser.seenval('X')) planner.set_max_jerk(X_AXIS, parser.value_linear_units()), + if (parser.seenval('Y')) planner.set_max_jerk(Y_AXIS, parser.value_linear_units()), + if ((seenZ = parser.seenval('Z'))) planner.set_max_jerk(Z_AXIS, parser.value_linear_units()), + if (parser.seenval(AXIS4_NAME)) planner.set_max_jerk(I_AXIS, parser.value_linear_units()), + if (parser.seenval(AXIS5_NAME)) planner.set_max_jerk(J_AXIS, parser.value_linear_units()), + if (parser.seenval(AXIS6_NAME)) planner.set_max_jerk(K_AXIS, parser.value_linear_units()) + ); + #if HAS_MESH && DISABLED(LIMITED_JERK_EDITING) + if (seenZ && planner.max_jerk.z <= 0.1f) + SERIAL_ECHOLNPGM("WARNING! Low Z Jerk may lead to unwanted pauses."); + #endif + #endif // HAS_CLASSIC_JERK +} + +void GcodeSuite::M205_report(const bool forReplay/*=true*/) { + report_heading_etc(forReplay, F( + "Advanced (B S T" + TERN_(HAS_JUNCTION_DEVIATION, " J") + #if HAS_CLASSIC_JERK + LINEAR_AXIS_GANG( + " X", " Y", " Z", + " " STR_I "", " " STR_J "", " " STR_K "" + ) + #endif + TERN_(HAS_CLASSIC_E_JERK, " E") + ")" + )); + SERIAL_ECHOLNPGM_P( + PSTR(" M205 B"), LINEAR_UNIT(planner.settings.min_segment_time_us) + , PSTR(" S"), LINEAR_UNIT(planner.settings.min_feedrate_mm_s) + , SP_T_STR, LINEAR_UNIT(planner.settings.min_travel_feedrate_mm_s) + #if HAS_JUNCTION_DEVIATION + , PSTR(" J"), LINEAR_UNIT(planner.junction_deviation_mm) + #endif + #if HAS_CLASSIC_JERK + , LIST_N(DOUBLE(LINEAR_AXES), + SP_X_STR, LINEAR_UNIT(planner.max_jerk.x), + SP_Y_STR, LINEAR_UNIT(planner.max_jerk.y), + SP_Z_STR, LINEAR_UNIT(planner.max_jerk.z), + SP_I_STR, LINEAR_UNIT(planner.max_jerk.i), + SP_J_STR, LINEAR_UNIT(planner.max_jerk.j), + SP_K_STR, LINEAR_UNIT(planner.max_jerk.k) + ) + #if HAS_CLASSIC_E_JERK + , SP_E_STR, LINEAR_UNIT(planner.max_jerk.e) #endif - } - #if HAS_CLASSIC_E_JERK - if (parser.seen('E')) planner.set_max_jerk(E_AXIS, parser.value_linear_units()); #endif - #endif + ); } diff --git a/Marlin/src/gcode/config/M217.cpp b/Marlin/src/gcode/config/M217.cpp index 2035ae55abeb..344adc34e320 100644 --- a/Marlin/src/gcode/config/M217.cpp +++ b/Marlin/src/gcode/config/M217.cpp @@ -33,44 +33,6 @@ #include "../../MarlinCore.h" // for SP_X_STR, etc. -void M217_report(const bool eeprom=false) { - - #if ENABLED(TOOLCHANGE_FILAMENT_SWAP) - SERIAL_ECHOPGM_P(eeprom ? PSTR(" M217") : PSTR("Toolchange:")); - SERIAL_ECHOPAIR(" S", LINEAR_UNIT(toolchange_settings.swap_length)); - SERIAL_ECHOPAIR_P(SP_B_STR, LINEAR_UNIT(toolchange_settings.extra_resume), - SP_E_STR, LINEAR_UNIT(toolchange_settings.extra_prime), - SP_P_STR, LINEAR_UNIT(toolchange_settings.prime_speed)); - SERIAL_ECHOPAIR(" R", LINEAR_UNIT(toolchange_settings.retract_speed), - " U", LINEAR_UNIT(toolchange_settings.unretract_speed), - " F", toolchange_settings.fan_speed, - " G", toolchange_settings.fan_time); - - #if ENABLED(TOOLCHANGE_MIGRATION_FEATURE) - SERIAL_ECHOPAIR(" A", migration.automode); - SERIAL_ECHOPAIR(" L", LINEAR_UNIT(migration.last)); - #endif - - #if ENABLED(TOOLCHANGE_PARK) - SERIAL_ECHOPAIR(" W", LINEAR_UNIT(toolchange_settings.enable_park)); - SERIAL_ECHOPAIR_P(SP_X_STR, LINEAR_UNIT(toolchange_settings.change_point.x)); - SERIAL_ECHOPAIR_P(SP_Y_STR, LINEAR_UNIT(toolchange_settings.change_point.y)); - #endif - - #if ENABLED(TOOLCHANGE_FS_PRIME_FIRST_USED) - SERIAL_ECHOPAIR(" V", LINEAR_UNIT(enable_first_prime)); - #endif - - #else - - UNUSED(eeprom); - - #endif - - SERIAL_ECHOPAIR_P(SP_Z_STR, LINEAR_UNIT(toolchange_settings.z_raise)); - SERIAL_EOL(); -} - /** * M217 - Set SINGLENOZZLE toolchange parameters * @@ -88,6 +50,9 @@ void M217_report(const bool eeprom=false) { * W[linear] 0/1 Enable park & Z Raise * X[linear] Park X (Requires TOOLCHANGE_PARK) * Y[linear] Park Y (Requires TOOLCHANGE_PARK) + * I[linear] Park I (Requires TOOLCHANGE_PARK and LINEAR_AXES >= 4) + * J[linear] Park J (Requires TOOLCHANGE_PARK and LINEAR_AXES >= 5) + * K[linear] Park K (Requires TOOLCHANGE_PARK and LINEAR_AXES >= 6) * Z[linear] Z Raise * F[linear] Fan Speed 0-255 * G[linear/s] Fan time @@ -126,10 +91,23 @@ void GcodeSuite::M217() { #if ENABLED(TOOLCHANGE_PARK) if (parser.seenval('W')) { toolchange_settings.enable_park = parser.value_linear_units(); } if (parser.seenval('X')) { const int16_t v = parser.value_linear_units(); toolchange_settings.change_point.x = constrain(v, X_MIN_POS, X_MAX_POS); } - if (parser.seenval('Y')) { const int16_t v = parser.value_linear_units(); toolchange_settings.change_point.y = constrain(v, Y_MIN_POS, Y_MAX_POS); } + #if HAS_Y_AXIS + if (parser.seenval('Y')) { const int16_t v = parser.value_linear_units(); toolchange_settings.change_point.y = constrain(v, Y_MIN_POS, Y_MAX_POS); } + #endif + #if HAS_I_AXIS + if (parser.seenval('I')) { const int16_t v = parser.value_linear_units(); toolchange_settings.change_point.i = constrain(v, I_MIN_POS, I_MAX_POS); } + #endif + #if HAS_J_AXIS + if (parser.seenval('J')) { const int16_t v = parser.value_linear_units(); toolchange_settings.change_point.j = constrain(v, J_MIN_POS, J_MAX_POS); } + #endif + #if HAS_K_AXIS + if (parser.seenval('K')) { const int16_t v = parser.value_linear_units(); toolchange_settings.change_point.k = constrain(v, K_MIN_POS, K_MAX_POS); } + #endif #endif - if (parser.seenval('Z')) { toolchange_settings.z_raise = parser.value_linear_units(); } + #if HAS_Z_AXIS + if (parser.seenval('Z')) { toolchange_settings.z_raise = parser.value_linear_units(); } + #endif #if ENABLED(TOOLCHANGE_MIGRATION_FEATURE) migration.target = 0; // 0 = disabled @@ -168,4 +146,54 @@ void GcodeSuite::M217() { M217_report(); } +void GcodeSuite::M217_report(const bool forReplay/*=true*/) { + report_heading_etc(forReplay, F(STR_TOOL_CHANGING)); + + SERIAL_ECHOPGM(" M217"); + + #if ENABLED(TOOLCHANGE_FILAMENT_SWAP) + SERIAL_ECHOPGM(" S", LINEAR_UNIT(toolchange_settings.swap_length)); + SERIAL_ECHOPGM_P(SP_B_STR, LINEAR_UNIT(toolchange_settings.extra_resume), + SP_E_STR, LINEAR_UNIT(toolchange_settings.extra_prime), + SP_P_STR, LINEAR_UNIT(toolchange_settings.prime_speed)); + SERIAL_ECHOPGM(" R", LINEAR_UNIT(toolchange_settings.retract_speed), + " U", LINEAR_UNIT(toolchange_settings.unretract_speed), + " F", toolchange_settings.fan_speed, + " G", toolchange_settings.fan_time); + + #if ENABLED(TOOLCHANGE_MIGRATION_FEATURE) + SERIAL_ECHOPGM(" A", migration.automode); + SERIAL_ECHOPGM(" L", LINEAR_UNIT(migration.last)); + #endif + + #if ENABLED(TOOLCHANGE_PARK) + { + SERIAL_ECHOPGM(" W", LINEAR_UNIT(toolchange_settings.enable_park)); + SERIAL_ECHOPGM_P( + SP_X_STR, LINEAR_UNIT(toolchange_settings.change_point.x) + #if HAS_Y_AXIS + , SP_Y_STR, LINEAR_UNIT(toolchange_settings.change_point.y) + #endif + #if HAS_I_AXIS + , SP_I_STR, LINEAR_UNIT(toolchange_settings.change_point.i) + #endif + #if HAS_J_AXIS + , SP_J_STR, LINEAR_UNIT(toolchange_settings.change_point.j) + #endif + #if HAS_K_AXIS + , SP_K_STR, LINEAR_UNIT(toolchange_settings.change_point.k) + #endif + ); + } + #endif + + #if ENABLED(TOOLCHANGE_FS_PRIME_FIRST_USED) + SERIAL_ECHOPGM(" V", LINEAR_UNIT(enable_first_prime)); + #endif + + #endif + + SERIAL_ECHOLNPGM_P(SP_Z_STR, LINEAR_UNIT(toolchange_settings.z_raise)); +} + #endif // HAS_MULTI_EXTRUDER diff --git a/Marlin/src/gcode/config/M218.cpp b/Marlin/src/gcode/config/M218.cpp index 7701320e9eae..c39447a28d38 100644 --- a/Marlin/src/gcode/config/M218.cpp +++ b/Marlin/src/gcode/config/M218.cpp @@ -41,6 +41,8 @@ */ void GcodeSuite::M218() { + if (!parser.seen_any()) return M218_report(); + const int8_t target_extruder = get_target_extruder_from_command(); if (target_extruder < 0) return; @@ -48,24 +50,23 @@ void GcodeSuite::M218() { if (parser.seenval('Y')) hotend_offset[target_extruder].y = parser.value_linear_units(); if (parser.seenval('Z')) hotend_offset[target_extruder].z = parser.value_linear_units(); - if (!parser.seen("XYZ")) { - SERIAL_ECHO_START(); - SERIAL_ECHOPGM(STR_HOTEND_OFFSET); - HOTEND_LOOP() { - SERIAL_CHAR(' '); - SERIAL_ECHO(hotend_offset[e].x); - SERIAL_CHAR(','); - SERIAL_ECHO(hotend_offset[e].y); - SERIAL_CHAR(','); - SERIAL_ECHO_F(hotend_offset[e].z, 3); - } - SERIAL_EOL(); - } - #if ENABLED(DELTA) if (target_extruder == active_extruder) do_blocking_move_to_xy(current_position, planner.settings.max_feedrate_mm_s[X_AXIS]); #endif } +void GcodeSuite::M218_report(const bool forReplay/*=true*/) { + report_heading_etc(forReplay, F(STR_HOTEND_OFFSETS)); + LOOP_S_L_N(e, 1, HOTENDS) { + report_echo_start(forReplay); + SERIAL_ECHOPGM_P( + PSTR(" M218 T"), e, + SP_X_STR, LINEAR_UNIT(hotend_offset[e].x), + SP_Y_STR, LINEAR_UNIT(hotend_offset[e].y) + ); + SERIAL_ECHOLNPAIR_F_P(SP_Z_STR, LINEAR_UNIT(hotend_offset[e].z), 3); + } +} + #endif // HAS_HOTEND_OFFSET diff --git a/Marlin/src/gcode/config/M220.cpp b/Marlin/src/gcode/config/M220.cpp index 75339f10b9fc..c9070df803b8 100644 --- a/Marlin/src/gcode/config/M220.cpp +++ b/Marlin/src/gcode/config/M220.cpp @@ -44,7 +44,7 @@ void GcodeSuite::M220() { if (parser.seenval('S')) feedrate_percentage = parser.value_int(); if (!parser.seen_any()) { - SERIAL_ECHOPAIR("FR:", feedrate_percentage); + SERIAL_ECHOPGM("FR:", feedrate_percentage); SERIAL_CHAR('%'); SERIAL_EOL(); } diff --git a/Marlin/src/gcode/config/M221.cpp b/Marlin/src/gcode/config/M221.cpp index 7ba22d1901d5..f653aded7c16 100644 --- a/Marlin/src/gcode/config/M221.cpp +++ b/Marlin/src/gcode/config/M221.cpp @@ -23,7 +23,7 @@ #include "../gcode.h" #include "../../module/planner.h" -#if EXTRUDERS +#if HAS_EXTRUDERS /** * M221: Set extrusion percentage (M221 T0 S95) @@ -38,7 +38,7 @@ void GcodeSuite::M221() { else { SERIAL_ECHO_START(); SERIAL_CHAR('E', '0' + target_extruder); - SERIAL_ECHOPAIR(" Flow: ", planner.flow_percentage[target_extruder]); + SERIAL_ECHOPGM(" Flow: ", planner.flow_percentage[target_extruder]); SERIAL_CHAR('%'); SERIAL_EOL(); } diff --git a/Marlin/src/gcode/config/M281.cpp b/Marlin/src/gcode/config/M281.cpp index eeb0fcc470e1..b90de6be30f9 100644 --- a/Marlin/src/gcode/config/M281.cpp +++ b/Marlin/src/gcode/config/M281.cpp @@ -19,6 +19,7 @@ * along with this program. If not, see . * */ + #include "../../inc/MarlinConfig.h" #if ENABLED(EDITABLE_SERVO_ANGLES) @@ -34,6 +35,7 @@ * U - Stowed Angle */ void GcodeSuite::M281() { + if (!parser.seen_any()) return M281_report(); if (!parser.seenval('P')) return; @@ -45,24 +47,32 @@ void GcodeSuite::M281() { return; } #endif - bool angle_change = false; - if (parser.seen('L')) { - servo_angles[servo_index][0] = parser.value_int(); - angle_change = true; - } - if (parser.seen('U')) { - servo_angles[servo_index][1] = parser.value_int(); - angle_change = true; - } - if (!angle_change) { - SERIAL_ECHO_MSG(" Servo ", servo_index, - " L", servo_angles[servo_index][0], - " U", servo_angles[servo_index][1]); - } + if (parser.seen('L')) servo_angles[servo_index][0] = parser.value_int(); + if (parser.seen('U')) servo_angles[servo_index][1] = parser.value_int(); } else SERIAL_ERROR_MSG("Servo ", servo_index, " out of range"); +} +void GcodeSuite::M281_report(const bool forReplay/*=true*/) { + report_heading_etc(forReplay, F(STR_SERVO_ANGLES)); + LOOP_L_N(i, NUM_SERVOS) { + switch (i) { + default: break; + #if ENABLED(SWITCHING_EXTRUDER) + case SWITCHING_EXTRUDER_SERVO_NR: + #if EXTRUDERS > 3 + case SWITCHING_EXTRUDER_E23_SERVO_NR: + #endif + #elif ENABLED(SWITCHING_NOZZLE) + case SWITCHING_NOZZLE_SERVO_NR: + #elif ENABLED(BLTOUCH) || (HAS_Z_SERVO_PROBE && defined(Z_SERVO_ANGLES)) + case Z_PROBE_SERVO_NR: + #endif + report_echo_start(forReplay); + SERIAL_ECHOLNPGM(" M281 P", i, " L", servo_angles[i][0], " U", servo_angles[i][1]); + } + } } #endif // EDITABLE_SERVO_ANGLES diff --git a/Marlin/src/gcode/config/M301.cpp b/Marlin/src/gcode/config/M301.cpp index 7b3f57608bad..fc9f1883d616 100644 --- a/Marlin/src/gcode/config/M301.cpp +++ b/Marlin/src/gcode/config/M301.cpp @@ -46,46 +46,64 @@ * F[float] Kf term */ void GcodeSuite::M301() { - // multi-extruder PID patch: M301 updates or prints a single extruder's PID values // default behavior (omitting E parameter) is to update for extruder 0 only - const uint8_t e = parser.byteval('E'); // extruder being updated + int8_t e = E_TERN0(parser.byteval('E', -1)); // extruder being updated + + if (!parser.seen("PID" TERN_(PID_EXTRUSION_SCALING, "CL") TERN_(PID_FAN_SCALING, "F"))) + return M301_report(true E_OPTARG(e)); + + if (e == -1) e = 0; if (e < HOTENDS) { // catch bad input value - if (parser.seen('P')) PID_PARAM(Kp, e) = parser.value_float(); - if (parser.seen('I')) PID_PARAM(Ki, e) = scalePID_i(parser.value_float()); - if (parser.seen('D')) PID_PARAM(Kd, e) = scalePID_d(parser.value_float()); + + if (parser.seenval('P')) PID_PARAM(Kp, e) = parser.value_float(); + if (parser.seenval('I')) PID_PARAM(Ki, e) = scalePID_i(parser.value_float()); + if (parser.seenval('D')) PID_PARAM(Kd, e) = scalePID_d(parser.value_float()); + #if ENABLED(PID_EXTRUSION_SCALING) - if (parser.seen('C')) PID_PARAM(Kc, e) = parser.value_float(); + if (parser.seenval('C')) PID_PARAM(Kc, e) = parser.value_float(); if (parser.seenval('L')) thermalManager.lpq_len = parser.value_int(); NOMORE(thermalManager.lpq_len, LPQ_MAX_LEN); NOLESS(thermalManager.lpq_len, 0); #endif #if ENABLED(PID_FAN_SCALING) - if (parser.seen('F')) PID_PARAM(Kf, e) = parser.value_float(); + if (parser.seenval('F')) PID_PARAM(Kf, e) = parser.value_float(); #endif thermalManager.updatePID(); - - SERIAL_ECHO_START(); - #if ENABLED(PID_PARAMS_PER_HOTEND) - SERIAL_ECHOPAIR(" e:", e); // specify extruder in serial output - #endif - SERIAL_ECHOPAIR(" p:", PID_PARAM(Kp, e), - " i:", unscalePID_i(PID_PARAM(Ki, e)), - " d:", unscalePID_d(PID_PARAM(Kd, e))); - #if ENABLED(PID_EXTRUSION_SCALING) - SERIAL_ECHOPAIR(" c:", PID_PARAM(Kc, e)); - #endif - #if ENABLED(PID_FAN_SCALING) - SERIAL_ECHOPAIR(" f:", PID_PARAM(Kf, e)); - #endif - - SERIAL_EOL(); } else SERIAL_ERROR_MSG(STR_INVALID_EXTRUDER); } +void GcodeSuite::M301_report(const bool forReplay/*=true*/ E_OPTARG(const int8_t eindex/*=-1*/)) { + report_heading(forReplay, F(STR_HOTEND_PID)); + IF_DISABLED(HAS_MULTI_EXTRUDER, constexpr int8_t eindex = -1); + HOTEND_LOOP() { + if (e == eindex || eindex == -1) { + report_echo_start(forReplay); + SERIAL_ECHOPGM_P( + #if ENABLED(PID_PARAMS_PER_HOTEND) + PSTR(" M301 E"), e, SP_P_STR + #else + PSTR(" M301 P") + #endif + , PID_PARAM(Kp, e) + , PSTR(" I"), unscalePID_i(PID_PARAM(Ki, e)) + , PSTR(" D"), unscalePID_d(PID_PARAM(Kd, e)) + ); + #if ENABLED(PID_EXTRUSION_SCALING) + SERIAL_ECHOPGM_P(SP_C_STR, PID_PARAM(Kc, e)); + if (e == 0) SERIAL_ECHOPGM(" L", thermalManager.lpq_len); + #endif + #if ENABLED(PID_FAN_SCALING) + SERIAL_ECHOPGM(" F", PID_PARAM(Kf, e)); + #endif + SERIAL_EOL(); + } + } +} + #endif // PIDTEMP diff --git a/Marlin/src/gcode/config/M302.cpp b/Marlin/src/gcode/config/M302.cpp index e3ce5a10ef0e..e271dcd469ee 100644 --- a/Marlin/src/gcode/config/M302.cpp +++ b/Marlin/src/gcode/config/M302.cpp @@ -55,8 +55,8 @@ void GcodeSuite::M302() { // Report current state SERIAL_ECHO_START(); SERIAL_ECHOPGM("Cold extrudes are "); - SERIAL_ECHOPGM_P(thermalManager.allow_cold_extrude ? PSTR("en") : PSTR("dis")); - SERIAL_ECHOLNPAIR("abled (min temp ", thermalManager.extrude_min_temp, "C)"); + SERIAL_ECHOF(thermalManager.allow_cold_extrude ? F("en") : F("dis")); + SERIAL_ECHOLNPGM("abled (min temp ", thermalManager.extrude_min_temp, "C)"); } } diff --git a/Marlin/src/gcode/config/M304.cpp b/Marlin/src/gcode/config/M304.cpp index b1af5a5ae21f..97dc4be25e1c 100644 --- a/Marlin/src/gcode/config/M304.cpp +++ b/Marlin/src/gcode/config/M304.cpp @@ -35,15 +35,19 @@ * D - Set the D value */ void GcodeSuite::M304() { - + if (!parser.seen("PID")) return M304_report(); if (parser.seen('P')) thermalManager.temp_bed.pid.Kp = parser.value_float(); if (parser.seen('I')) thermalManager.temp_bed.pid.Ki = scalePID_i(parser.value_float()); if (parser.seen('D')) thermalManager.temp_bed.pid.Kd = scalePID_d(parser.value_float()); +} - SERIAL_ECHO_MSG(" p:", thermalManager.temp_bed.pid.Kp, - " i:", unscalePID_i(thermalManager.temp_bed.pid.Ki), - " d:", unscalePID_d(thermalManager.temp_bed.pid.Kd)); - +void GcodeSuite::M304_report(const bool forReplay/*=true*/) { + report_heading_etc(forReplay, F(STR_BED_PID)); + SERIAL_ECHOLNPGM( + " M304 P", thermalManager.temp_bed.pid.Kp + , " I", unscalePID_i(thermalManager.temp_bed.pid.Ki) + , " D", unscalePID_d(thermalManager.temp_bed.pid.Kd) + ); } #endif // PIDTEMPBED diff --git a/Marlin/src/gcode/config/M305.cpp b/Marlin/src/gcode/config/M305.cpp index 10ef55c17341..6957eef050dd 100644 --- a/Marlin/src/gcode/config/M305.cpp +++ b/Marlin/src/gcode/config/M305.cpp @@ -70,10 +70,10 @@ void GcodeSuite::M305() { } // If not setting then report parameters else if (t_index < 0) { // ...all user thermistors LOOP_L_N(i, USER_THERMISTORS) - thermalManager.log_user_thermistor(i); + thermalManager.M305_report(i); } else // ...one user thermistor - thermalManager.log_user_thermistor(t_index); + thermalManager.M305_report(t_index); } #endif // HAS_USER_THERMISTORS diff --git a/Marlin/src/gcode/config/M309.cpp b/Marlin/src/gcode/config/M309.cpp index 2247481b2579..577023292e2d 100644 --- a/Marlin/src/gcode/config/M309.cpp +++ b/Marlin/src/gcode/config/M309.cpp @@ -35,14 +35,19 @@ * D - Set the D value */ void GcodeSuite::M309() { + if (!parser.seen("PID")) return M309_report(); if (parser.seen('P')) thermalManager.temp_chamber.pid.Kp = parser.value_float(); if (parser.seen('I')) thermalManager.temp_chamber.pid.Ki = scalePID_i(parser.value_float()); if (parser.seen('D')) thermalManager.temp_chamber.pid.Kd = scalePID_d(parser.value_float()); +} - SERIAL_ECHO_START(); - SERIAL_ECHOLNPAIR(" p:", thermalManager.temp_chamber.pid.Kp, - " i:", unscalePID_i(thermalManager.temp_chamber.pid.Ki), - " d:", unscalePID_d(thermalManager.temp_chamber.pid.Kd)); +void GcodeSuite::M309_report(const bool forReplay/*=true*/) { + report_heading_etc(forReplay, F(STR_CHAMBER_PID)); + SERIAL_ECHOLNPGM( + " M309 P", thermalManager.temp_chamber.pid.Kp + , " I", unscalePID_i(thermalManager.temp_chamber.pid.Ki) + , " D", unscalePID_d(thermalManager.temp_chamber.pid.Kd) + ); } #endif // PIDTEMPCHAMBER diff --git a/Marlin/src/gcode/config/M43.cpp b/Marlin/src/gcode/config/M43.cpp index 4009721a57ed..097aa142f03f 100644 --- a/Marlin/src/gcode/config/M43.cpp +++ b/Marlin/src/gcode/config/M43.cpp @@ -65,12 +65,12 @@ inline void toggle_pins() { pin_t pin = GET_PIN_MAP_PIN_M43(i); if (!VALID_PIN(pin)) continue; if (M43_NEVER_TOUCH(i) || (!ignore_protection && pin_is_protected(pin))) { - report_pin_state_extended(pin, ignore_protection, true, PSTR("Untouched ")); + report_pin_state_extended(pin, ignore_protection, true, F("Untouched ")); SERIAL_EOL(); } else { watchdog_refresh(); - report_pin_state_extended(pin, ignore_protection, true, PSTR("Pulsing ")); + report_pin_state_extended(pin, ignore_protection, true, F("Pulsing ")); #ifdef __STM32F1__ const auto prior_mode = _GET_MODE(i); #else @@ -112,7 +112,7 @@ inline void toggle_pins() { } SERIAL_EOL(); } - SERIAL_ECHOLNPGM("Done."); + SERIAL_ECHOLNPGM(STR_DONE); } // toggle_pins @@ -130,7 +130,7 @@ inline void servo_probe_test() { const uint8_t probe_index = parser.byteval('P', Z_PROBE_SERVO_NR); - SERIAL_ECHOLNPAIR("Servo probe test\n" + SERIAL_ECHOLNPGM("Servo probe test\n" ". using index: ", probe_index, ", deploy angle: ", servo_angles[probe_index][0], ", stow angle: ", servo_angles[probe_index][1] @@ -143,7 +143,7 @@ inline void servo_probe_test() { #define PROBE_TEST_PIN Z_MIN_PIN constexpr bool probe_inverting = Z_MIN_ENDSTOP_INVERTING; - SERIAL_ECHOLNPAIR(". Probe Z_MIN_PIN: ", PROBE_TEST_PIN); + SERIAL_ECHOLNPGM(". Probe Z_MIN_PIN: ", PROBE_TEST_PIN); SERIAL_ECHOPGM(". Z_MIN_ENDSTOP_INVERTING: "); #else @@ -151,7 +151,7 @@ inline void servo_probe_test() { #define PROBE_TEST_PIN Z_MIN_PROBE_PIN constexpr bool probe_inverting = Z_MIN_PROBE_ENDSTOP_INVERTING; - SERIAL_ECHOLNPAIR(". Probe Z_MIN_PROBE_PIN: ", PROBE_TEST_PIN); + SERIAL_ECHOLNPGM(". Probe Z_MIN_PROBE_PIN: ", PROBE_TEST_PIN); SERIAL_ECHOPGM( ". Z_MIN_PROBE_ENDSTOP_INVERTING: "); #endif @@ -211,11 +211,11 @@ inline void servo_probe_test() { if (deploy_state != stow_state) { SERIAL_ECHOLNPGM("= Mechanical Switch detected"); if (deploy_state) { - SERIAL_ECHOLNPAIR(" DEPLOYED state: HIGH (logic 1)", + SERIAL_ECHOLNPGM(" DEPLOYED state: HIGH (logic 1)", " STOWED (triggered) state: LOW (logic 0)"); } else { - SERIAL_ECHOLNPAIR(" DEPLOYED state: LOW (logic 0)", + SERIAL_ECHOLNPGM(" DEPLOYED state: LOW (logic 0)", " STOWED (triggered) state: HIGH (logic 1)"); } #if ENABLED(BLTOUCH) @@ -244,7 +244,7 @@ inline void servo_probe_test() { if (probe_counter == 15) SERIAL_ECHOLNPGM(": 30ms or more"); else - SERIAL_ECHOLNPAIR(" (+/- 4ms): ", probe_counter * 2); + SERIAL_ECHOLNPGM(" (+/- 4ms): ", probe_counter * 2); if (probe_counter >= 4) { if (probe_counter == 15) { @@ -288,8 +288,8 @@ inline void servo_probe_test() { * S - Start Pin number. If not given, will default to 0 * L - End Pin number. If not given, will default to last pin defined for this board * I - Flag to ignore Marlin's pin protection. Use with caution!!!! - * R - Repeat pulses on each pin this number of times before continueing to next pin - * W - Wait time (in miliseconds) between pulses. If not given will default to 500 + * R - Repeat pulses on each pin this number of times before continuing to next pin + * W - Wait time (in milliseconds) between pulses. If not given will default to 500 * * M43 S - Servo probe test * P - Probe index (optional - defaults to 0 @@ -303,7 +303,7 @@ void GcodeSuite::M43() { if (parser.seen('E')) { endstops.monitor_flag = parser.value_bool(); SERIAL_ECHOPGM("endstop monitor "); - SERIAL_ECHOPGM_P(endstops.monitor_flag ? PSTR("en") : PSTR("dis")); + SERIAL_ECHOF(endstops.monitor_flag ? F("en") : F("dis")); SERIAL_ECHOLNPGM("abled"); return; } @@ -344,8 +344,8 @@ void GcodeSuite::M43() { #if HAS_RESUME_CONTINUE KEEPALIVE_STATE(PAUSED_FOR_USER); wait_for_user = true; - TERN_(HOST_PROMPT_SUPPORT, host_prompt_do(PROMPT_USER_CONTINUE, PSTR("M43 Wait Called"), CONTINUE_STR)); - TERN_(EXTENSIBLE_UI, ExtUI::onUserConfirmRequired_P(PSTR("M43 Wait Called"))); + TERN_(HOST_PROMPT_SUPPORT, hostui.prompt_do(PROMPT_USER_CONTINUE, F("M43 Wait Called"), FPSTR(CONTINUE_STR))); + TERN_(EXTENSIBLE_UI, ExtUI::onUserConfirmRequired(F("M43 Wait Called"))); #endif for (;;) { diff --git a/Marlin/src/gcode/config/M575.cpp b/Marlin/src/gcode/config/M575.cpp index 44723b7f2fe6..2c12428d982a 100644 --- a/Marlin/src/gcode/config/M575.cpp +++ b/Marlin/src/gcode/config/M575.cpp @@ -52,19 +52,25 @@ void GcodeSuite::M575() { case 2400: case 9600: case 19200: case 38400: case 57600: case 115200: case 250000: case 500000: case 1000000: { const int8_t port = parser.intval('P', -99); - const bool set0 = (port == -99 || port == 0); - if (set0) SERIAL_ECHO_MSG(" Serial ", '0', " baud rate set to ", baud); + const bool set1 = (port == -99 || port == 0); + if (set1) SERIAL_ECHO_MSG(" Serial ", AS_DIGIT(0), " baud rate set to ", baud); #if HAS_MULTI_SERIAL - const bool set1 = (port == -99 || port == 1); - if (set1) SERIAL_ECHO_MSG(" Serial ", '1', " baud rate set to ", baud); + const bool set2 = (port == -99 || port == 1); + if (set2) SERIAL_ECHO_MSG(" Serial ", AS_DIGIT(1), " baud rate set to ", baud); + #ifdef SERIAL_PORT_3 + const bool set3 = (port == -99 || port == 2); + if (set3) SERIAL_ECHO_MSG(" Serial ", AS_DIGIT(2), " baud rate set to ", baud); + #endif #endif SERIAL_FLUSH(); - if (set0) { MYSERIAL0.end(); MYSERIAL0.begin(baud); } - + if (set1) { MYSERIAL1.end(); MYSERIAL1.begin(baud); } #if HAS_MULTI_SERIAL - if (set1) { MYSERIAL1.end(); MYSERIAL1.begin(baud); } + if (set2) { MYSERIAL2.end(); MYSERIAL2.begin(baud); } + #ifdef SERIAL_PORT_3 + if (set3) { MYSERIAL3.end(); MYSERIAL3.begin(baud); } + #endif #endif } break; diff --git a/Marlin/src/gcode/config/M672.cpp b/Marlin/src/gcode/config/M672.cpp index af74230516e1..257b49471f61 100644 --- a/Marlin/src/gcode/config/M672.cpp +++ b/Marlin/src/gcode/config/M672.cpp @@ -53,7 +53,7 @@ // b7 b6 b5 b4 ~b4 ... hi bits, NOT last bit // b3 b2 b1 b0 ~b0 ... lo bits, NOT last bit // -void M672_send(uint8_t b) { // bit rate requirement: 1KHz +/- 30% +void M672_send(uint8_t b) { // bit rate requirement: 1kHz +/- 30% LOOP_L_N(bits, 14) { switch (bits) { default: { OUT_WRITE(SMART_EFFECTOR_MOD_PIN, !!(b & 0x80)); b <<= 1; break; } // send bit, shift next into place diff --git a/Marlin/src/gcode/config/M92.cpp b/Marlin/src/gcode/config/M92.cpp index bdb95db8d6d9..8f527919fd83 100644 --- a/Marlin/src/gcode/config/M92.cpp +++ b/Marlin/src/gcode/config/M92.cpp @@ -23,40 +23,19 @@ #include "../gcode.h" #include "../../module/planner.h" -void report_M92(const bool echo=true, const int8_t e=-1) { - if (echo) SERIAL_ECHO_START(); else SERIAL_CHAR(' '); - SERIAL_ECHOPAIR_P(PSTR(" M92 X"), LINEAR_UNIT(planner.settings.axis_steps_per_mm[X_AXIS]), - SP_Y_STR, LINEAR_UNIT(planner.settings.axis_steps_per_mm[Y_AXIS]), - SP_Z_STR, LINEAR_UNIT(planner.settings.axis_steps_per_mm[Z_AXIS])); - #if DISABLED(DISTINCT_E_FACTORS) - SERIAL_ECHOPAIR_P(SP_E_STR, VOLUMETRIC_UNIT(planner.settings.axis_steps_per_mm[E_AXIS])); - #endif - SERIAL_EOL(); - - #if ENABLED(DISTINCT_E_FACTORS) - LOOP_L_N(i, E_STEPPERS) { - if (e >= 0 && i != e) continue; - if (echo) SERIAL_ECHO_START(); else SERIAL_CHAR(' '); - SERIAL_ECHOLNPAIR_P(PSTR(" M92 T"), i, - SP_E_STR, VOLUMETRIC_UNIT(planner.settings.axis_steps_per_mm[E_AXIS_N(i)])); - } - #endif - - UNUSED_E(e); -} - /** - * M92: Set axis steps-per-unit for one or more axes, X, Y, Z, and E. + * M92: Set axis steps-per-unit for one or more axes, X, Y, Z, [I, [J, [K]]] and E. * (Follows the same syntax as G92) * * With multiple extruders use T to specify which one. * * If no argument is given print the current values. * - * With MAGIC_NUMBERS_GCODE: - * Use 'H' and/or 'L' to get ideal layer-height information. - * 'H' specifies micro-steps to use. We guess if it's not supplied. - * 'L' specifies a desired layer height. Nearest good heights are shown. + * With MAGIC_NUMBERS_GCODE: + * + * Use 'H' and/or 'L' to get ideal layer-height information. + * H - Specify micro-steps to use. Best guess if not supplied. + * L - Desired layer height in current units. Nearest good heights are shown. */ void GcodeSuite::M92() { @@ -64,28 +43,26 @@ void GcodeSuite::M92() { if (target_extruder < 0) return; // No arguments? Show M92 report. - if (!parser.seen("XYZE" - #if ENABLED(MAGIC_NUMBERS_GCODE) - "HL" - #endif - )) return report_M92(true, target_extruder); + if (!parser.seen(LOGICAL_AXES_STRING TERN_(MAGIC_NUMBERS_GCODE, "HL"))) + return M92_report(true, target_extruder); - LOOP_XYZE(i) { + LOOP_LOGICAL_AXES(i) { if (parser.seenval(axis_codes[i])) { - if (i == E_AXIS) { - const float value = parser.value_per_axis_units((AxisEnum)(E_AXIS_N(target_extruder))); - if (value < 20) { - float factor = planner.settings.axis_steps_per_mm[E_AXIS_N(target_extruder)] / value; // increase e constants if M92 E14 is given for netfab. - #if HAS_CLASSIC_JERK && HAS_CLASSIC_E_JERK - planner.max_jerk.e *= factor; - #endif - planner.settings.max_feedrate_mm_s[E_AXIS_N(target_extruder)] *= factor; - planner.max_acceleration_steps_per_s2[E_AXIS_N(target_extruder)] *= factor; - } - planner.settings.axis_steps_per_mm[E_AXIS_N(target_extruder)] = value; - } - else { + if (TERN1(HAS_EXTRUDERS, i != E_AXIS)) planner.settings.axis_steps_per_mm[i] = parser.value_per_axis_units((AxisEnum)i); + else { + #if HAS_EXTRUDERS + const float value = parser.value_per_axis_units((AxisEnum)(E_AXIS_N(target_extruder))); + if (value < 20) { + float factor = planner.settings.axis_steps_per_mm[E_AXIS_N(target_extruder)] / value; // increase e constants if M92 E14 is given for netfab. + #if HAS_CLASSIC_JERK && HAS_CLASSIC_E_JERK + planner.max_jerk.e *= factor; + #endif + planner.settings.max_feedrate_mm_s[E_AXIS_N(target_extruder)] *= factor; + planner.max_acceleration_steps_per_s2[E_AXIS_N(target_extruder)] *= factor; + } + planner.settings.axis_steps_per_mm[E_AXIS_N(target_extruder)] = value; + #endif } } } @@ -95,16 +72,16 @@ void GcodeSuite::M92() { #ifndef Z_MICROSTEPS #define Z_MICROSTEPS 16 #endif - const float wanted = parser.floatval('L'); + const float wanted = parser.linearval('L'); if (parser.seen('H') || wanted) { const uint16_t argH = parser.ushortval('H'), micro_steps = argH ?: Z_MICROSTEPS; - const float z_full_step_mm = micro_steps * planner.steps_to_mm[Z_AXIS]; + const float z_full_step_mm = micro_steps * planner.mm_per_step[Z_AXIS]; SERIAL_ECHO_START(); - SERIAL_ECHOPAIR("{ micro_steps:", micro_steps, ", z_full_step_mm:", z_full_step_mm); + SERIAL_ECHOPGM("{ micro_steps:", micro_steps, ", z_full_step_mm:", z_full_step_mm); if (wanted) { const float best = uint16_t(wanted / z_full_step_mm) * z_full_step_mm; - SERIAL_ECHOPAIR(", best:[", best); + SERIAL_ECHOPGM(", best:[", best); if (best != wanted) { SERIAL_CHAR(','); SERIAL_DECIMAL(best + z_full_step_mm); } SERIAL_CHAR(']'); } @@ -112,3 +89,32 @@ void GcodeSuite::M92() { } #endif } + +void GcodeSuite::M92_report(const bool forReplay/*=true*/, const int8_t e/*=-1*/) { + report_heading_etc(forReplay, F(STR_STEPS_PER_UNIT)); + SERIAL_ECHOPGM_P(LIST_N(DOUBLE(LINEAR_AXES), + PSTR(" M92 X"), LINEAR_UNIT(planner.settings.axis_steps_per_mm[X_AXIS]), + SP_Y_STR, LINEAR_UNIT(planner.settings.axis_steps_per_mm[Y_AXIS]), + SP_Z_STR, LINEAR_UNIT(planner.settings.axis_steps_per_mm[Z_AXIS]), + SP_I_STR, LINEAR_UNIT(planner.settings.axis_steps_per_mm[I_AXIS]), + SP_J_STR, LINEAR_UNIT(planner.settings.axis_steps_per_mm[J_AXIS]), + SP_K_STR, LINEAR_UNIT(planner.settings.axis_steps_per_mm[K_AXIS])) + ); + #if HAS_EXTRUDERS && DISABLED(DISTINCT_E_FACTORS) + SERIAL_ECHOPGM_P(SP_E_STR, VOLUMETRIC_UNIT(planner.settings.axis_steps_per_mm[E_AXIS])); + #endif + SERIAL_EOL(); + + #if ENABLED(DISTINCT_E_FACTORS) + LOOP_L_N(i, E_STEPPERS) { + if (e >= 0 && i != e) continue; + report_echo_start(forReplay); + SERIAL_ECHOLNPGM_P( + PSTR(" M92 T"), i, + SP_E_STR, VOLUMETRIC_UNIT(planner.settings.axis_steps_per_mm[E_AXIS_N(i)]) + ); + } + #else + UNUSED(e); + #endif +} diff --git a/Marlin/src/gcode/control/M10-M11.cpp b/Marlin/src/gcode/control/M10-M11.cpp new file mode 100644 index 000000000000..d5a69dcfccfb --- /dev/null +++ b/Marlin/src/gcode/control/M10-M11.cpp @@ -0,0 +1,44 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2021 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + * + */ + +#include "../../inc/MarlinConfig.h" + +#if ENABLED(AIR_EVACUATION) + +#include "../gcode.h" +#include "../../feature/spindle_laser.h" + +/** + * M10: Vacuum or Blower On + */ +void GcodeSuite::M10() { + cutter.air_evac_enable(); // Turn on Vacuum or Blower motor +} + +/** + * M11: Vacuum or Blower OFF + */ +void GcodeSuite::M11() { + cutter.air_evac_disable(); // Turn off Vacuum or Blower motor +} + +#endif // AIR_EVACUATION diff --git a/Marlin/src/gcode/control/M108_M112_M410.cpp b/Marlin/src/gcode/control/M108_M112_M410.cpp index 309c806c8fc5..39f9c04e1915 100644 --- a/Marlin/src/gcode/control/M108_M112_M410.cpp +++ b/Marlin/src/gcode/control/M108_M112_M410.cpp @@ -40,7 +40,7 @@ void GcodeSuite::M108() { * M112: Full Shutdown */ void GcodeSuite::M112() { - kill(M112_KILL_STR, nullptr, true); + kill(FPSTR(M112_KILL_STR), nullptr, true); } /** diff --git a/Marlin/src/gcode/control/M111.cpp b/Marlin/src/gcode/control/M111.cpp index 8e677080e0b3..d6aeb774108b 100644 --- a/Marlin/src/gcode/control/M111.cpp +++ b/Marlin/src/gcode/control/M111.cpp @@ -26,7 +26,7 @@ * M111: Set the debug level */ void GcodeSuite::M111() { - if (parser.seen('S')) marlin_debug_flags = parser.byteval('S'); + if (parser.seenval('S')) marlin_debug_flags = parser.value_byte(); static PGMSTR(str_debug_1, STR_DEBUG_ECHO); static PGMSTR(str_debug_2, STR_DEBUG_INFO); @@ -34,12 +34,12 @@ void GcodeSuite::M111() { static PGMSTR(str_debug_8, STR_DEBUG_DRYRUN); static PGMSTR(str_debug_16, STR_DEBUG_COMMUNICATION); #if ENABLED(DEBUG_LEVELING_FEATURE) - static PGMSTR(str_debug_lvl, STR_DEBUG_LEVELING); + static PGMSTR(str_debug_detail, STR_DEBUG_DETAIL); #endif static PGM_P const debug_strings[] PROGMEM = { str_debug_1, str_debug_2, str_debug_4, str_debug_8, str_debug_16, - TERN_(DEBUG_LEVELING_FEATURE, str_debug_lvl) + TERN_(DEBUG_LEVELING_FEATURE, str_debug_detail) }; SERIAL_ECHO_START(); @@ -57,19 +57,19 @@ void GcodeSuite::M111() { SERIAL_ECHOPGM(STR_DEBUG_OFF); #if !defined(__AVR__) || !defined(USBCON) #if ENABLED(SERIAL_STATS_RX_BUFFER_OVERRUNS) - SERIAL_ECHOPAIR("\nBuffer Overruns: ", MYSERIAL0.buffer_overruns()); + SERIAL_ECHOPGM("\nBuffer Overruns: ", MYSERIAL1.buffer_overruns()); #endif #if ENABLED(SERIAL_STATS_RX_FRAMING_ERRORS) - SERIAL_ECHOPAIR("\nFraming Errors: ", MYSERIAL0.framing_errors()); + SERIAL_ECHOPGM("\nFraming Errors: ", MYSERIAL1.framing_errors()); #endif #if ENABLED(SERIAL_STATS_DROPPED_RX) - SERIAL_ECHOPAIR("\nDropped bytes: ", MYSERIAL0.dropped()); + SERIAL_ECHOPGM("\nDropped bytes: ", MYSERIAL1.dropped()); #endif #if ENABLED(SERIAL_STATS_MAX_RX_QUEUED) - SERIAL_ECHOPAIR("\nMax RX Queue Size: ", MYSERIAL0.rxMaxEnqueued()); + SERIAL_ECHOPGM("\nMax RX Queue Size: ", MYSERIAL1.rxMaxEnqueued()); #endif #endif // !__AVR__ || !USBCON } diff --git a/Marlin/src/gcode/control/M17_M18_M84.cpp b/Marlin/src/gcode/control/M17_M18_M84.cpp index b35b4653319c..4683786f1fd0 100644 --- a/Marlin/src/gcode/control/M17_M18_M84.cpp +++ b/Marlin/src/gcode/control/M17_M18_M84.cpp @@ -29,24 +29,186 @@ #include "../../feature/bedlevel/bedlevel.h" #endif +#define DEBUG_OUT ENABLED(MARLIN_DEV_MODE) +#include "../../core/debug_out.h" +#include "../../libs/hex_print.h" + +inline axis_flags_t selected_axis_bits() { + axis_flags_t selected{0}; + #if HAS_EXTRUDERS + if (parser.seen('E')) { + if (E_TERN0(parser.has_value())) { + const uint8_t e = parser.value_int(); + if (e < EXTRUDERS) + selected.bits = _BV(INDEX_OF_AXIS(E_AXIS, e)); + } + else + selected.bits = selected.e_bits(); + } + #endif + selected.bits |= LINEAR_AXIS_GANG( + (parser.seen_test('X') << X_AXIS), + | (parser.seen_test('Y') << Y_AXIS), + | (parser.seen_test('Z') << Z_AXIS), + | (parser.seen_test(AXIS4_NAME) << I_AXIS), + | (parser.seen_test(AXIS5_NAME) << J_AXIS), + | (parser.seen_test(AXIS6_NAME) << K_AXIS) + ); + return selected; +} + +// Enable specified axes and warn about other affected axes +void do_enable(const axis_flags_t to_enable) { + const ena_mask_t was_enabled = stepper.axis_enabled.bits, + shall_enable = to_enable.bits & ~was_enabled; + + DEBUG_ECHOLNPGM("Now Enabled: ", hex_word(stepper.axis_enabled.bits), " Enabling: ", hex_word(to_enable.bits), " | ", shall_enable); + + if (!shall_enable) return; // All specified axes already enabled? + + ena_mask_t also_enabled = 0; // Track steppers enabled due to overlap + + // Enable all flagged axes + LOOP_LINEAR_AXES(a) { + if (TEST(shall_enable, a)) { + stepper.enable_axis(AxisEnum(a)); // Mark and enable the requested axis + DEBUG_ECHOLNPGM("Enabled ", axis_codes[a], " (", a, ") with overlap ", hex_word(enable_overlap[a]), " ... Enabled: ", hex_word(stepper.axis_enabled.bits)); + also_enabled |= enable_overlap[a]; + } + } + #if HAS_EXTRUDERS + LOOP_L_N(e, EXTRUDERS) { + const uint8_t a = INDEX_OF_AXIS(E_AXIS, e); + if (TEST(shall_enable, a)) { + stepper.ENABLE_EXTRUDER(e); + DEBUG_ECHOLNPGM("Enabled E", AS_DIGIT(e), " (", a, ") with overlap ", hex_word(enable_overlap[a]), " ... ", hex_word(stepper.axis_enabled.bits)); + also_enabled |= enable_overlap[a]; + } + } + #endif + + if ((also_enabled &= ~(shall_enable | was_enabled))) { + SERIAL_CHAR('('); + LOOP_LINEAR_AXES(a) if (TEST(also_enabled, a)) SERIAL_CHAR(axis_codes[a], ' '); + #if HAS_EXTRUDERS + #define _EN_ALSO(N) if (TEST(also_enabled, INDEX_OF_AXIS(E_AXIS, N))) SERIAL_CHAR('E', '0' + N, ' '); + REPEAT(EXTRUDERS, _EN_ALSO) + #endif + SERIAL_ECHOLNPGM("also enabled)"); + } + + DEBUG_ECHOLNPGM("Enabled Now: ", hex_word(stepper.axis_enabled.bits)); +} + /** - * M17: Enable stepper motors + * M17: Enable stepper motor power for one or more axes. + * Print warnings for axes that share an ENABLE_PIN. + * + * Examples: + * + * M17 XZ ; Enable X and Z axes + * M17 E ; Enable all E steppers + * M17 E1 ; Enable just the E1 stepper */ void GcodeSuite::M17() { - if (parser.seen("XYZE")) { - if (parser.seen('X')) ENABLE_AXIS_X(); - if (parser.seen('Y')) ENABLE_AXIS_Y(); - if (parser.seen('Z')) ENABLE_AXIS_Z(); - if (TERN0(HAS_E_STEPPER_ENABLE, parser.seen('E'))) enable_e_steppers(); + if (parser.seen_axis()) { + if (any_enable_overlap()) + do_enable(selected_axis_bits()); + else { + #if HAS_EXTRUDERS + if (parser.seen('E')) { + if (parser.has_value()) { + const uint8_t e = parser.value_int(); + if (e < EXTRUDERS) stepper.ENABLE_EXTRUDER(e); + } + else + stepper.enable_e_steppers(); + } + #endif + LINEAR_AXIS_CODE( + if (parser.seen_test('X')) stepper.enable_axis(X_AXIS), + if (parser.seen_test('Y')) stepper.enable_axis(Y_AXIS), + if (parser.seen_test('Z')) stepper.enable_axis(Z_AXIS), + if (parser.seen_test(AXIS4_NAME)) stepper.enable_axis(I_AXIS), + if (parser.seen_test(AXIS5_NAME)) stepper.enable_axis(J_AXIS), + if (parser.seen_test(AXIS6_NAME)) stepper.enable_axis(K_AXIS) + ); + } } else { - LCD_MESSAGEPGM(MSG_NO_MOVE); - enable_all_steppers(); + LCD_MESSAGE(MSG_NO_MOVE); + stepper.enable_all_steppers(); + } +} + +void try_to_disable(const axis_flags_t to_disable) { + ena_mask_t still_enabled = to_disable.bits & stepper.axis_enabled.bits; + + DEBUG_ECHOLNPGM("Enabled: ", hex_word(stepper.axis_enabled.bits), " To Disable: ", hex_word(to_disable.bits), " | ", hex_word(still_enabled)); + + if (!still_enabled) return; + + // Attempt to disable all flagged axes + LOOP_LINEAR_AXES(a) + if (TEST(to_disable.bits, a)) { + DEBUG_ECHOPGM("Try to disable ", axis_codes[a], " (", a, ") with overlap ", hex_word(enable_overlap[a]), " ... "); + if (stepper.disable_axis(AxisEnum(a))) { // Mark the requested axis and request to disable + DEBUG_ECHOPGM("OK"); + still_enabled &= ~(_BV(a) | enable_overlap[a]); // If actually disabled, clear one or more tracked bits + } + else + DEBUG_ECHOPGM("OVERLAP"); + DEBUG_ECHOLNPGM(" ... still_enabled=", hex_word(still_enabled)); + } + #if HAS_EXTRUDERS + LOOP_L_N(e, EXTRUDERS) { + const uint8_t a = INDEX_OF_AXIS(E_AXIS, e); + if (TEST(to_disable.bits, a)) { + DEBUG_ECHOPGM("Try to disable E", AS_DIGIT(e), " (", a, ") with overlap ", hex_word(enable_overlap[a]), " ... "); + if (stepper.DISABLE_EXTRUDER(e)) { + DEBUG_ECHOPGM("OK"); + still_enabled &= ~(_BV(a) | enable_overlap[a]); + } + else + DEBUG_ECHOPGM("OVERLAP"); + DEBUG_ECHOLNPGM(" ... still_enabled=", hex_word(still_enabled)); + } + } + #endif + + auto overlap_warning = [](const ena_mask_t axis_bits) { + SERIAL_ECHOPGM(" not disabled. Shared with"); + LOOP_LINEAR_AXES(a) if (TEST(axis_bits, a)) SERIAL_CHAR(' ', axis_codes[a]); + #if HAS_EXTRUDERS + #define _EN_STILLON(N) if (TEST(axis_bits, INDEX_OF_AXIS(E_AXIS, N))) SERIAL_CHAR(' ', 'E', '0' + N); + REPEAT(EXTRUDERS, _EN_STILLON) + #endif + SERIAL_ECHOLNPGM("."); + }; + + // If any of the requested axes are still enabled, give a warning + LOOP_LINEAR_AXES(a) { + if (TEST(still_enabled, a)) { + SERIAL_CHAR(axis_codes[a]); + overlap_warning(stepper.axis_enabled.bits & enable_overlap[a]); + } } + #if HAS_EXTRUDERS + LOOP_L_N(e, EXTRUDERS) { + const uint8_t a = INDEX_OF_AXIS(E_AXIS, e); + if (TEST(still_enabled, a)) { + SERIAL_CHAR('E', '0' + e); + overlap_warning(stepper.axis_enabled.bits & enable_overlap[a]); + } + } + #endif + + DEBUG_ECHOLNPGM("Enabled Now: ", hex_word(stepper.axis_enabled.bits)); } /** - * M18, M84: Disable stepper motors + * M18, M84: Disable stepper motor power for one or more axes. + * Print warnings for axes that share an ENABLE_PIN. */ void GcodeSuite::M18_M84() { if (parser.seenval('S')) { @@ -54,12 +216,28 @@ void GcodeSuite::M18_M84() { stepper_inactive_time = parser.value_millis_from_seconds(); } else { - if (parser.seen("XYZE")) { + if (parser.seen_axis()) { planner.synchronize(); - if (parser.seen('X')) DISABLE_AXIS_X(); - if (parser.seen('Y')) DISABLE_AXIS_Y(); - if (parser.seen('Z')) DISABLE_AXIS_Z(); - if (TERN0(HAS_E_STEPPER_ENABLE, parser.seen('E'))) disable_e_steppers(); + if (any_enable_overlap()) + try_to_disable(selected_axis_bits()); + else { + #if HAS_EXTRUDERS + if (parser.seen('E')) { + if (E_TERN0(parser.has_value())) + stepper.DISABLE_EXTRUDER(parser.value_int()); + else + stepper.disable_e_steppers(); + } + #endif + LINEAR_AXIS_CODE( + if (parser.seen_test('X')) stepper.disable_axis(X_AXIS), + if (parser.seen_test('Y')) stepper.disable_axis(Y_AXIS), + if (parser.seen_test('Z')) stepper.disable_axis(Z_AXIS), + if (parser.seen_test(AXIS4_NAME)) stepper.disable_axis(I_AXIS), + if (parser.seen_test(AXIS5_NAME)) stepper.disable_axis(J_AXIS), + if (parser.seen_test(AXIS6_NAME)) stepper.disable_axis(K_AXIS) + ); + } } else planner.finish_and_disable(); diff --git a/Marlin/src/gcode/control/M211.cpp b/Marlin/src/gcode/control/M211.cpp index 2049f1eb6955..95ae052a7bd4 100644 --- a/Marlin/src/gcode/control/M211.cpp +++ b/Marlin/src/gcode/control/M211.cpp @@ -33,14 +33,22 @@ * Usage: M211 S1 to enable, M211 S0 to disable, M211 alone for report */ void GcodeSuite::M211() { + if (parser.seen('S')) + soft_endstop._enabled = parser.value_bool(); + else + M211_report(); +} + +void GcodeSuite::M211_report(const bool forReplay/*=true*/) { + report_heading_etc(forReplay, F(STR_SOFT_ENDSTOPS)); + SERIAL_ECHOPGM(" M211 S", AS_DIGIT(soft_endstop._enabled), " ; "); + serialprintln_onoff(soft_endstop._enabled); + + report_echo_start(forReplay); const xyz_pos_t l_soft_min = soft_endstop.min.asLogical(), l_soft_max = soft_endstop.max.asLogical(); - SERIAL_ECHO_START(); - SERIAL_ECHOPGM(STR_SOFT_ENDSTOPS); - if (parser.seen('S')) soft_endstop._enabled = parser.value_bool(); - serialprint_onoff(soft_endstop._enabled); - print_xyz(l_soft_min, PSTR(STR_SOFT_MIN), PSTR(" ")); - print_xyz(l_soft_max, PSTR(STR_SOFT_MAX)); + print_pos(l_soft_min, F(STR_SOFT_MIN), F(" ")); + print_pos(l_soft_max, F(STR_SOFT_MAX)); } -#endif +#endif // HAS_SOFTWARE_ENDSTOPS diff --git a/Marlin/src/gcode/control/M280.cpp b/Marlin/src/gcode/control/M280.cpp index 187c9a9b1998..2a8e73eafbf2 100644 --- a/Marlin/src/gcode/control/M280.cpp +++ b/Marlin/src/gcode/control/M280.cpp @@ -26,22 +26,44 @@ #include "../gcode.h" #include "../../module/servo.h" +#include "../../module/planner.h" /** - * M280: Get or set servo position. P [S] + * M280: Get or set servo position. + * P - Servo index + * S - Angle to set, omit to read current angle, or use -1 to detach + * + * With POLARGRAPH: + * T - Duration of servo move */ void GcodeSuite::M280() { - if (!parser.seen('P')) return; + if (!parser.seenval('P')) return; + + TERN_(POLARGRAPH, planner.synchronize()); const int servo_index = parser.value_int(); if (WITHIN(servo_index, 0, NUM_SERVOS - 1)) { - if (parser.seen('S')) { - const int a = parser.value_int(); - if (a == -1) - servo[servo_index].detach(); + if (parser.seenval('S')) { + const int anew = parser.value_int(); + if (anew >= 0) { + #if ENABLED(POLARGRAPH) + if (parser.seen('T')) { // (ms) Total duration of servo move + const int16_t t = constrain(parser.value_int(), 0, 10000); + const int aold = servo[servo_index].read(); + millis_t now = millis(); + const millis_t start = now, end = start + t; + while (PENDING(now, end)) { + safe_delay(50); + now = _MIN(millis(), end); + MOVE_SERVO(servo_index, LROUND(aold + (anew - aold) * (float(now - start) / t))); + } + } + #endif // POLARGRAPH + MOVE_SERVO(servo_index, anew); + } else - MOVE_SERVO(servo_index, a); + DETACH_SERVO(servo_index); } else SERIAL_ECHO_MSG(" Servo ", servo_index, ": ", servo[servo_index].read()); diff --git a/Marlin/src/gcode/control/M282.cpp b/Marlin/src/gcode/control/M282.cpp new file mode 100644 index 000000000000..e6f5ce7dccc3 --- /dev/null +++ b/Marlin/src/gcode/control/M282.cpp @@ -0,0 +1,45 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2021 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + * + */ + +#include "../../inc/MarlinConfig.h" + +#if ENABLED(SERVO_DETACH_GCODE) + +#include "../gcode.h" +#include "../../module/servo.h" + +/** + * M282: Detach Servo. P + */ +void GcodeSuite::M282() { + + if (!parser.seenval('P')) return; + + const int servo_index = parser.value_int(); + if (WITHIN(servo_index, 0, NUM_SERVOS - 1)) + DETACH_SERVO(servo_index); + else + SERIAL_ECHO_MSG("Servo ", servo_index, " out of range"); + +} + +#endif // SERVO_DETACH_GCODE diff --git a/Marlin/src/gcode/control/M3-M5.cpp b/Marlin/src/gcode/control/M3-M5.cpp index 711bb7e5e4a7..2cf22e81ce0c 100644 --- a/Marlin/src/gcode/control/M3-M5.cpp +++ b/Marlin/src/gcode/control/M3-M5.cpp @@ -43,7 +43,7 @@ * * If no PWM pin is defined then M3/M4 just turns it on. * - * At least 12.8KHz (50Hz * 256) is needed for Spindle PWM. + * At least 12.8kHz (50Hz * 256) is needed for Spindle PWM. * Hardware PWM is required on AVR. ISRs are too slow. * * NOTE: WGM for timers 3, 4, and 5 must be either Mode 1 or Mode 5. @@ -66,27 +66,29 @@ * PWM duty cycle goes from 0 (off) to 255 (always on). */ void GcodeSuite::M3_M4(const bool is_M4) { - auto get_s_power = [] { - if (parser.seenval('S')) { - const float spwr = parser.value_float(); - #if ENABLED(SPINDLE_SERVO) - cutter.unitPower = spwr; - #else - cutter.unitPower = TERN(SPINDLE_LASER_PWM, - cutter.power_to_range(cutter_power_t(round(spwr))), - spwr > 0 ? 255 : 0); - #endif - } - else - cutter.unitPower = cutter.cpwr_to_upwr(SPEED_POWER_STARTUP); - return cutter.unitPower; - }; + #if EITHER(SPINDLE_LASER_USE_PWM, SPINDLE_SERVO) + auto get_s_power = [] { + if (parser.seenval('S')) { + const float spwr = parser.value_float(); + #if ENABLED(SPINDLE_SERVO) + cutter.unitPower = spwr; + #else + cutter.unitPower = TERN(SPINDLE_LASER_USE_PWM, + cutter.power_to_range(cutter_power_t(round(spwr))), + spwr > 0 ? 255 : 0); + #endif + } + else + cutter.unitPower = cutter.cpwr_to_upwr(SPEED_POWER_STARTUP); + return cutter.unitPower; + }; + #endif #if ENABLED(LASER_POWER_INLINE) if (parser.seen('I') == DISABLED(LASER_POWER_INLINE_INVERT)) { // Laser power in inline mode cutter.inline_direction(is_M4); // Should always be unused - #if ENABLED(SPINDLE_LASER_PWM) + #if ENABLED(SPINDLE_LASER_USE_PWM) if (parser.seen('O')) { cutter.unitPower = cutter.power_to_range(parser.value_byte(), 0); cutter.inline_ocr_power(cutter.unitPower); // The OCR is a value from 0 to 255 (uint8_t) @@ -105,10 +107,10 @@ void GcodeSuite::M3_M4(const bool is_M4) { planner.synchronize(); // Wait for previous movement commands (G0/G0/G2/G3) to complete before changing power cutter.set_reverse(is_M4); - #if ENABLED(SPINDLE_LASER_PWM) + #if ENABLED(SPINDLE_LASER_USE_PWM) if (parser.seenval('O')) { cutter.unitPower = cutter.power_to_range(parser.value_byte(), 0); - cutter.set_ocr_power(cutter.unitPower); // The OCR is a value from 0 to 255 (uint8_t) + cutter.ocr_set_power(cutter.unitPower); // The OCR is a value from 0 to 255 (uint8_t) } else cutter.set_power(cutter.upower_to_ocr(get_s_power())); diff --git a/Marlin/src/gcode/control/M350_M351.cpp b/Marlin/src/gcode/control/M350_M351.cpp index 463bd2ad5815..a92238e4bbf2 100644 --- a/Marlin/src/gcode/control/M350_M351.cpp +++ b/Marlin/src/gcode/control/M350_M351.cpp @@ -34,7 +34,7 @@ */ void GcodeSuite::M350() { if (parser.seen('S')) LOOP_LE_N(i, 4) stepper.microstep_mode(i, parser.value_byte()); - LOOP_XYZE(i) if (parser.seen(axis_codes[i])) stepper.microstep_mode(i, parser.value_byte()); + LOOP_LOGICAL_AXES(i) if (parser.seen(axis_codes[i])) stepper.microstep_mode(i, parser.value_byte()); if (parser.seen('B')) stepper.microstep_mode(4, parser.value_byte()); stepper.microstep_readings(); } @@ -46,15 +46,15 @@ void GcodeSuite::M350() { void GcodeSuite::M351() { if (parser.seenval('S')) switch (parser.value_byte()) { case 1: - LOOP_XYZE(i) if (parser.seenval(axis_codes[i])) stepper.microstep_ms(i, parser.value_byte(), -1, -1); + LOOP_LOGICAL_AXES(i) if (parser.seenval(axis_codes[i])) stepper.microstep_ms(i, parser.value_byte(), -1, -1); if (parser.seenval('B')) stepper.microstep_ms(4, parser.value_byte(), -1, -1); break; case 2: - LOOP_XYZE(i) if (parser.seenval(axis_codes[i])) stepper.microstep_ms(i, -1, parser.value_byte(), -1); + LOOP_LOGICAL_AXES(i) if (parser.seenval(axis_codes[i])) stepper.microstep_ms(i, -1, parser.value_byte(), -1); if (parser.seenval('B')) stepper.microstep_ms(4, -1, parser.value_byte(), -1); break; case 3: - LOOP_XYZE(i) if (parser.seenval(axis_codes[i])) stepper.microstep_ms(i, -1, -1, parser.value_byte()); + LOOP_LOGICAL_AXES(i) if (parser.seenval(axis_codes[i])) stepper.microstep_ms(i, -1, -1, parser.value_byte()); if (parser.seenval('B')) stepper.microstep_ms(4, -1, -1, parser.value_byte()); break; } diff --git a/Marlin/src/gcode/control/M42.cpp b/Marlin/src/gcode/control/M42.cpp index 6ef8455e0b5c..1b3a29d10056 100644 --- a/Marlin/src/gcode/control/M42.cpp +++ b/Marlin/src/gcode/control/M42.cpp @@ -31,6 +31,13 @@ #include "../../module/temperature.h" #endif +#ifdef MAPLE_STM32F1 + // these are enums on the F1... + #define INPUT_PULLDOWN INPUT_PULLDOWN + #define INPUT_ANALOG INPUT_ANALOG + #define OUTPUT_OPEN_DRAIN OUTPUT_OPEN_DRAIN +#endif + void protected_pin_err() { SERIAL_ERROR_MSG(STR_ERR_PROTECTED_PIN); } @@ -45,7 +52,7 @@ void protected_pin_err() { * S Pin status from 0 - 255 * I Flag to ignore Marlin's pin protection * - * M Pin mode: 0=INPUT 1=OUTPUT 2=INPUT_PULLUP 3=INPUT_PULLDOWN + * T Pin mode: 0=INPUT 1=OUTPUT 2=INPUT_PULLUP 3=INPUT_PULLDOWN */ void GcodeSuite::M42() { const int pin_index = PARSED_PIN_INDEX('P', GET_PIN_MAP_INDEX(LED_PIN)); @@ -55,13 +62,20 @@ void GcodeSuite::M42() { if (!parser.boolval('I') && pin_is_protected(pin)) return protected_pin_err(); - if (parser.seenval('M')) { + bool avoidWrite = false; + if (parser.seenval('T')) { switch (parser.value_byte()) { - case 0: pinMode(pin, INPUT); break; + case 0: pinMode(pin, INPUT); avoidWrite = true; break; case 1: pinMode(pin, OUTPUT); break; - case 2: pinMode(pin, INPUT_PULLUP); break; + case 2: pinMode(pin, INPUT_PULLUP); avoidWrite = true; break; #ifdef INPUT_PULLDOWN - case 3: pinMode(pin, INPUT_PULLDOWN); break; + case 3: pinMode(pin, INPUT_PULLDOWN); avoidWrite = true; break; + #endif + #ifdef INPUT_ANALOG + case 4: pinMode(pin, INPUT_ANALOG); avoidWrite = true; break; + #endif + #ifdef OUTPUT_OPEN_DRAIN + case 5: pinMode(pin, OUTPUT_OPEN_DRAIN); break; #endif default: SERIAL_ECHOLNPGM("Invalid Pin Mode"); return; } @@ -99,9 +113,23 @@ void GcodeSuite::M42() { } #endif - pinMode(pin, OUTPUT); + if (avoidWrite) { + SERIAL_ECHOLNPGM("?Cannot write to INPUT"); + return; + } + + // An OUTPUT_OPEN_DRAIN should not be changed to normal OUTPUT (STM32) + // Use M42 Px M1/5 S0/1 to set the output type and then set value + #ifndef OUTPUT_OPEN_DRAIN + pinMode(pin, OUTPUT); + #endif extDigitalWrite(pin, pin_status); - analogWrite(pin, pin_status); + + #ifdef ARDUINO_ARCH_STM32 + // A simple I/O will be set to 0 by hal.set_pwm_duty() + if (pin_status <= 1 && !PWM_PIN(pin)) return; + #endif + hal.set_pwm_duty(pin, pin_status); } #endif // DIRECT_PIN_CONTROL diff --git a/Marlin/src/gcode/control/M605.cpp b/Marlin/src/gcode/control/M605.cpp index 3d13cb1c2444..788659e7e27a 100644 --- a/Marlin/src/gcode/control/M605.cpp +++ b/Marlin/src/gcode/control/M605.cpp @@ -70,26 +70,12 @@ dual_x_carriage_mode = (DualXMode)parser.value_byte(); idex_set_mirrored_mode(false); - if (dual_x_carriage_mode == DXC_MIRRORED_MODE) { - if (previous_mode != DXC_DUPLICATION_MODE) { - SERIAL_ECHOLNPGM("Printer must be in DXC_DUPLICATION_MODE prior to "); - SERIAL_ECHOLNPGM("specifying DXC_MIRRORED_MODE."); - dual_x_carriage_mode = DEFAULT_DUAL_X_CARRIAGE_MODE; - return; - } - idex_set_mirrored_mode(true); - float x_jog = current_position.x - .1; - for (uint8_t i = 2; --i;) { - planner.buffer_line(x_jog, current_position.y, current_position.z, current_position.e, feedrate_mm_s, 0); - x_jog += .1; - } - return; - } - switch (dual_x_carriage_mode) { + case DXC_FULL_CONTROL_MODE: case DXC_AUTO_PARK_MODE: break; + case DXC_DUPLICATION_MODE: // Set the X offset, but no less than the safety gap if (parser.seen('X')) duplicate_extruder_x_offset = _MAX(parser.value_linear_units(), (X2_MIN_POS) - (X1_MIN_POS)); @@ -97,15 +83,34 @@ // Always switch back to tool 0 if (active_extruder != 0) tool_change(0); break; + + case DXC_MIRRORED_MODE: { + if (previous_mode != DXC_DUPLICATION_MODE) { + SERIAL_ECHOLNPGM("Printer must be in DXC_DUPLICATION_MODE prior to "); + SERIAL_ECHOLNPGM("specifying DXC_MIRRORED_MODE."); + dual_x_carriage_mode = DEFAULT_DUAL_X_CARRIAGE_MODE; + return; + } + idex_set_mirrored_mode(true); + + // Do a small 'jog' motion in the X axis + xyze_pos_t dest = current_position; dest.x -= 0.1f; + for (uint8_t i = 2; --i;) { + planner.buffer_line(dest, feedrate_mm_s, 0); + dest.x += 0.1f; + } + } return; + default: dual_x_carriage_mode = DEFAULT_DUAL_X_CARRIAGE_MODE; break; } + idex_set_parked(false); set_duplication_enabled(false); #ifdef EVENT_GCODE_IDEX_AFTER_MODECHANGE - gcode.process_subcommands_now_P(PSTR(EVENT_GCODE_IDEX_AFTER_MODECHANGE)); + process_subcommands_now(F(EVENT_GCODE_IDEX_AFTER_MODECHANGE)); #endif } else if (!parser.seen('W')) // if no S or W parameter, the DXC mode gets reset to the user's default @@ -122,26 +127,26 @@ case DXC_DUPLICATION_MODE: DEBUG_ECHOPGM("DUPLICATION"); break; case DXC_MIRRORED_MODE: DEBUG_ECHOPGM("MIRRORED"); break; } - DEBUG_ECHOPAIR("\nActive Ext: ", active_extruder); + DEBUG_ECHOPGM("\nActive Ext: ", active_extruder); if (!active_extruder_parked) DEBUG_ECHOPGM(" NOT "); DEBUG_ECHOPGM(" parked."); - DEBUG_ECHOPAIR("\nactive_extruder_x_pos: ", current_position.x); - DEBUG_ECHOPAIR("\ninactive_extruder_x: ", inactive_extruder_x); - DEBUG_ECHOPAIR("\nextruder_duplication_enabled: ", extruder_duplication_enabled); - DEBUG_ECHOPAIR("\nduplicate_extruder_x_offset: ", duplicate_extruder_x_offset); - DEBUG_ECHOPAIR("\nduplicate_extruder_temp_offset: ", duplicate_extruder_temp_offset); - DEBUG_ECHOPAIR("\ndelayed_move_time: ", delayed_move_time); - DEBUG_ECHOPAIR("\nX1 Home X: ", x_home_pos(0), "\nX1_MIN_POS=", X1_MIN_POS, "\nX1_MAX_POS=", X1_MAX_POS); - DEBUG_ECHOPAIR("\nX2 Home X: ", x_home_pos(1), "\nX2_MIN_POS=", X2_MIN_POS, "\nX2_MAX_POS=", X2_MAX_POS); - DEBUG_ECHOPAIR("\nX2_HOME_DIR=", X2_HOME_DIR, "\nX2_HOME_POS=", X2_HOME_POS); - DEBUG_ECHOPAIR("\nDEFAULT_DUAL_X_CARRIAGE_MODE=", STRINGIFY(DEFAULT_DUAL_X_CARRIAGE_MODE)); - DEBUG_ECHOPAIR("\toolchange_settings.z_raise=", toolchange_settings.z_raise); - DEBUG_ECHOPAIR("\nDEFAULT_DUPLICATION_X_OFFSET=", DEFAULT_DUPLICATION_X_OFFSET); + DEBUG_ECHOPGM("\nactive_extruder_x_pos: ", current_position.x); + DEBUG_ECHOPGM("\ninactive_extruder_x: ", inactive_extruder_x); + DEBUG_ECHOPGM("\nextruder_duplication_enabled: ", extruder_duplication_enabled); + DEBUG_ECHOPGM("\nduplicate_extruder_x_offset: ", duplicate_extruder_x_offset); + DEBUG_ECHOPGM("\nduplicate_extruder_temp_offset: ", duplicate_extruder_temp_offset); + DEBUG_ECHOPGM("\ndelayed_move_time: ", delayed_move_time); + DEBUG_ECHOPGM("\nX1 Home X: ", x_home_pos(0), "\nX1_MIN_POS=", X1_MIN_POS, "\nX1_MAX_POS=", X1_MAX_POS); + DEBUG_ECHOPGM("\nX2 Home X: ", x_home_pos(1), "\nX2_MIN_POS=", X2_MIN_POS, "\nX2_MAX_POS=", X2_MAX_POS); + DEBUG_ECHOPGM("\nX2_HOME_DIR=", X2_HOME_DIR, "\nX2_HOME_POS=", X2_HOME_POS); + DEBUG_ECHOPGM("\nDEFAULT_DUAL_X_CARRIAGE_MODE=", STRINGIFY(DEFAULT_DUAL_X_CARRIAGE_MODE)); + DEBUG_ECHOPGM("\toolchange_settings.z_raise=", toolchange_settings.z_raise); + DEBUG_ECHOPGM("\nDEFAULT_DUPLICATION_X_OFFSET=", DEFAULT_DUPLICATION_X_OFFSET); DEBUG_EOL(); HOTEND_LOOP() { - DEBUG_ECHOPAIR_P(SP_T_STR, e); - LOOP_XYZ(a) DEBUG_ECHOPAIR(" hotend_offset[", e, "].", XYZ_CHAR(a) | 0x20, "=", hotend_offset[e][a]); + DEBUG_ECHOPGM_P(SP_T_STR, e); + LOOP_LINEAR_AXES(a) DEBUG_ECHOPGM(" hotend_offset[", e, "].", AS_CHAR(AXIS_CHAR(a) | 0x20), "=", hotend_offset[e][a]); DEBUG_EOL(); } DEBUG_EOL(); diff --git a/Marlin/src/gcode/control/M7-M9.cpp b/Marlin/src/gcode/control/M7-M9.cpp index a33e43288b75..ccde4f552cb9 100644 --- a/Marlin/src/gcode/control/M7-M9.cpp +++ b/Marlin/src/gcode/control/M7-M9.cpp @@ -20,9 +20,9 @@ * */ -#include "../../inc/MarlinConfig.h" +#include "../../inc/MarlinConfigPre.h" -#if ENABLED(COOLANT_CONTROL) +#if ANY(COOLANT_MIST, COOLANT_FLOOD, AIR_ASSIST) #include "../gcode.h" #include "../../module/planner.h" @@ -37,27 +37,41 @@ } #endif -#if ENABLED(COOLANT_FLOOD) +#if EITHER(COOLANT_FLOOD, AIR_ASSIST) + + #if ENABLED(AIR_ASSIST) + #include "../../feature/spindle_laser.h" + #endif + /** - * M8: Flood Coolant On + * M8: Flood Coolant / Air Assist ON */ void GcodeSuite::M8() { - planner.synchronize(); // Wait for move to arrive - WRITE(COOLANT_FLOOD_PIN, !(COOLANT_FLOOD_INVERT)); // Turn on Flood coolant + planner.synchronize(); // Wait for move to arrive + #if ENABLED(COOLANT_FLOOD) + WRITE(COOLANT_FLOOD_PIN, !(COOLANT_FLOOD_INVERT)); // Turn on Flood coolant + #endif + #if ENABLED(AIR_ASSIST) + cutter.air_assist_enable(); // Turn on Air Assist + #endif } + #endif /** - * M9: Coolant OFF + * M9: Coolant / Air Assist OFF */ void GcodeSuite::M9() { - planner.synchronize(); // Wait for move to arrive + planner.synchronize(); // Wait for move to arrive #if ENABLED(COOLANT_MIST) - WRITE(COOLANT_MIST_PIN, COOLANT_MIST_INVERT); // Turn off Mist coolant + WRITE(COOLANT_MIST_PIN, COOLANT_MIST_INVERT); // Turn off Mist coolant #endif #if ENABLED(COOLANT_FLOOD) - WRITE(COOLANT_FLOOD_PIN, COOLANT_FLOOD_INVERT); // Turn off Flood coolant + WRITE(COOLANT_FLOOD_PIN, COOLANT_FLOOD_INVERT); // Turn off Flood coolant + #endif + #if ENABLED(AIR_ASSIST) + cutter.air_assist_disable(); // Turn off Air Assist #endif } -#endif // COOLANT_CONTROL +#endif // COOLANT_MIST | COOLANT_FLOOD | AIR_ASSIST diff --git a/Marlin/src/gcode/control/M80_M81.cpp b/Marlin/src/gcode/control/M80_M81.cpp index 1b5ea2f7eff0..90b25e7ed34d 100644 --- a/Marlin/src/gcode/control/M80_M81.cpp +++ b/Marlin/src/gcode/control/M80_M81.cpp @@ -25,29 +25,21 @@ #include "../../module/temperature.h" #include "../../module/planner.h" // for planner.finish_and_disable #include "../../module/printcounter.h" // for print_job_timer.stop -#include "../../lcd/marlinui.h" // for LCD_MESSAGEPGM_P +#include "../../lcd/marlinui.h" // for LCD_MESSAGE_F #include "../../inc/MarlinConfig.h" +#if ENABLED(PSU_CONTROL) + #include "../queue.h" + #include "../../feature/power.h" +#endif + #if HAS_SUICIDE #include "../../MarlinCore.h" #endif #if ENABLED(PSU_CONTROL) - #if ENABLED(AUTO_POWER_CONTROL) - #include "../../feature/power.h" - #else - void restore_stepper_drivers(); - #endif - - // Could be moved to a feature, but this is all the data - bool powersupply_on; - - #if HAS_TRINAMIC_CONFIG - #include "../../feature/tmc_util.h" - #endif - /** * M80 : Turn on the Power Supply * M80 S : Report the current state and exit @@ -56,11 +48,11 @@ // S: Report the current power supply state and exit if (parser.seen('S')) { - SERIAL_ECHOPGM_P(powersupply_on ? PSTR("PS:1\n") : PSTR("PS:0\n")); + SERIAL_ECHOF(powerManager.psu_on ? F("PS:1\n") : F("PS:0\n")); return; } - PSU_ON(); + powerManager.power_on(); /** * If you have a switch on suicide pin, this is useful @@ -68,16 +60,10 @@ * a print without suicide... */ #if HAS_SUICIDE - OUT_WRITE(SUICIDE_PIN, !SUICIDE_PIN_INVERTING); + OUT_WRITE(SUICIDE_PIN, !SUICIDE_PIN_STATE); #endif - #if DISABLED(AUTO_POWER_CONTROL) - safe_delay(PSU_POWERUP_DELAY); - restore_stepper_drivers(); - TERN_(HAS_TRINAMIC_CONFIG, safe_delay(PSU_POWERUP_DELAY)); - #endif - - TERN_(HAS_LCD_MENU, ui.reset_status()); + TERN_(HAS_MARLINUI_MENU, ui.reset_status()); } #endif // PSU_CONTROL @@ -88,26 +74,47 @@ * This code should ALWAYS be available for FULL SHUTDOWN! */ void GcodeSuite::M81() { - thermalManager.disable_all_heaters(); planner.finish_and_disable(); + thermalManager.cooldown(); print_job_timer.stop(); - #if HAS_FAN - thermalManager.zero_fan_speeds(); - #if ENABLED(PROBING_FANS_OFF) - thermalManager.fans_paused = false; - ZERO(thermalManager.saved_fan_speed); - #endif + #if BOTH(HAS_FAN, PROBING_FANS_OFF) + thermalManager.fans_paused = false; + ZERO(thermalManager.saved_fan_speed); #endif safe_delay(1000); // Wait 1 second before switching off + LCD_MESSAGE_F(MACHINE_NAME " " STR_OFF "."); + + bool delayed_power_off = false; + + #if ENABLED(POWER_OFF_TIMER) + if (parser.seenval('D')) { + uint16_t delay = parser.value_ushort(); + if (delay > 1) { // skip already observed 1s delay + delayed_power_off = true; + powerManager.setPowerOffTimer(SEC_TO_MS(delay - 1)); + } + } + #endif + + #if ENABLED(POWER_OFF_WAIT_FOR_COOLDOWN) + if (parser.boolval('S')) { + delayed_power_off = true; + powerManager.setPowerOffOnCooldown(true); + } + #endif + + if (delayed_power_off) { + SERIAL_ECHOLNPGM(STR_DELAYED_POWEROFF); + return; + } + #if HAS_SUICIDE suicide(); #elif ENABLED(PSU_CONTROL) - PSU_OFF_SOON(); + powerManager.power_off_soon(); #endif - - LCD_MESSAGEPGM_P(PSTR(MACHINE_NAME " " STR_OFF ".")); } diff --git a/Marlin/src/gcode/control/M993_M994.cpp b/Marlin/src/gcode/control/M993_M994.cpp index ff9ff85bf5bd..252792e5224e 100644 --- a/Marlin/src/gcode/control/M993_M994.cpp +++ b/Marlin/src/gcode/control/M993_M994.cpp @@ -37,7 +37,7 @@ void GcodeSuite::M993() { char fname[] = "spiflash.bin"; card.openFileWrite(fname); if (!card.isFileOpen()) { - SERIAL_ECHOLNPAIR("Failed to open ", fname, " to write."); + SERIAL_ECHOLNPGM("Failed to open ", fname, " to write."); return; } @@ -65,7 +65,7 @@ void GcodeSuite::M994() { char fname[] = "spiflash.bin"; card.openFileRead(fname); if (!card.isFileOpen()) { - SERIAL_ECHOLNPAIR("Failed to open ", fname, " to read."); + SERIAL_ECHOLNPGM("Failed to open ", fname, " to read."); return; } diff --git a/Marlin/src/gcode/control/M997.cpp b/Marlin/src/gcode/control/M997.cpp index cdff96f1acd8..359172faad7c 100644 --- a/Marlin/src/gcode/control/M997.cpp +++ b/Marlin/src/gcode/control/M997.cpp @@ -24,11 +24,17 @@ #if ENABLED(PLATFORM_M997_SUPPORT) +#if ENABLED(DWIN_CREALITY_LCD_ENHANCED) + #include "../../lcd/e3v2/proui/dwin.h" +#endif + /** * M997: Perform in-application firmware update */ void GcodeSuite::M997() { + TERN_(DWIN_CREALITY_LCD_ENHANCED, DWIN_RebootScreen()); + flashFirmware(parser.intval('S')); } diff --git a/Marlin/src/gcode/control/M999.cpp b/Marlin/src/gcode/control/M999.cpp index b7219673a3d6..b7d6db9f2312 100644 --- a/Marlin/src/gcode/control/M999.cpp +++ b/Marlin/src/gcode/control/M999.cpp @@ -22,7 +22,7 @@ #include "../gcode.h" -#include "../../lcd/marlinui.h" // for lcd_reset_alert_level +#include "../../lcd/marlinui.h" // for ui.reset_alert_level #include "../../MarlinCore.h" // for marlin_state #include "../queue.h" // for flush_and_request_resend diff --git a/Marlin/src/gcode/control/T.cpp b/Marlin/src/gcode/control/T.cpp index 3ce284f82e86..5e8f6b5436d4 100644 --- a/Marlin/src/gcode/control/T.cpp +++ b/Marlin/src/gcode/control/T.cpp @@ -40,7 +40,7 @@ * F[units/min] Set the movement feedrate * S1 Don't move the tool in XY after change * - * For PRUSA_MMU2(S) and SMUFF_EMU_MMU2(S) + * For PRUSA_MMU2(S) and EXTENDABLE_EMU_MMU2(S) * T[n] Gcode to extrude at least 38.10 mm at feedrate 19.02 mm/s must follow immediately to load to extruder wheels. * T? Gcode to extrude shouldn't have to follow. Load to extruder wheels is done automatically. * Tx Same as T?, but nozzle doesn't have to be preheated. Tc requires a preheated nozzle to finish filament load. @@ -49,7 +49,7 @@ void GcodeSuite::T(const int8_t tool_index) { DEBUG_SECTION(log_T, "T", DEBUGGING(LEVELING)); - if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPAIR("...(", tool_index, ")"); + if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPGM("...(", tool_index, ")"); // Count this command as movement / activity reset_stepper_timeout(); diff --git a/Marlin/src/gcode/eeprom/M500-M504.cpp b/Marlin/src/gcode/eeprom/M500-M504.cpp index cd7833c701bf..412d0033558f 100644 --- a/Marlin/src/gcode/eeprom/M500-M504.cpp +++ b/Marlin/src/gcode/eeprom/M500-M504.cpp @@ -25,6 +25,11 @@ #include "../../core/serial.h" #include "../../inc/MarlinConfig.h" +#if ENABLED(CONFIGURATION_EMBEDDING) + #include "../../sd/SdBaseFile.h" + #include "../../mczip.h" +#endif + /** * M500: Store settings in EEPROM */ @@ -50,9 +55,24 @@ void GcodeSuite::M502() { /** * M503: print settings currently in memory + * + * S : Include / exclude header comments in the output. (Default: S1) + * + * With CONFIGURATION_EMBEDDING: + * C : Save the full Marlin configuration to SD Card as "mc.zip" */ void GcodeSuite::M503() { (void)settings.report(!parser.boolval('S', true)); + + #if ENABLED(CONFIGURATION_EMBEDDING) + if (parser.seen_test('C')) { + SdBaseFile file; + const uint16_t size = sizeof(mc_zip); + // Need to create the config size on the SD card + if (file.open("mc.zip", O_WRITE|O_CREAT) && file.write(pgm_read_ptr(mc_zip), size) != -1 && file.close()) + SERIAL_ECHO_MSG("Configuration saved as 'mc.zip'"); + } + #endif } #endif // !DISABLE_M503 @@ -75,14 +95,14 @@ void GcodeSuite::M502() { if (dowrite) { val = parser.byteval('V'); persistentStore.write_data(addr, &val); - SERIAL_ECHOLNPAIR("Wrote address ", addr, " with ", val); + SERIAL_ECHOLNPGM("Wrote address ", addr, " with ", val); } else { if (parser.seenval('T')) { const int endaddr = parser.value_ushort(); while (addr <= endaddr) { persistentStore.read_data(addr, &val); - SERIAL_ECHOLNPAIR("0x", hex_word(addr), ":", hex_byte(val)); + SERIAL_ECHOLNPGM("0x", hex_word(addr), ":", hex_byte(val)); addr++; safe_delay(10); } @@ -90,7 +110,7 @@ void GcodeSuite::M502() { } else { persistentStore.read_data(addr, &val); - SERIAL_ECHOLNPAIR("Read address ", addr, " and got ", val); + SERIAL_ECHOLNPGM("Read address ", addr, " and got ", val); } } return; diff --git a/Marlin/src/gcode/feature/L6470/M122.cpp b/Marlin/src/gcode/feature/L6470/M122.cpp index cfac42764278..4a5629b04920 100644 --- a/Marlin/src/gcode/feature/L6470/M122.cpp +++ b/Marlin/src/gcode/feature/L6470/M122.cpp @@ -47,10 +47,10 @@ inline void L6470_say_status(const L64XX_axis_t axis) { } #endif SERIAL_ECHOPGM("\n...OUTPUT: "); - SERIAL_ECHOPGM_P(sh.STATUS_AXIS & STATUS_HIZ ? PSTR("OFF") : PSTR("ON ")); + SERIAL_ECHOF(sh.STATUS_AXIS & STATUS_HIZ ? F("OFF") : F("ON ")); SERIAL_ECHOPGM(" BUSY: "); echo_yes_no((sh.STATUS_AXIS & STATUS_BUSY) == 0); SERIAL_ECHOPGM(" DIR: "); - SERIAL_ECHOPGM_P((((sh.STATUS_AXIS & STATUS_DIR) >> 4) ^ L64xxManager.index_to_dir[axis]) ? PSTR("FORWARD") : PSTR("REVERSE")); + SERIAL_ECHOF((((sh.STATUS_AXIS & STATUS_DIR) >> 4) ^ L64xxManager.index_to_dir[axis]) ? F("FORWARD") : F("REVERSE")); if (sh.STATUS_AXIS_LAYOUT == L6480_STATUS_LAYOUT) { SERIAL_ECHOPGM(" Last Command: "); if (sh.STATUS_AXIS & sh.STATUS_AXIS_WRONG_CMD) SERIAL_ECHOPGM("VALID"); @@ -67,8 +67,8 @@ inline void L6470_say_status(const L64XX_axis_t axis) { SERIAL_ECHOPGM(" Last Command: "); if (!(sh.STATUS_AXIS & sh.STATUS_AXIS_WRONG_CMD)) SERIAL_ECHOPGM("IN"); SERIAL_ECHOPGM("VALID "); - SERIAL_ECHOPGM_P(sh.STATUS_AXIS & sh.STATUS_AXIS_NOTPERF_CMD ? PSTR("COMPLETED ") : PSTR("Not PERFORMED")); - SERIAL_ECHOPAIR("\n...THERMAL: ", !(sh.STATUS_AXIS & sh.STATUS_AXIS_TH_SD) ? "SHUTDOWN " : !(sh.STATUS_AXIS & sh.STATUS_AXIS_TH_WRN) ? "WARNING " : "OK "); + SERIAL_ECHOF(sh.STATUS_AXIS & sh.STATUS_AXIS_NOTPERF_CMD ? F("COMPLETED ") : F("Not PERFORMED")); + SERIAL_ECHOPGM("\n...THERMAL: ", !(sh.STATUS_AXIS & sh.STATUS_AXIS_TH_SD) ? "SHUTDOWN " : !(sh.STATUS_AXIS & sh.STATUS_AXIS_TH_WRN) ? "WARNING " : "OK "); } SERIAL_ECHOPGM(" OVERCURRENT:"); echo_yes_no((sh.STATUS_AXIS & sh.STATUS_AXIS_OCD) == 0); if (sh.STATUS_AXIS_LAYOUT != L6474_STATUS_LAYOUT) { diff --git a/Marlin/src/gcode/feature/L6470/M906.cpp b/Marlin/src/gcode/feature/L6470/M906.cpp index 3638fae45b5c..f55405b798ad 100644 --- a/Marlin/src/gcode/feature/L6470/M906.cpp +++ b/Marlin/src/gcode/feature/L6470/M906.cpp @@ -24,6 +24,10 @@ #if HAS_L64XX +#if AXIS_COLLISION('I') + #error "M906 parameter collision with axis name." +#endif + #include "../../gcode.h" #include "../../../libs/L64XX/L64XX_Marlin.h" #include "../../../module/stepper/indirection.h" @@ -32,31 +36,6 @@ #define DEBUG_OUT ENABLED(L6470_CHITCHAT) #include "../../../core/debug_out.h" -/** - * M906: report or set KVAL_HOLD which sets the maximum effective voltage provided by the - * PWMs to the steppers - * - * On L6474 this sets the TVAL register (same address). - * - * I - select which driver(s) to change on multi-driver axis - * 0 - (default) all drivers on the axis or E0 - * 1 - monitor only X, Y, Z or E1 - * 2 - monitor only X2, Y2, Z2 or E2 - * 3 - monitor only Z3 or E3 - * 4 - monitor only Z4 or E4 - * 5 - monitor only E5 - * Xxxx, Yxxx, Zxxx, Exxx - axis to change (optional) - * L6474 - current in mA (4A max) - * All others - 0-255 - */ - -/** - * Sets KVAL_HOLD wich affects the current being driven through the stepper. - * - * L6470 is used in the STEP-CLOCK mode. KVAL_HOLD is the only KVAL_xxx - * that affects the effective voltage seen by the stepper. - */ - /** * MACRO to fetch information on the items associated with current limiting * and maximum voltage output. @@ -88,7 +67,7 @@ void L64XX_report_current(L64XX &motor, const L64XX_axis_t axis) { #if ENABLED(L6470_CHITCHAT) char tmp[10]; sprintf_P(tmp, PSTR("%4x "), status); - DEBUG_ECHOPAIR(" status: ", tmp); + DEBUG_ECHOPGM(" status: ", tmp); print_bin(status); #else UNUSED(status); @@ -129,13 +108,13 @@ void L64XX_report_current(L64XX &motor, const L64XX_axis_t axis) { } SERIAL_EOL(); - SERIAL_ECHOPAIR("...MicroSteps: ", MicroSteps, + SERIAL_ECHOPGM("...MicroSteps: ", MicroSteps, " ADC_OUT: ", L6470_ADC_out); SERIAL_ECHOPGM(" Vs_compensation: "); - SERIAL_ECHOPGM_P((motor.GetParam(sh.L6470_AXIS_CONFIG) & CONFIG_EN_VSCOMP) ? PSTR("ENABLED ") : PSTR("DISABLED")); - SERIAL_ECHOLNPAIR(" Compensation coefficient: ~", comp_coef * 0.01f); + SERIAL_ECHOF((motor.GetParam(sh.L6470_AXIS_CONFIG) & CONFIG_EN_VSCOMP) ? F("ENABLED ") : F("DISABLED")); + SERIAL_ECHOLNPGM(" Compensation coefficient: ~", comp_coef * 0.01f); - SERIAL_ECHOPAIR("...KVAL_HOLD: ", motor.GetParam(L6470_KVAL_HOLD), + SERIAL_ECHOPGM("...KVAL_HOLD: ", motor.GetParam(L6470_KVAL_HOLD), " KVAL_RUN : ", motor.GetParam(L6470_KVAL_RUN), " KVAL_ACC: ", motor.GetParam(L6470_KVAL_ACC), " KVAL_DEC: ", motor.GetParam(L6470_KVAL_DEC), @@ -193,7 +172,7 @@ void L64XX_report_current(L64XX &motor, const L64XX_axis_t axis) { SERIAL_ECHOLNPGM(" mA) Motor Status: NA"); const uint16_t MicroSteps = _BV(motor.GetParam(L6470_STEP_MODE) & 0x07); //NOMORE(MicroSteps, 16); - SERIAL_ECHOPAIR("...MicroSteps: ", MicroSteps, + SERIAL_ECHOPGM("...MicroSteps: ", MicroSteps, " ADC_OUT: ", L6470_ADC_out); SERIAL_ECHOLNPGM(" Vs_compensation: NA\n"); @@ -210,7 +189,7 @@ void L64XX_report_current(L64XX &motor, const L64XX_axis_t axis) { case 1: DEBUG_ECHOLNPGM("75V/uS") ; break; case 2: DEBUG_ECHOLNPGM("110V/uS") ; break; case 3: DEBUG_ECHOLNPGM("260V/uS") ; break; - default: DEBUG_ECHOLNPAIR("slew rate: ", (motor.GetParam(sh.L6470_AXIS_CONFIG) & CONFIG_POW_SR) >> CONFIG_POW_SR_BIT); break; + default: DEBUG_ECHOLNPGM("slew rate: ", (motor.GetParam(sh.L6470_AXIS_CONFIG) & CONFIG_POW_SR) >> CONFIG_POW_SR_BIT); break; } #endif SERIAL_EOL(); @@ -220,6 +199,27 @@ void L64XX_report_current(L64XX &motor, const L64XX_axis_t axis) { } } +/** + * M906: report or set KVAL_HOLD which sets the maximum effective voltage provided by the + * PWMs to the steppers + * + * On L6474 this sets the TVAL register (same address). + * + * I - select which driver(s) to change on multi-driver axis + * (default) all drivers on the axis + * 0 - monitor only the first XYZ... driver + * 1 - monitor only X2, Y2, Z2 + * 2 - monitor only Z3 + * 3 - monitor only Z4 + * Xxxx, Yxxx, Zxxx, Exxx - axis to change (optional) + * L6474 - current in mA (4A max) + * All others - 0-255 + * + * Sets KVAL_HOLD which affects the current being driven through the stepper. + * + * L6470 is used in the STEP-CLOCK mode. KVAL_HOLD is the only KVAL_xxx + * that affects the effective voltage seen by the stepper. + */ void GcodeSuite::M906() { L64xxManager.pause_monitor(true); // Keep monitor_driver() from stealing status @@ -230,11 +230,13 @@ void GcodeSuite::M906() { uint8_t report_current = true; - #if HAS_L64XX - const uint8_t index = parser.byteval('I'); + #if AXIS_IS_L64XX(X2) || AXIS_IS_L64XX(Y2) || AXIS_IS_L64XX(Z2) || AXIS_IS_L64XX(Z3) || AXIS_IS_L64XX(Z4) + const int8_t index = parser.byteval('I', -1); + #else + constexpr int8_t index = -1; #endif - LOOP_XYZE(i) if (uint16_t value = parser.intval(axis_codes[i])) { + LOOP_LOGICAL_AXES(i) if (uint16_t value = parser.intval(axis_codes[i])) { report_current = false; @@ -244,66 +246,74 @@ void GcodeSuite::M906() { } switch (i) { - case X_AXIS: - #if AXIS_IS_L64XX(X) - if (index == 0) L6470_SET_KVAL_HOLD(X); - #endif - #if AXIS_IS_L64XX(X2) - if (index == 1) L6470_SET_KVAL_HOLD(X2); - #endif - break; - case Y_AXIS: - #if AXIS_IS_L64XX(Y) - if (index == 0) L6470_SET_KVAL_HOLD(Y); - #endif - #if AXIS_IS_L64XX(Y2) - if (index == 1) L6470_SET_KVAL_HOLD(Y2); - #endif - break; - case Z_AXIS: - #if AXIS_IS_L64XX(Z) - if (index == 0) L6470_SET_KVAL_HOLD(Z); - #endif - #if AXIS_IS_L64XX(Z2) - if (index == 1) L6470_SET_KVAL_HOLD(Z2); - #endif - #if AXIS_IS_L64XX(Z3) - if (index == 2) L6470_SET_KVAL_HOLD(Z3); - #endif - #if AXIS_DRIVER_TYPE_Z4(L6470) - if (index == 3) L6470_SET_KVAL_HOLD(Z4); - #endif - break; - case E_AXIS: { - const int8_t target_extruder = get_target_extruder_from_command(); - if (target_extruder < 0) return; - switch (target_extruder) { + #if AXIS_IS_L64XX(X) || AXIS_IS_L64XX(X2) + case X_AXIS: + #if AXIS_IS_L64XX(X) + if (index < 0 || index == 0) L6470_SET_KVAL_HOLD(X); + #endif + #if AXIS_IS_L64XX(X2) + if (index < 0 || index == 1) L6470_SET_KVAL_HOLD(X2); + #endif + break; + #endif + + #if AXIS_IS_L64XX(Y) || AXIS_IS_L64XX(Y2) + case Y_AXIS: + #if AXIS_IS_L64XX(Y) + if (index < 0 || index == 0) L6470_SET_KVAL_HOLD(Y); + #endif + #if AXIS_IS_L64XX(Y2) + if (index < 0 || index == 1) L6470_SET_KVAL_HOLD(Y2); + #endif + break; + #endif + + #if AXIS_IS_L64XX(Z) || AXIS_IS_L64XX(Z2) || AXIS_IS_L64XX(Z3) || AXIS_IS_L64XX(Z4) + case Z_AXIS: + #if AXIS_IS_L64XX(Z) + if (index < 0 || index == 0) L6470_SET_KVAL_HOLD(Z); + #endif + #if AXIS_IS_L64XX(Z2) + if (index < 0 || index == 1) L6470_SET_KVAL_HOLD(Z2); + #endif + #if AXIS_IS_L64XX(Z3) + if (index < 0 || index == 2) L6470_SET_KVAL_HOLD(Z3); + #endif + #if AXIS_IS_L64XX(Z4) + if (index < 0 || index == 3) L6470_SET_KVAL_HOLD(Z4); + #endif + break; + #endif + + #if AXIS_IS_L64XX(E0) || AXIS_IS_L64XX(E1) || AXIS_IS_L64XX(E2) || AXIS_IS_L64XX(E3) || AXIS_IS_L64XX(E4) || AXIS_IS_L64XX(E5) || AXIS_IS_L64XX(E6) || AXIS_IS_L64XX(E7) + case E_AXIS: { + const int8_t eindex = get_target_e_stepper_from_command(-2); #if AXIS_IS_L64XX(E0) - case 0: L6470_SET_KVAL_HOLD(E0); break; + if (eindex < 0 || eindex == 0) L6470_SET_KVAL_HOLD(E0); #endif #if AXIS_IS_L64XX(E1) - case 1: L6470_SET_KVAL_HOLD(E1); break; + if (eindex < 0 || eindex == 1) L6470_SET_KVAL_HOLD(E1); #endif #if AXIS_IS_L64XX(E2) - case 2: L6470_SET_KVAL_HOLD(E2); break; + if (eindex < 0 || eindex == 2) L6470_SET_KVAL_HOLD(E2); #endif #if AXIS_IS_L64XX(E3) - case 3: L6470_SET_KVAL_HOLD(E3); break; + if (eindex < 0 || eindex == 3) L6470_SET_KVAL_HOLD(E3); #endif #if AXIS_IS_L64XX(E4) - case 4: L6470_SET_KVAL_HOLD(E4); break; + if (eindex < 0 || eindex == 4) L6470_SET_KVAL_HOLD(E4); #endif #if AXIS_IS_L64XX(E5) - case 5: L6470_SET_KVAL_HOLD(E5); break; + if (eindex < 0 || eindex == 5) L6470_SET_KVAL_HOLD(E5); #endif #if AXIS_IS_L64XX(E6) - case 6: L6470_SET_KVAL_HOLD(E6); break; + if (eindex < 0 || eindex == 6) L6470_SET_KVAL_HOLD(E6); #endif #if AXIS_IS_L64XX(E7) - case 7: L6470_SET_KVAL_HOLD(E7); break; + if (eindex < 0 || eindex == 7) L6470_SET_KVAL_HOLD(E7); #endif - } - } break; + } break; + #endif } } diff --git a/Marlin/src/gcode/feature/L6470/M916-918.cpp b/Marlin/src/gcode/feature/L6470/M916-M918.cpp similarity index 94% rename from Marlin/src/gcode/feature/L6470/M916-918.cpp rename to Marlin/src/gcode/feature/L6470/M916-M918.cpp index 8a1ea4830606..8d614603eda0 100644 --- a/Marlin/src/gcode/feature/L6470/M916-918.cpp +++ b/Marlin/src/gcode/feature/L6470/M916-M918.cpp @@ -96,7 +96,7 @@ void GcodeSuite::M916() { if (L64xxManager.get_user_input(driver_count, axis_index, axis_mon, position_max, position_min, final_feedrate, kval_hold, over_current_flag, OCD_TH_val, STALL_TH_val, over_current_threshold)) return; // quit if invalid user input - DEBUG_ECHOLNPAIR("feedrate = ", final_feedrate); + DEBUG_ECHOLNPGM("feedrate = ", final_feedrate); planner.synchronize(); // wait for all current movement commands to complete @@ -127,9 +127,9 @@ void GcodeSuite::M916() { do { if (sh.STATUS_AXIS_LAYOUT == L6474_STATUS_LAYOUT) - DEBUG_ECHOLNPAIR("TVAL current (mA) = ", (M91x_counter + 1) * sh.AXIS_STALL_CURRENT_CONSTANT_INV); // report TVAL current for this run + DEBUG_ECHOLNPGM("TVAL current (mA) = ", (M91x_counter + 1) * sh.AXIS_STALL_CURRENT_CONSTANT_INV); // report TVAL current for this run else - DEBUG_ECHOLNPAIR("kval_hold = ", M91x_counter); // report KVAL_HOLD for this run + DEBUG_ECHOLNPGM("kval_hold = ", M91x_counter); // report KVAL_HOLD for this run for (j = 0; j < driver_count; j++) L64xxManager.set_param(axis_index[j], L6470_KVAL_HOLD, M91x_counter); //set KVAL_HOLD or TVAL (same register address) @@ -138,10 +138,10 @@ void GcodeSuite::M916() { do { // turn the motor(s) both directions sprintf_P(gcode_string, PSTR("G0 %s%03d F%03d"), temp_axis_string, uint16_t(position_min), uint16_t(final_feedrate)); - gcode.process_subcommands_now_P(gcode_string); + process_subcommands_now(gcode_string); sprintf_P(gcode_string, PSTR("G0 %s%03d F%03d"), temp_axis_string, uint16_t(position_max), uint16_t(final_feedrate)); - gcode.process_subcommands_now_P(gcode_string); + process_subcommands_now(gcode_string); // get the status after the motors have stopped planner.synchronize(); @@ -177,7 +177,7 @@ void GcodeSuite::M916() { if ((status_composite & (sh.STATUS_AXIS_TH_WRN | sh.STATUS_AXIS_TH_SD))) DEBUG_ECHOLNPGM(".\n.\nTest completed normally - Thermal warning/shutdown has occurred"); else if (status_composite) - DEBUG_ECHOLNPGM(".\n.\nTest completed abnormally - non-thermal error has occured"); + DEBUG_ECHOLNPGM(".\n.\nTest completed abnormally - non-thermal error has occurred"); else DEBUG_ECHOLNPGM(".\n.\nTest completed normally - Unable to get to thermal warning/shutdown"); @@ -236,7 +236,7 @@ void GcodeSuite::M917() { if (L64xxManager.get_user_input(driver_count, axis_index, axis_mon, position_max, position_min, final_feedrate, kval_hold, over_current_flag, OCD_TH_val, STALL_TH_val, over_current_threshold)) return; // quit if invalid user input - DEBUG_ECHOLNPAIR("feedrate = ", final_feedrate); + DEBUG_ECHOLNPGM("feedrate = ", final_feedrate); planner.synchronize(); // wait for all current movement commands to complete @@ -252,24 +252,24 @@ void GcodeSuite::M917() { // 2 - OCD finalized - decreasing STALL - exit when STALL warning happens // 3 - OCD finalized - increasing STALL - exit when STALL warning stop // 4 - all testing completed - DEBUG_ECHOPAIR(".\n.\n.\nover_current threshold : ", (OCD_TH_val + 1) * 375); // first status display - DEBUG_ECHOPAIR(" (OCD_TH: : ", OCD_TH_val); + DEBUG_ECHOPGM(".\n.\n.\nover_current threshold : ", (OCD_TH_val + 1) * 375); // first status display + DEBUG_ECHOPGM(" (OCD_TH: : ", OCD_TH_val); if (sh.STATUS_AXIS_LAYOUT != L6474_STATUS_LAYOUT) { - DEBUG_ECHOPAIR(") Stall threshold: ", (STALL_TH_val + 1) * 31.25); - DEBUG_ECHOPAIR(" (STALL_TH: ", STALL_TH_val); + DEBUG_ECHOPGM(") Stall threshold: ", (STALL_TH_val + 1) * 31.25); + DEBUG_ECHOPGM(" (STALL_TH: ", STALL_TH_val); } DEBUG_ECHOLNPGM(")"); do { - if (sh.STATUS_AXIS_LAYOUT != L6474_STATUS_LAYOUT) DEBUG_ECHOPAIR("STALL threshold : ", (STALL_TH_val + 1) * 31.25); - DEBUG_ECHOLNPAIR(" OCD threshold : ", (OCD_TH_val + 1) * 375); + if (sh.STATUS_AXIS_LAYOUT != L6474_STATUS_LAYOUT) DEBUG_ECHOPGM("STALL threshold : ", (STALL_TH_val + 1) * 31.25); + DEBUG_ECHOLNPGM(" OCD threshold : ", (OCD_TH_val + 1) * 375); sprintf_P(gcode_string, PSTR("G0 %s%03d F%03d"), temp_axis_string, uint16_t(position_min), uint16_t(final_feedrate)); - gcode.process_subcommands_now_P(gcode_string); + process_subcommands_now(gcode_string); sprintf_P(gcode_string, PSTR("G0 %s%03d F%03d"), temp_axis_string, uint16_t(position_max), uint16_t(final_feedrate)); - gcode.process_subcommands_now_P(gcode_string); + process_subcommands_now(gcode_string); planner.synchronize(); @@ -303,12 +303,12 @@ void GcodeSuite::M917() { if (!(k % 4)) { kval_hold *= 0.95; DEBUG_EOL(); - DEBUG_ECHOLNPAIR("Lowering KVAL_HOLD by about 5% to ", kval_hold); + DEBUG_ECHOLNPGM("Lowering KVAL_HOLD by about 5% to ", kval_hold); for (j = 0; j < driver_count; j++) L64xxManager.set_param(axis_index[j], L6470_KVAL_HOLD, kval_hold); } DEBUG_ECHOLNPGM("."); - gcode.reset_stepper_timeout(); // keep steppers powered + reset_stepper_timeout(); // keep steppers powered watchdog_refresh(); safe_delay(5000); status_composite_temp = 0; @@ -590,8 +590,8 @@ void GcodeSuite::M918() { } m_steps = L64xxManager.get_param(axis_index[0], L6470_STEP_MODE) & 0x07; // get microsteps - DEBUG_ECHOLNPAIR("Microsteps = ", _BV(m_steps)); - DEBUG_ECHOLNPAIR("target (maximum) feedrate = ", final_feedrate); + DEBUG_ECHOLNPGM("Microsteps = ", _BV(m_steps)); + DEBUG_ECHOLNPGM("target (maximum) feedrate = ", final_feedrate); const float feedrate_inc = final_feedrate / 10, // Start at 1/10 of max & go up by 1/10 per step fr_limit = final_feedrate * 0.99f; // Rounding-safe comparison value @@ -612,13 +612,13 @@ void GcodeSuite::M918() { do { current_feedrate += feedrate_inc; - DEBUG_ECHOLNPAIR("...feedrate = ", current_feedrate); + DEBUG_ECHOLNPGM("...feedrate = ", current_feedrate); sprintf_P(gcode_string, PSTR("G0 %s%03d F%03d"), temp_axis_string, uint16_t(position_min), uint16_t(current_feedrate)); - gcode.process_subcommands_now_P(gcode_string); + process_subcommands_now(gcode_string); sprintf_P(gcode_string, PSTR("G0 %s%03d F%03d"), temp_axis_string, uint16_t(position_max), uint16_t(current_feedrate)); - gcode.process_subcommands_now_P(gcode_string); + process_subcommands_now(gcode_string); planner.synchronize(); diff --git a/Marlin/src/gcode/feature/adc/M3426.cpp b/Marlin/src/gcode/feature/adc/M3426.cpp new file mode 100644 index 000000000000..8205fa01f2bd --- /dev/null +++ b/Marlin/src/gcode/feature/adc/M3426.cpp @@ -0,0 +1,63 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2021 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + * + */ + +#include "../../../inc/MarlinConfig.h" + +#if ENABLED(HAS_MCP3426_ADC) + +#include "../../gcode.h" + +#include "../../../feature/adc/adc_mcp3426.h" + +/** + * M3426: Read 16 bit (signed) value from I2C MCP3426 ADC device + * + * M3426 C channel 1 or 2 + * M3426 G gain 1, 2, 4 or 8 + * M3426 I 0 or 1, invert reply + */ +void GcodeSuite::M3426() { + uint8_t channel = parser.byteval('C', 1), // Select the channel 1 or 2 + gain = parser.byteval('G', 1); + const bool inverted = parser.byteval('I') == 1; + + if (channel <= 2 && (gain == 1 || gain == 2 || gain == 4 || gain == 8)) { + int16_t result = mcp3426.ReadValue(channel, gain); + + if (mcp3426.Error == false) { + if (inverted) { + // Should we invert the reading (32767 - ADC value) ? + // Caters to end devices that expect values to increase when in reality they decrease. + // e.g., A pressure sensor in a vacuum when the end device expects a positive pressure. + result = INT16_MAX - result; + } + //SERIAL_ECHOPGM(STR_OK); + SERIAL_ECHOLNPGM("V:", result, " C:", channel, " G:", gain, " I:", inverted ? 1 : 0); + } + else + SERIAL_ERROR_MSG("MCP342X i2c error"); + } + else + SERIAL_ERROR_MSG("MCP342X Bad request"); +} + +#endif // HAS_MCP3426_ADC diff --git a/Marlin/src/gcode/feature/advance/M900.cpp b/Marlin/src/gcode/feature/advance/M900.cpp index 1d76ebf7d531..054ea3617f9b 100644 --- a/Marlin/src/gcode/feature/advance/M900.cpp +++ b/Marlin/src/gcode/feature/advance/M900.cpp @@ -115,11 +115,11 @@ void GcodeSuite::M900() { #if ENABLED(EXTRA_LIN_ADVANCE_K) #if EXTRUDERS < 2 - SERIAL_ECHOLNPAIR("Advance S", new_slot, " K", kref, "(S", !new_slot, " K", lref, ")"); + SERIAL_ECHOLNPGM("Advance S", new_slot, " K", kref, "(S", !new_slot, " K", lref, ")"); #else LOOP_L_N(i, EXTRUDERS) { const bool slot = TEST(lin_adv_slot, i); - SERIAL_ECHOLNPAIR("Advance T", i, " S", slot, " K", planner.extruder_advance_K[i], + SERIAL_ECHOLNPGM("Advance T", i, " S", slot, " K", planner.extruder_advance_K[i], "(S", !slot, " K", other_extruder_advance_K[i], ")"); SERIAL_EOL(); } @@ -129,7 +129,7 @@ void GcodeSuite::M900() { SERIAL_ECHO_START(); #if EXTRUDERS < 2 - SERIAL_ECHOLNPAIR("Advance K=", planner.extruder_advance_K[0]); + SERIAL_ECHOLNPGM("Advance K=", planner.extruder_advance_K[0]); #else SERIAL_ECHOPGM("Advance K"); LOOP_L_N(i, EXTRUDERS) { @@ -144,4 +144,17 @@ void GcodeSuite::M900() { } +void GcodeSuite::M900_report(const bool forReplay/*=true*/) { + report_heading(forReplay, F(STR_LINEAR_ADVANCE)); + #if EXTRUDERS < 2 + report_echo_start(forReplay); + SERIAL_ECHOLNPGM(" M900 K", planner.extruder_advance_K[0]); + #else + LOOP_L_N(i, EXTRUDERS) { + report_echo_start(forReplay); + SERIAL_ECHOLNPGM(" M900 T", i, " K", planner.extruder_advance_K[i]); + } + #endif +} + #endif // LIN_ADVANCE diff --git a/Marlin/src/gcode/feature/camera/M240.cpp b/Marlin/src/gcode/feature/camera/M240.cpp index fc350d8a558c..19051ffd423e 100644 --- a/Marlin/src/gcode/feature/camera/M240.cpp +++ b/Marlin/src/gcode/feature/camera/M240.cpp @@ -47,7 +47,7 @@ #endif #ifdef PHOTO_RETRACT_MM - inline void e_move_m240(const float length, const feedRate_t &fr_mm_s) { + inline void e_move_m240(const float length, const_feedRate_t fr_mm_s) { if (length && thermalManager.hotEnoughToExtrude(active_extruder)) unscaled_e_move(length, fr_mm_s); } @@ -135,17 +135,8 @@ void GcodeSuite::M240() { }; #ifdef PHOTO_RETRACT_MM - const float rval = parser.seenval('R') ? parser.value_linear_units() : _PHOTO_RETRACT_MM; - feedRate_t sval = ( - #if ENABLED(ADVANCED_PAUSE_FEATURE) - PAUSE_PARK_RETRACT_FEEDRATE - #elif ENABLED(FWRETRACT) - RETRACT_FEEDRATE - #else - 45 - #endif - ); - if (parser.seenval('S')) sval = parser.value_feedrate(); + const float rval = parser.linearval('R', _PHOTO_RETRACT_MM); + const feedRate_t sval = parser.feedrateval('S', TERN(ADVANCED_PAUSE_FEATURE, PAUSE_PARK_RETRACT_FEEDRATE, TERN(FWRETRACT, RETRACT_FEEDRATE, 45))); e_move_m240(-rval, sval); #endif diff --git a/Marlin/src/gcode/feature/caselight/M355.cpp b/Marlin/src/gcode/feature/caselight/M355.cpp index b0d94e7cd88a..b3b863f02e1f 100644 --- a/Marlin/src/gcode/feature/caselight/M355.cpp +++ b/Marlin/src/gcode/feature/caselight/M355.cpp @@ -61,7 +61,7 @@ void GcodeSuite::M355() { SERIAL_ECHOLNPGM(STR_OFF); else { #if CASELIGHT_USES_BRIGHTNESS - if (TERN(CASE_LIGHT_USE_NEOPIXEL, true, PWM_PIN(CASE_LIGHT_PIN))) { + if (TERN(CASE_LIGHT_USE_NEOPIXEL, true, TERN0(NEED_CASE_LIGHT_PIN, PWM_PIN(CASE_LIGHT_PIN)))) { SERIAL_ECHOLN(int(caselight.brightness)); return; } diff --git a/Marlin/src/gcode/feature/clean/G12.cpp b/Marlin/src/gcode/feature/clean/G12.cpp index 216db5bae374..a0b87b1abc9e 100644 --- a/Marlin/src/gcode/feature/clean/G12.cpp +++ b/Marlin/src/gcode/feature/clean/G12.cpp @@ -42,6 +42,7 @@ * P0 S : Stroke cleaning with S strokes * P1 Sn T : Zigzag cleaning with S repeats and T zigzags * P2 Sn R : Circle cleaning with S repeats and R radius + * X, Y, Z : Specify axes to move during cleaning. Default: ALL. */ void GcodeSuite::G12() { // Don't allow nozzle cleaning without homing first @@ -49,7 +50,7 @@ void GcodeSuite::G12() { #ifdef WIPE_SEQUENCE_COMMANDS if (!parser.seen_any()) { - gcode.process_subcommands_now_P(PSTR(WIPE_SEQUENCE_COMMANDS)); + process_subcommands_now(F(WIPE_SEQUENCE_COMMANDS)); return; } #endif diff --git a/Marlin/src/gcode/feature/controllerfan/M710.cpp b/Marlin/src/gcode/feature/controllerfan/M710.cpp index cc450732baed..b98d88845dec 100644 --- a/Marlin/src/gcode/feature/controllerfan/M710.cpp +++ b/Marlin/src/gcode/feature/controllerfan/M710.cpp @@ -27,18 +27,6 @@ #include "../../gcode.h" #include "../../../feature/controllerfan.h" -void M710_report(const bool forReplay) { - if (!forReplay) { SERIAL_ECHOLNPGM("; Controller Fan"); SERIAL_ECHO_START(); } - SERIAL_ECHOLNPAIR(" M710" - " S", int(controllerFan.settings.active_speed), - " I", int(controllerFan.settings.idle_speed), - " A", int(controllerFan.settings.auto_mode), - " D", controllerFan.settings.duration, - " ; (", (int(controllerFan.settings.active_speed) * 100) / 255, "%" - " ", (int(controllerFan.settings.idle_speed) * 100) / 255, "%)" - ); -} - /** * M710: Set controller fan settings * @@ -75,7 +63,19 @@ void GcodeSuite::M710() { if (seenD) controllerFan.settings.duration = parser.value_ushort(); if (!(seenR || seenS || seenI || seenA || seenD)) - M710_report(false); + M710_report(); +} + +void GcodeSuite::M710_report(const bool forReplay/*=true*/) { + report_heading_etc(forReplay, F(STR_CONTROLLER_FAN)); + SERIAL_ECHOLNPGM(" M710" + " S", int(controllerFan.settings.active_speed), + " I", int(controllerFan.settings.idle_speed), + " A", int(controllerFan.settings.auto_mode), + " D", controllerFan.settings.duration, + " ; (", (int(controllerFan.settings.active_speed) * 100) / 255, "%" + " ", (int(controllerFan.settings.idle_speed) * 100) / 255, "%)" + ); } #endif // CONTROLLER_FAN_EDITABLE diff --git a/Marlin/src/gcode/feature/digipot/M907-M910.cpp b/Marlin/src/gcode/feature/digipot/M907-M910.cpp index e46366620798..95adde3ea532 100644 --- a/Marlin/src/gcode/feature/digipot/M907-M910.cpp +++ b/Marlin/src/gcode/feature/digipot/M907-M910.cpp @@ -22,7 +22,7 @@ #include "../../../inc/MarlinConfig.h" -#if ANY(HAS_MOTOR_CURRENT_SPI, HAS_MOTOR_CURRENT_PWM, HAS_MOTOR_CURRENT_I2C, HAS_MOTOR_CURRENT_DAC) +#if HAS_MOTOR_CURRENT_SPI || HAS_MOTOR_CURRENT_PWM || HAS_MOTOR_CURRENT_I2C || HAS_MOTOR_CURRENT_DAC #include "../../gcode.h" @@ -34,22 +34,39 @@ #include "../../../feature/digipot/digipot.h" #endif -#if ENABLED(HAS_MOTOR_CURRENT_DAC) +#if HAS_MOTOR_CURRENT_DAC #include "../../../feature/dac/stepper_dac.h" #endif /** - * M907: Set digital trimpot motor current using axis codes X, Y, Z, E, B, S + * M907: Set digital trimpot motor current using axis codes X [Y] [Z] [E] + * B - Special case for 4th (E) axis + * S - Special case to set first 3 axes */ void GcodeSuite::M907() { #if HAS_MOTOR_CURRENT_SPI - LOOP_XYZE(i) if (parser.seenval(axis_codes[i])) stepper.set_digipot_current(i, parser.value_int()); + if (!parser.seen("BS" LOGICAL_AXES_STRING)) + return M907_report(); + + LOOP_LOGICAL_AXES(i) if (parser.seenval(axis_codes[i])) stepper.set_digipot_current(i, parser.value_int()); if (parser.seenval('B')) stepper.set_digipot_current(4, parser.value_int()); if (parser.seenval('S')) LOOP_LE_N(i, 4) stepper.set_digipot_current(i, parser.value_int()); #elif HAS_MOTOR_CURRENT_PWM + if (!parser.seen( + #if ANY_PIN(MOTOR_CURRENT_PWM_X, MOTOR_CURRENT_PWM_Y, MOTOR_CURRENT_PWM_XY) + "XY" + #endif + #if PIN_EXISTS(MOTOR_CURRENT_PWM_Z) + "Z" + #endif + #if PIN_EXISTS(MOTOR_CURRENT_PWM_E) + "E" + #endif + )) return M907_report(); + #if ANY_PIN(MOTOR_CURRENT_PWM_X, MOTOR_CURRENT_PWM_Y, MOTOR_CURRENT_PWM_XY) if (parser.seenval('X') || parser.seenval('Y')) stepper.set_digipot_current(0, parser.value_int()); #endif @@ -60,27 +77,52 @@ void GcodeSuite::M907() { if (parser.seenval('E')) stepper.set_digipot_current(2, parser.value_int()); #endif - #endif + #endif // HAS_MOTOR_CURRENT_PWM #if HAS_MOTOR_CURRENT_I2C // this one uses actual amps in floating point - LOOP_XYZE(i) if (parser.seenval(axis_codes[i])) digipot_i2c.set_current(i, parser.value_float()); + LOOP_LOGICAL_AXES(i) if (parser.seenval(axis_codes[i])) digipot_i2c.set_current(i, parser.value_float()); // Additional extruders use B,C,D for channels 4,5,6. // TODO: Change these parameters because 'E' is used. B? - for (uint8_t i = E_AXIS + 1; i < DIGIPOT_I2C_NUM_CHANNELS; i++) - if (parser.seenval('B' + i - (E_AXIS + 1))) digipot_i2c.set_current(i, parser.value_float()); + #if HAS_EXTRUDERS + for (uint8_t i = E_AXIS + 1; i < DIGIPOT_I2C_NUM_CHANNELS; i++) + if (parser.seenval('B' + i - (E_AXIS + 1))) digipot_i2c.set_current(i, parser.value_float()); + #endif #endif - #if ENABLED(HAS_MOTOR_CURRENT_DAC) + #if HAS_MOTOR_CURRENT_DAC if (parser.seenval('S')) { const float dac_percent = parser.value_float(); LOOP_LE_N(i, 4) stepper_dac.set_current_percent(i, dac_percent); } - LOOP_XYZE(i) if (parser.seenval(axis_codes[i])) stepper_dac.set_current_percent(i, parser.value_float()); + LOOP_LOGICAL_AXES(i) if (parser.seenval(axis_codes[i])) stepper_dac.set_current_percent(i, parser.value_float()); #endif } -#if EITHER(HAS_MOTOR_CURRENT_SPI, HAS_MOTOR_CURRENT_DAC) +#if HAS_MOTOR_CURRENT_SPI || HAS_MOTOR_CURRENT_PWM + + void GcodeSuite::M907_report(const bool forReplay/*=true*/) { + report_heading_etc(forReplay, F(STR_STEPPER_MOTOR_CURRENTS)); + #if HAS_MOTOR_CURRENT_PWM + SERIAL_ECHOLNPGM_P( // PWM-based has 3 values: + PSTR(" M907 X"), stepper.motor_current_setting[0] // X and Y + , SP_Z_STR, stepper.motor_current_setting[1] // Z + , SP_E_STR, stepper.motor_current_setting[2] // E + ); + #elif HAS_MOTOR_CURRENT_SPI + SERIAL_ECHOPGM(" M907"); // SPI-based has 5 values: + LOOP_LOGICAL_AXES(q) { // X Y Z (I J K) E (map to X Y Z (I J K) E0 by default) + SERIAL_CHAR(' ', axis_codes[q]); + SERIAL_ECHO(stepper.motor_current_setting[q]); + } + SERIAL_CHAR(' ', 'B'); // B (maps to E1 by default) + SERIAL_ECHOLN(stepper.motor_current_setting[4]); + #endif + } + +#endif // HAS_MOTOR_CURRENT_SPI || HAS_MOTOR_CURRENT_PWM + +#if HAS_MOTOR_CURRENT_SPI || HAS_MOTOR_CURRENT_DAC /** * M908: Control digital trimpot directly (M908 P S) @@ -90,7 +132,7 @@ void GcodeSuite::M907() { TERN_(HAS_MOTOR_CURRENT_DAC, stepper_dac.set_current_value(parser.byteval('P', -1), parser.ushortval('S', 0))); } - #if ENABLED(HAS_MOTOR_CURRENT_DAC) + #if HAS_MOTOR_CURRENT_DAC void GcodeSuite::M909() { stepper_dac.print_values(); } void GcodeSuite::M910() { stepper_dac.commit_eeprom(); } diff --git a/Marlin/src/gcode/feature/filwidth/M404-M407.cpp b/Marlin/src/gcode/feature/filwidth/M404-M407.cpp index a70f7a61fec7..ff174ecf1358 100644 --- a/Marlin/src/gcode/feature/filwidth/M404-M407.cpp +++ b/Marlin/src/gcode/feature/filwidth/M404-M407.cpp @@ -38,7 +38,7 @@ void GcodeSuite::M404() { planner.volumetric_area_nominal = CIRCLE_AREA(filwidth.nominal_mm * 0.5); } else - SERIAL_ECHOLNPAIR("Filament dia (nominal mm):", filwidth.nominal_mm); + SERIAL_ECHOLNPGM("Filament dia (nominal mm):", filwidth.nominal_mm); } /** @@ -65,7 +65,7 @@ void GcodeSuite::M406() { * M407: Get measured filament diameter on serial output */ void GcodeSuite::M407() { - SERIAL_ECHOLNPAIR("Filament dia (measured mm):", filwidth.measured_mm); + SERIAL_ECHOLNPGM("Filament dia (measured mm):", filwidth.measured_mm); } #endif // FILAMENT_WIDTH_SENSOR diff --git a/Marlin/src/gcode/feature/fwretract/G10_G11.cpp b/Marlin/src/gcode/feature/fwretract/G10_G11.cpp index 219502f28a8e..1889f83d6211 100644 --- a/Marlin/src/gcode/feature/fwretract/G10_G11.cpp +++ b/Marlin/src/gcode/feature/fwretract/G10_G11.cpp @@ -32,16 +32,7 @@ * G10 - Retract filament according to settings of M207 * TODO: Handle 'G10 P' for tool settings and 'G10 L' for workspace settings */ -void GcodeSuite::G10() { - #if HAS_MULTI_EXTRUDER - const bool rs = parser.boolval('S'); - #endif - fwretract.retract(true - #if HAS_MULTI_EXTRUDER - , rs - #endif - ); -} +void GcodeSuite::G10() { fwretract.retract(true E_OPTARG(parser.boolval('S'))); } /** * G11 - Recover filament according to settings of M208 diff --git a/Marlin/src/gcode/feature/fwretract/M207-M209.cpp b/Marlin/src/gcode/feature/fwretract/M207-M209.cpp index 538f16cde60b..173c2894dcd6 100644 --- a/Marlin/src/gcode/feature/fwretract/M207-M209.cpp +++ b/Marlin/src/gcode/feature/fwretract/M207-M209.cpp @@ -35,11 +35,11 @@ * F[units/min] retract_feedrate_mm_s * Z[units] retract_zraise */ -void GcodeSuite::M207() { - if (parser.seen('S')) fwretract.settings.retract_length = parser.value_axis_units(E_AXIS); - if (parser.seen('F')) fwretract.settings.retract_feedrate_mm_s = MMM_TO_MMS(parser.value_axis_units(E_AXIS)); - if (parser.seen('Z')) fwretract.settings.retract_zraise = parser.value_linear_units(); - if (parser.seen('W')) fwretract.settings.swap_retract_length = parser.value_axis_units(E_AXIS); +void GcodeSuite::M207() { fwretract.M207(); } + +void GcodeSuite::M207_report(const bool forReplay/*=true*/) { + report_heading_etc(forReplay, F(STR_RETRACT_S_F_Z)); + fwretract.M207_report(); } /** @@ -50,25 +50,28 @@ void GcodeSuite::M207() { * F[units/min] retract_recover_feedrate_mm_s * R[units/min] swap_retract_recover_feedrate_mm_s */ -void GcodeSuite::M208() { - if (parser.seen('S')) fwretract.settings.retract_recover_extra = parser.value_axis_units(E_AXIS); - if (parser.seen('F')) fwretract.settings.retract_recover_feedrate_mm_s = MMM_TO_MMS(parser.value_axis_units(E_AXIS)); - if (parser.seen('R')) fwretract.settings.swap_retract_recover_feedrate_mm_s = MMM_TO_MMS(parser.value_axis_units(E_AXIS)); - if (parser.seen('W')) fwretract.settings.swap_retract_recover_extra = parser.value_axis_units(E_AXIS); +void GcodeSuite::M208() { fwretract.M208(); } + +void GcodeSuite::M208_report(const bool forReplay/*=true*/) { + report_heading_etc(forReplay, F(STR_RECOVER_S_F)); + fwretract.M208_report(); } #if ENABLED(FWRETRACT_AUTORETRACT) /** * M209: Enable automatic retract (M209 S1) - * For slicers that don't support G10/11, reversed extrude-only - * moves will be classified as retraction. + * + * For slicers that don't support G10/11, reversed + * extruder-only moves can be classified as retraction. */ - void GcodeSuite::M209() { - if (MIN_AUTORETRACT <= MAX_AUTORETRACT && parser.seen('S')) - fwretract.enable_autoretract(parser.value_bool()); + void GcodeSuite::M209() { fwretract.M209(); } + + void GcodeSuite::M209_report(const bool forReplay/*=true*/) { + report_heading_etc(forReplay, F(STR_AUTO_RETRACT_S)); + fwretract.M209_report(); } -#endif // FWRETRACT_AUTORETRACT +#endif #endif // FWRETRACT diff --git a/Marlin/src/gcode/feature/i2c/M260_M261.cpp b/Marlin/src/gcode/feature/i2c/M260_M261.cpp index 438d1527f5f7..e978fb5048fb 100644 --- a/Marlin/src/gcode/feature/i2c/M260_M261.cpp +++ b/Marlin/src/gcode/feature/i2c/M260_M261.cpp @@ -60,15 +60,16 @@ void GcodeSuite::M260() { /** * M261: Request X bytes from I2C slave device * - * Usage: M261 A B + * Usage: M261 A B S