Browse Source

Merge branch 'vanilla_fb_2.1.x' into FB4S_WIFI

Signed-off-by: Sergey Terentiev <sergey@terentiev.me>
FB4S_WIFI
Sergey Terentiev 3 years ago
parent
commit
97834f0921
No known key found for this signature in database GPG Key ID: AFAA57ACD15D1EB0
  1. 314
      Marlin/Configuration.h
  2. 349
      Marlin/Configuration_adv.h
  3. 4
      Marlin/Makefile
  4. 4
      Marlin/Version.h
  5. 57
      Marlin/src/HAL/AVR/HAL.cpp
  6. 11
      Marlin/src/HAL/AVR/HAL.h
  7. 202
      Marlin/src/HAL/AVR/Servo.cpp
  8. 45
      Marlin/src/HAL/AVR/endstop_interrupts.h
  9. 6
      Marlin/src/HAL/AVR/inc/SanityCheck.h
  10. 2
      Marlin/src/HAL/AVR/pinsDebug.h
  11. 70
      Marlin/src/HAL/AVR/watchdog.cpp
  12. 31
      Marlin/src/HAL/AVR/watchdog.h
  13. 110
      Marlin/src/HAL/DUE/HAL.cpp
  14. 11
      Marlin/src/HAL/DUE/HAL.h
  15. 2
      Marlin/src/HAL/DUE/HAL_SPI.cpp
  16. 138
      Marlin/src/HAL/DUE/Servo.cpp
  17. 2
      Marlin/src/HAL/DUE/ServoTimers.h
  18. 36
      Marlin/src/HAL/DUE/eeprom_flash.cpp
  19. 6
      Marlin/src/HAL/DUE/endstop_interrupts.h
  20. 6
      Marlin/src/HAL/DUE/inc/SanityCheck.h
  21. 11
      Marlin/src/HAL/DUE/timers.cpp
  22. 3
      Marlin/src/HAL/DUE/timers.h
  23. 2
      Marlin/src/HAL/DUE/usb/compiler.h
  24. 114
      Marlin/src/HAL/DUE/watchdog.cpp
  25. 33
      Marlin/src/HAL/DUE/watchdog.h
  26. 101
      Marlin/src/HAL/ESP32/HAL.cpp
  27. 22
      Marlin/src/HAL/ESP32/HAL.h
  28. 6
      Marlin/src/HAL/ESP32/endstop_interrupts.h
  29. 20
      Marlin/src/HAL/ESP32/i2s.cpp
  30. 7
      Marlin/src/HAL/ESP32/inc/Conditionals_adv.h
  31. 4
      Marlin/src/HAL/ESP32/inc/SanityCheck.h
  32. 21
      Marlin/src/HAL/ESP32/u8g_esp32_spi.cpp
  33. 42
      Marlin/src/HAL/ESP32/watchdog.cpp
  34. 38
      Marlin/src/HAL/ESP32/watchdog.h
  35. 5
      Marlin/src/HAL/HAL.h
  36. 10
      Marlin/src/HAL/LINUX/HAL.h
  37. 16
      Marlin/src/HAL/LINUX/eeprom.cpp
  38. 4
      Marlin/src/HAL/LINUX/hardware/Heater.h
  39. 37
      Marlin/src/HAL/LINUX/watchdog.cpp
  40. 54
      Marlin/src/HAL/LPC1768/HAL.cpp
  41. 13
      Marlin/src/HAL/LPC1768/HAL.h
  42. 33
      Marlin/src/HAL/LPC1768/endstop_interrupts.h
  43. 72
      Marlin/src/HAL/LPC1768/watchdog.cpp
  44. 28
      Marlin/src/HAL/LPC1768/watchdog.h
  45. 7
      Marlin/src/HAL/NATIVE_SIM/HAL.h
  46. 27
      Marlin/src/HAL/NATIVE_SIM/watchdog.h
  47. 36
      Marlin/src/HAL/SAMD51/HAL.cpp
  48. 5
      Marlin/src/HAL/SAMD51/HAL.h
  49. 45
      Marlin/src/HAL/SAMD51/Servo.cpp
  50. 45
      Marlin/src/HAL/SAMD51/endstop_interrupts.h
  51. 54
      Marlin/src/HAL/SAMD51/watchdog.cpp
  52. 31
      Marlin/src/HAL/SAMD51/watchdog.h
  53. 28
      Marlin/src/HAL/STM32/HAL.cpp
  54. 9
      Marlin/src/HAL/STM32/HAL.h
  55. 3
      Marlin/src/HAL/STM32/MinSerial.cpp
  56. 6
      Marlin/src/HAL/STM32/endstop_interrupts.h
  57. 8
      Marlin/src/HAL/STM32/msc_sd.cpp
  58. 378
      Marlin/src/HAL/STM32/sdio.cpp
  59. 16
      Marlin/src/HAL/STM32/tft/gt911.cpp
  60. 2
      Marlin/src/HAL/STM32/tft/gt911.h
  61. 52
      Marlin/src/HAL/STM32/watchdog.cpp
  62. 50
      Marlin/src/HAL/STM32F1/HAL.cpp
  63. 5
      Marlin/src/HAL/STM32F1/HAL.h
  64. 3
      Marlin/src/HAL/STM32F1/MinSerial.cpp
  65. 16
      Marlin/src/HAL/STM32F1/Servo.cpp
  66. 6
      Marlin/src/HAL/STM32F1/endstop_interrupts.h
  67. 2
      Marlin/src/HAL/STM32F1/tft/tft_spi.cpp
  68. 66
      Marlin/src/HAL/STM32F1/watchdog.cpp
  69. 35
      Marlin/src/HAL/STM32F1/watchdog.h
  70. 62
      Marlin/src/HAL/TEENSY31_32/HAL.cpp
  71. 5
      Marlin/src/HAL/TEENSY31_32/HAL.h
  72. 6
      Marlin/src/HAL/TEENSY31_32/endstop_interrupts.h
  73. 40
      Marlin/src/HAL/TEENSY31_32/watchdog.cpp
  74. 34
      Marlin/src/HAL/TEENSY31_32/watchdog.h
  75. 64
      Marlin/src/HAL/TEENSY35_36/HAL.cpp
  76. 7
      Marlin/src/HAL/TEENSY35_36/HAL.h
  77. 6
      Marlin/src/HAL/TEENSY35_36/endstop_interrupts.h
  78. 40
      Marlin/src/HAL/TEENSY35_36/watchdog.cpp
  79. 30
      Marlin/src/HAL/TEENSY35_36/watchdog.h
  80. 65
      Marlin/src/HAL/TEENSY40_41/HAL.cpp
  81. 7
      Marlin/src/HAL/TEENSY40_41/HAL.h
  82. 52
      Marlin/src/HAL/TEENSY40_41/watchdog.cpp
  83. 30
      Marlin/src/HAL/TEENSY40_41/watchdog.h
  84. 4
      Marlin/src/HAL/shared/backtrace/unwarmbytab.cpp
  85. 2
      Marlin/src/HAL/shared/cpu_exception/exception_arm.cpp
  86. 13
      Marlin/src/HAL/shared/servo.cpp
  87. 12
      Marlin/src/HAL/shared/servo_private.h
  88. 111
      Marlin/src/MarlinCore.cpp
  89. 44
      Marlin/src/core/boards.h
  90. 15
      Marlin/src/core/drivers.h
  91. 79
      Marlin/src/core/language.h
  92. 48
      Marlin/src/core/macros.h
  93. 1
      Marlin/src/core/multi_language.h
  94. 27
      Marlin/src/core/serial.cpp
  95. 46
      Marlin/src/core/serial.h
  96. 4
      Marlin/src/core/serial_hook.h
  97. 409
      Marlin/src/core/types.h
  98. 13
      Marlin/src/core/utility.cpp
  99. 9
      Marlin/src/core/utility.h
  100. 49
      Marlin/src/feature/backlash.cpp

314
Marlin/Configuration.h

@ -35,7 +35,7 @@
*
* Advanced settings can be found in Configuration_adv.h
*/
#define CONFIGURATION_H_VERSION 02000903
#define CONFIGURATION_H_VERSION 02010000
//===========================================================================
//============================= Getting Started =============================
@ -151,42 +151,84 @@
//#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:
* Stepper Drivers
*
* These settings allow Marlin to tune stepper driver timing and enable advanced options for
* stepper drivers that support them. You may also override timing options in Configuration_adv.h.
*
* 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
* Use TMC2208/TMC2208_STANDALONE for TMC2225 drivers and TMC2209/TMC2209_STANDALONE for TMC2226 drivers.
*
* :[3, 4, 5, 6]
* Options: A4988, A5984, DRV8825, LV8729, L6470, L6474, POWERSTEP01,
* TB6560, TB6600, TMC2100,
* TMC2130, TMC2130_STANDALONE, TMC2160, TMC2160_STANDALONE,
* TMC2208, TMC2208_STANDALONE, TMC2209, TMC2209_STANDALONE,
* TMC26X, TMC26X_STANDALONE, TMC2660, TMC2660_STANDALONE,
* TMC5130, TMC5130_STANDALONE, TMC5160, TMC5160_STANDALONE
* :['A4988', 'A5984', 'DRV8825', 'LV8729', 'L6470', 'L6474', 'POWERSTEP01', 'TB6560', 'TB6600', 'TMC2100', 'TMC2130', 'TMC2130_STANDALONE', 'TMC2160', 'TMC2160_STANDALONE', 'TMC2208', 'TMC2208_STANDALONE', 'TMC2209', 'TMC2209_STANDALONE', 'TMC26X', 'TMC26X_STANDALONE', 'TMC2660', 'TMC2660_STANDALONE', 'TMC5130', 'TMC5130_STANDALONE', 'TMC5160', 'TMC5160_STANDALONE']
*/
//#define LINEAR_AXES 3
#define X_DRIVER_TYPE A4988
#define Y_DRIVER_TYPE A4988
#define Z_DRIVER_TYPE A4988
//#define X2_DRIVER_TYPE A4988
//#define Y2_DRIVER_TYPE A4988
//#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 U_DRIVER_TYPE A4988
//#define V_DRIVER_TYPE A4988
//#define W_DRIVER_TYPE A4988
#define E0_DRIVER_TYPE A4988
//#define E1_DRIVER_TYPE A4988
//#define E2_DRIVER_TYPE A4988
//#define E3_DRIVER_TYPE A4988
//#define E4_DRIVER_TYPE A4988
//#define E5_DRIVER_TYPE A4988
//#define E6_DRIVER_TYPE A4988
//#define E7_DRIVER_TYPE A4988
/**
* 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).
* Additional Axis Settings
*
* Define AXISn_ROTATES for all axes that rotate or pivot.
* Rotational axis coordinates are expressed in degrees.
*
* AXISn_NAME defines the letter used to refer to the axis in (most) G-code commands.
* By convention the names and roles are typically:
* 'A' : Rotational axis parallel to X
* 'B' : Rotational axis parallel to Y
* 'C' : Rotational axis parallel to Z
* 'U' : Secondary linear axis parallel to X
* 'V' : Secondary linear axis parallel to Y
* 'W' : Secondary linear axis parallel to Z
*
* Regardless of these settings the axes are internally named I, J, K, U, V, W.
*/
#if LINEAR_AXES >= 4
#ifdef I_DRIVER_TYPE
#define AXIS4_NAME 'A' // :['A', 'B', 'C', 'U', 'V', 'W']
#define AXIS4_ROTATES
#endif
#ifdef J_DRIVER_TYPE
#define AXIS5_NAME 'B' // :['B', 'C', 'U', 'V', 'W']
#define AXIS5_ROTATES
#endif
#ifdef K_DRIVER_TYPE
#define AXIS6_NAME 'C' // :['C', 'U', 'V', 'W']
#define AXIS6_ROTATES
#endif
#if LINEAR_AXES >= 5
#define AXIS5_NAME 'B' // :['A', 'B', 'C', 'U', 'V', 'W']
#ifdef U_DRIVER_TYPE
#define AXIS7_NAME 'U' // :['U', 'V', 'W']
//#define AXIS7_ROTATES
#endif
#if LINEAR_AXES >= 6
#define AXIS6_NAME 'C' // :['A', 'B', 'C', 'U', 'V', 'W']
#ifdef V_DRIVER_TYPE
#define AXIS8_NAME 'V' // :['V', 'W']
//#define AXIS8_ROTATES
#endif
#ifdef W_DRIVER_TYPE
#define AXIS9_NAME 'W' // :['W']
//#define AXIS9_ROTATES
#endif
// @section extruder
@ -590,19 +632,19 @@
//===========================================================================
//============================= PID Settings ================================
//===========================================================================
// PID Tuning Guide here: https://reprap.org/wiki/PID_Tuning
// Comment the following line to disable PID and enable bang-bang.
#define PIDTEMP
// Enable PIDTEMP for PID control or MPCTEMP for Predictive Model.
// temperature control. Disable both for bang-bang heating.
#define PIDTEMP // See the PID Tuning Guide at https://reprap.org/wiki/PID_Tuning
//#define MPCTEMP // ** EXPERIMENTAL **
#define BANG_MAX 255 // Limits current to nozzle while in bang-bang mode; 255=full current
#define PID_MAX BANG_MAX // Limits current to nozzle while PID is active (see PID_FUNCTIONAL_RANGE below); 255=full current
#define PID_K1 0.95 // Smoothing factor within any PID loop
#if ENABLED(PIDTEMP)
#define PID_EDIT_MENU // Add PID editing to the "Advanced Settings" menu. (~700 bytes of PROGMEM)
#define PID_AUTOTUNE_MENU // Add PID auto-tuning to the "Advanced Settings" menu. (~250 bytes of PROGMEM)
//#define PID_PARAMS_PER_HOTEND // Uses separate PID parameters for each extruder (useful for mismatched extruders)
// Set/get with gcode: M301 E[extruder number, 0-2]
// Set/get with G-code: M301 E[extruder number, 0-2]
#if ENABLED(PID_PARAMS_PER_HOTEND)
// Specify up to one value per hotend here, according to your setup.
@ -615,7 +657,49 @@
#define DEFAULT_Ki 1.31
#define DEFAULT_Kd 55.34
#endif
#endif // PIDTEMP
#endif
/**
* Model Predictive Control for hotend
*
* Use a physical model of the hotend to control temperature. When configured correctly
* this gives better responsiveness and stability than PID and it also removes the need
* for PID_EXTRUSION_SCALING and PID_FAN_SCALING. Use M306 to autotune the model.
*/
#if ENABLED(MPCTEMP)
//#define MPC_EDIT_MENU // Add MPC editing to the "Advanced Settings" menu. (~1300 bytes of flash)
//#define MPC_AUTOTUNE_MENU // Add MPC auto-tuning to the "Advanced Settings" menu. (~350 bytes of flash)
#define MPC_MAX BANG_MAX // (0..255) Current to nozzle while MPC is active.
#define MPC_HEATER_POWER { 40.0f } // (W) Heat cartridge powers.
#define MPC_INCLUDE_FAN // Model the fan speed?
// Measured physical constants from M306
#define MPC_BLOCK_HEAT_CAPACITY { 16.7f } // (J/K) Heat block heat capacities.
#define MPC_SENSOR_RESPONSIVENESS { 0.22f } // (K/s per ∆K) Rate of change of sensor temperature from heat block.
#define MPC_AMBIENT_XFER_COEFF { 0.068f } // (W/K) Heat transfer coefficients from heat block to room air with fan off.
#if ENABLED(MPC_INCLUDE_FAN)
#define MPC_AMBIENT_XFER_COEFF_FAN255 { 0.097f } // (W/K) Heat transfer coefficients from heat block to room air with fan on full.
#endif
// For one fan and multiple hotends MPC needs to know how to apply the fan cooling effect.
#if ENABLED(MPC_INCLUDE_FAN)
//#define MPC_FAN_0_ALL_HOTENDS
//#define MPC_FAN_0_ACTIVE_HOTEND
#endif
#define FILAMENT_HEAT_CAPACITY_PERMM { 5.6e-3f } // 0.0056 J/K/mm for 1.75mm PLA (0.0149 J/K/mm for 2.85mm PLA).
//#define FILAMENT_HEAT_CAPACITY_PERMM { 3.6e-3f } // 0.0036 J/K/mm for 1.75mm PETG (0.0094 J/K/mm for 2.85mm PETG).
// Advanced options
#define MPC_SMOOTHING_FACTOR 0.5f // (0.0...1.0) Noisy temperature sensors may need a lower value for stabilization.
#define MPC_MIN_AMBIENT_CHANGE 1.0f // (K/s) Modeled ambient temperature rate of change, when correcting model inaccuracies.
#define MPC_STEADYSTATE 0.5f // (K/s) Temperature change rate for steady state logic to be enforced.
#define MPC_TUNING_POS { X_CENTER, Y_CENTER, 1.0f } // (mm) M306 Autotuning position, ideally bed center at first layer height.
#define MPC_TUNING_END_Z 10.0f // (mm) M306 Autotuning final Z position.
#endif
//===========================================================================
//====================== PID > Bed Temperature Control ======================
@ -708,6 +792,9 @@
//#define SLOW_PWM_HEATERS // PWM with very low frequency (roughly 0.125Hz=8s) and minimum state time of approximately 1s useful for heaters driven by a relay
#define PID_FUNCTIONAL_RANGE 20 // If the temperature difference between the target temperature and the actual temperature
// is more than PID_FUNCTIONAL_RANGE then the PID will be shut off and the heater will be set to min/max.
#define PID_EDIT_MENU // Add PID editing to the "Advanced Settings" menu. (~700 bytes of flash)
#define PID_AUTOTUNE_MENU // Add PID auto-tuning to the "Advanced Settings" menu. (~250 bytes of flash)
#endif
// @section extruder
@ -778,6 +865,13 @@
#define POLAR_SEGMENTS_PER_SECOND 5
#endif
// Enable for an articulated robot (robot arm). Joints are directly mapped to axes (no kinematics).
//#define ARTICULATED_ROBOT_ARM
// For a hot wire cutter with parallel horizontal axes (X, I) where the heights of the two wire
// ends are controlled by parallel axes (Y, J). Joints are directly mapped to axes (no kinematics).
//#define FOAMCUTTER_XYUV
//===========================================================================
//============================== Endstop Settings ===========================
//===========================================================================
@ -793,12 +887,18 @@
//#define USE_IMIN_PLUG
//#define USE_JMIN_PLUG
//#define USE_KMIN_PLUG
//#define USE_UMIN_PLUG
//#define USE_VMIN_PLUG
//#define USE_WMIN_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
//#define USE_UMAX_PLUG
//#define USE_VMAX_PLUG
//#define USE_WMAX_PLUG
// Enable pullup for all endstops to prevent a floating state
#define ENDSTOPPULLUPS
@ -810,12 +910,18 @@
//#define ENDSTOPPULLUP_IMIN
//#define ENDSTOPPULLUP_JMIN
//#define ENDSTOPPULLUP_KMIN
//#define ENDSTOPPULLUP_UMIN
//#define ENDSTOPPULLUP_VMIN
//#define ENDSTOPPULLUP_WMIN
//#define ENDSTOPPULLUP_XMAX
//#define ENDSTOPPULLUP_YMAX
//#define ENDSTOPPULLUP_ZMAX
//#define ENDSTOPPULLUP_IMAX
//#define ENDSTOPPULLUP_JMAX
//#define ENDSTOPPULLUP_KMAX
//#define ENDSTOPPULLUP_UMAX
//#define ENDSTOPPULLUP_VMAX
//#define ENDSTOPPULLUP_WMAX
//#define ENDSTOPPULLUP_ZMIN_PROBE
#endif
@ -829,12 +935,18 @@
//#define ENDSTOPPULLDOWN_IMIN
//#define ENDSTOPPULLDOWN_JMIN
//#define ENDSTOPPULLDOWN_KMIN
//#define ENDSTOPPULLDOWN_UMIN
//#define ENDSTOPPULLDOWN_VMIN
//#define ENDSTOPPULLDOWN_WMIN
//#define ENDSTOPPULLDOWN_XMAX
//#define ENDSTOPPULLDOWN_YMAX
//#define ENDSTOPPULLDOWN_ZMAX
//#define ENDSTOPPULLDOWN_IMAX
//#define ENDSTOPPULLDOWN_JMAX
//#define ENDSTOPPULLDOWN_KMAX
//#define ENDSTOPPULLDOWN_UMAX
//#define ENDSTOPPULLDOWN_VMAX
//#define ENDSTOPPULLDOWN_WMAX
//#define ENDSTOPPULLDOWN_ZMIN_PROBE
#endif
@ -845,52 +957,20 @@
#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 true // Set to true to invert the logic of the endstop.
#define Y_MAX_ENDSTOP_INVERTING true // Set to true to invert the logic of the endstop.
#define Z_MAX_ENDSTOP_INVERTING true // Set to true to invert the logic of the endstop.
#define U_MIN_ENDSTOP_INVERTING false // Set to true to invert the logic of the endstop.
#define V_MIN_ENDSTOP_INVERTING false // Set to true to invert the logic of the endstop.
#define W_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 U_MAX_ENDSTOP_INVERTING false // Set to true to invert the logic of the endstop.
#define V_MAX_ENDSTOP_INVERTING false // Set to true to invert the logic of the endstop.
#define W_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.
/**
* Stepper Drivers
*
* These settings allow Marlin to tune stepper driver timing and enable advanced options for
* stepper drivers that support them. You may also override timing options in Configuration_adv.h.
*
* A4988 is assumed for unspecified drivers.
*
* Use TMC2208/TMC2208_STANDALONE for TMC2225 drivers and TMC2209/TMC2209_STANDALONE for TMC2226 drivers.
*
* Options: A4988, A5984, DRV8825, LV8729, L6470, L6474, POWERSTEP01,
* TB6560, TB6600, TMC2100,
* TMC2130, TMC2130_STANDALONE, TMC2160, TMC2160_STANDALONE,
* TMC2208, TMC2208_STANDALONE, TMC2209, TMC2209_STANDALONE,
* TMC26X, TMC26X_STANDALONE, TMC2660, TMC2660_STANDALONE,
* TMC5130, TMC5130_STANDALONE, TMC5160, TMC5160_STANDALONE
* :['A4988', 'A5984', 'DRV8825', 'LV8729', 'L6470', 'L6474', 'POWERSTEP01', 'TB6560', 'TB6600', 'TMC2100', 'TMC2130', 'TMC2130_STANDALONE', 'TMC2160', 'TMC2160_STANDALONE', 'TMC2208', 'TMC2208_STANDALONE', 'TMC2209', 'TMC2209_STANDALONE', 'TMC26X', 'TMC26X_STANDALONE', 'TMC2660', 'TMC2660_STANDALONE', 'TMC5130', 'TMC5130_STANDALONE', 'TMC5160', 'TMC5160_STANDALONE']
*/
#define X_DRIVER_TYPE A4988
#define Y_DRIVER_TYPE A4988
#define Z_DRIVER_TYPE A4988
//#define X2_DRIVER_TYPE A4988
//#define Y2_DRIVER_TYPE A4988
//#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 A4988
//#define E1_DRIVER_TYPE A4988
//#define E2_DRIVER_TYPE A4988
//#define E3_DRIVER_TYPE A4988
//#define E4_DRIVER_TYPE A4988
//#define E5_DRIVER_TYPE A4988
//#define E6_DRIVER_TYPE A4988
//#define E7_DRIVER_TYPE A4988
// Enable this feature if all enabled endstop pins are interrupt-capable.
// This will remove the need to poll the interrupt pins, saving many CPU cycles.
#define ENDSTOP_INTERRUPTS_FEATURE
@ -933,16 +1013,16 @@
//#define DISTINCT_E_FACTORS
/**
* Default Axis Steps Per Unit (steps/mm)
* Default Axis Steps Per Unit (linear=steps/mm, rotational=steps/°)
* Override with M92
* X, Y, Z [, I [, J [, K]]], E0 [, E1[, E2...]]
* X, Y, Z [, I [, J [, K...]]], E0 [, E1[, E2...]]
*/
#define DEFAULT_AXIS_STEPS_PER_UNIT { 80, 80, 400, 400 }
/**
* Default Max Feed Rate (mm/s)
* Default Max Feed Rate (linear=mm/s, rotational=°/s)
* Override with M203
* X, Y, Z [, I [, J [, K]]], E0 [, E1[, E2...]]
* X, Y, Z [, I [, J [, K...]]], E0 [, E1[, E2...]]
*/
#define DEFAULT_MAX_FEEDRATE { 200, 200, 4, 50 }
@ -952,10 +1032,10 @@
#endif
/**
* Default Max Acceleration (change/s) change = mm/s
* Default Max Acceleration (speed change with time) (linear=mm/(s^2), rotational=°/(s^2))
* (Maximum start speed for accelerated moves)
* Override with M201
* X, Y, Z [, I [, J [, K]]], E0 [, E1[, E2...]]
* X, Y, Z [, I [, J [, K...]]], E0 [, E1[, E2...]]
*/
#define DEFAULT_MAX_ACCELERATION { 3000, 3000, 100, 3000 }
@ -965,7 +1045,7 @@
#endif
/**
* Default Acceleration (change/s) change = mm/s
* Default Acceleration (speed change with time) (linear=mm/(s^2), rotational=°/(s^2))
* Override with M204
*
* M204 P Acceleration
@ -978,7 +1058,7 @@
/**
* Default Jerk limits (mm/s)
* Override with M205 X Y Z E
* Override with M205 X Y Z . . . E
*
* "Jerk" specifies the minimum speed change that requires acceleration.
* When changing speed and direction, if the difference is less than the
@ -992,6 +1072,9 @@
//#define DEFAULT_IJERK 0.3
//#define DEFAULT_JJERK 0.3
//#define DEFAULT_KJERK 0.3
//#define DEFAULT_UJERK 0.3
//#define DEFAULT_VJERK 0.3
//#define DEFAULT_WJERK 0.3
//#define TRAVEL_EXTRA_XYJERK 0.0 // Additional jerk allowance for all travel moves
@ -1330,6 +1413,9 @@
//#define I_ENABLE_ON 0
//#define J_ENABLE_ON 0
//#define K_ENABLE_ON 0
//#define U_ENABLE_ON 0
//#define V_ENABLE_ON 0
//#define W_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!
@ -1339,6 +1425,9 @@
//#define DISABLE_I false
//#define DISABLE_J false
//#define DISABLE_K false
//#define DISABLE_U false
//#define DISABLE_V false
//#define DISABLE_W false
// Turn off the display blinking that warns about possible accuracy reduction
//#define DISABLE_REDUCED_ACCURACY_WARNING
@ -1429,6 +1518,7 @@
#endif
// Invert the stepper direction. Change (or reverse the motor connector) if an axis goes the wrong way.
#define INVERT_X_DIR USR_X_DIR
#define INVERT_Y_DIR USR_Y_DIR
@ -1436,6 +1526,9 @@
//#define INVERT_I_DIR false
//#define INVERT_J_DIR false
//#define INVERT_K_DIR false
//#define INVERT_U_DIR false
//#define INVERT_V_DIR false
//#define INVERT_W_DIR false
// @section extruder
@ -1474,6 +1567,9 @@
//#define I_HOME_DIR -1
//#define J_HOME_DIR -1
//#define K_HOME_DIR -1
//#define U_HOME_DIR -1
//#define V_HOME_DIR -1
//#define W_HOME_DIR -1
// @section machine
@ -1481,7 +1577,7 @@
#define X_BED_SIZE 250
#define Y_BED_SIZE 210
// Travel limits (mm) after homing, corresponding to endstop positions.
// Travel limits (linear=mm, rotational=°) after homing, corresponding to endstop positions.
#define X_MIN_POS 0
#define Y_MIN_POS 0
#define Z_MIN_POS 0
@ -1494,6 +1590,12 @@
//#define J_MAX_POS 50
//#define K_MIN_POS 0
//#define K_MAX_POS 50
//#define U_MIN_POS 0
//#define U_MAX_POS 50
//#define V_MIN_POS 0
//#define V_MAX_POS 50
//#define W_MIN_POS 0
//#define W_MAX_POS 50
/**
* Software Endstops
@ -1513,6 +1615,9 @@
#define MIN_SOFTWARE_ENDSTOP_I
#define MIN_SOFTWARE_ENDSTOP_J
#define MIN_SOFTWARE_ENDSTOP_K
#define MIN_SOFTWARE_ENDSTOP_U
#define MIN_SOFTWARE_ENDSTOP_V
#define MIN_SOFTWARE_ENDSTOP_W
#endif
// Max software endstops constrain movement within maximum coordinate bounds
@ -1524,6 +1629,9 @@
#define MAX_SOFTWARE_ENDSTOP_I
#define MAX_SOFTWARE_ENDSTOP_J
#define MAX_SOFTWARE_ENDSTOP_K
#define MAX_SOFTWARE_ENDSTOP_U
#define MAX_SOFTWARE_ENDSTOP_V
#define MAX_SOFTWARE_ENDSTOP_W
#endif
#if EITHER(MIN_SOFTWARE_ENDSTOPS, MAX_SOFTWARE_ENDSTOPS)
@ -1788,18 +1896,18 @@
#endif
// Add a menu item to move between bed corners for manual bed adjustment
#define LEVEL_BED_CORNERS
#if ENABLED(LEVEL_BED_CORNERS)
#define LEVEL_CORNERS_INSET_LFRB { 30, 30, 30, 30 } // (mm) Left, Front, Right, Back insets
#define LEVEL_CORNERS_HEIGHT 0.0 // (mm) Z height of nozzle at leveling points
#define LEVEL_CORNERS_Z_HOP 4.0 // (mm) Z height of nozzle between leveling points
//#define LEVEL_CENTER_TOO // Move to the center after the last corner
//#define LEVEL_CORNERS_USE_PROBE
#if ENABLED(LEVEL_CORNERS_USE_PROBE)
#define LEVEL_CORNERS_PROBE_TOLERANCE 0.1
#define LEVEL_CORNERS_VERIFY_RAISED // After adjustment triggers the probe, re-probe to verify
//#define LEVEL_CORNERS_AUDIO_FEEDBACK
#define LCD_BED_TRAMMING
#if ENABLED(LCD_BED_TRAMMING)
#define BED_TRAMMING_INSET_LFRB { 30, 30, 30, 30 } // (mm) Left, Front, Right, Back insets
#define BED_TRAMMING_HEIGHT 0.0 // (mm) Z height of nozzle at leveling points
#define BED_TRAMMING_Z_HOP 4.0 // (mm) Z height of nozzle between leveling points
//#define BED_TRAMMING_INCLUDE_CENTER // Move to the center after the last corner
//#define BED_TRAMMING_USE_PROBE
#if ENABLED(BED_TRAMMING_USE_PROBE)
#define BED_TRAMMING_PROBE_TOLERANCE 0.1 // (mm)
#define BED_TRAMMING_VERIFY_RAISED // After adjustment triggers the probe, re-probe to verify
//#define BED_TRAMMING_AUDIO_FEEDBACK
#endif
/**
@ -1819,7 +1927,7 @@
* | 1 2 | | 1 4 | | 1 2 | | 2 |
* LF --------- RF LF --------- RF LF --------- RF LF --------- RF
*/
#define LEVEL_CORNERS_LEVELING_ORDER { LF, RF, RB, LB }
#define BED_TRAMMING_LEVELING_ORDER { LF, RF, RB, LB }
#endif
/**
@ -1841,6 +1949,9 @@
//#define MANUAL_I_HOME_POS 0
//#define MANUAL_J_HOME_POS 0
//#define MANUAL_K_HOME_POS 0
//#define MANUAL_U_HOME_POS 0
//#define MANUAL_V_HOME_POS 0
//#define MANUAL_W_HOME_POS 0
/**
* Use "Z Safe Homing" to avoid homing with a Z probe outside the bed area.
@ -2740,9 +2851,6 @@ EEPROM_W25Q
// Touch-screen LCD for Malyan M200/M300 printers
//
//#define MALYAN_LCD
#if ENABLED(MALYAN_LCD)
#define LCD_SERIAL_PORT 1 // Default is 1 for Malyan M200
#endif
//
// Touch UI for FTDI EVE (FT800/FT810) displays
@ -2756,7 +2864,6 @@ EEPROM_W25Q
//#define ANYCUBIC_LCD_I3MEGA
//#define ANYCUBIC_LCD_CHIRON
#if EITHER(ANYCUBIC_LCD_I3MEGA, ANYCUBIC_LCD_CHIRON)
#define LCD_SERIAL_PORT 3 // Default is 3 for Anycubic
//#define ANYCUBIC_LCD_DEBUG
#endif
@ -2764,9 +2871,6 @@ EEPROM_W25Q
// 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.

349
Marlin/Configuration_adv.h

@ -30,7 +30,7 @@
*
* Basic settings can be found in Configuration.h
*/
#define CONFIGURATION_ADV_H_VERSION 02000903
#define CONFIGURATION_ADV_H_VERSION 02010000
//===========================================================================
//============================= Thermal Settings ============================
@ -699,73 +699,6 @@
//#define CLOSED_LOOP_MOVE_COMPLETE_PIN -1
#endif
/**
* Dual Steppers / Dual Endstops
*
* This section will allow you to use extra E drivers to drive a second motor for X, Y, or Z axes.
*
* For example, set X_DUAL_STEPPER_DRIVERS setting to use a second motor. If the motors need to
* spin in opposite directions set INVERT_X2_VS_X_DIR. If the second motor needs its own endstop
* set X_DUAL_ENDSTOPS. This can adjust for "racking." Use X2_USE_ENDSTOP to set the endstop plug
* that should be used for the second endstop. Extra endstops will appear in the output of 'M119'.
*
* Use X_DUAL_ENDSTOP_ADJUSTMENT to adjust for mechanical imperfection. After homing both motors
* this offset is applied to the X2 motor. To find the offset home the X axis, and measure the error
* in X2. Dual endstop offsets can be set at runtime with 'M666 X<offset> Y<offset> Z<offset>'.
*/
//#define X_DUAL_STEPPER_DRIVERS
#if ENABLED(X_DUAL_STEPPER_DRIVERS)
//#define INVERT_X2_VS_X_DIR // Enable if X2 direction signal is opposite to X
//#define X_DUAL_ENDSTOPS
#if ENABLED(X_DUAL_ENDSTOPS)
#define X2_USE_ENDSTOP _XMAX_
#define X2_ENDSTOP_ADJUSTMENT 0
#endif
#endif
//#define Y_DUAL_STEPPER_DRIVERS
#if ENABLED(Y_DUAL_STEPPER_DRIVERS)
//#define INVERT_Y2_VS_Y_DIR // Enable if Y2 direction signal is opposite to Y
//#define Y_DUAL_ENDSTOPS
#if ENABLED(Y_DUAL_ENDSTOPS)
#define Y2_USE_ENDSTOP _YMAX_
#define Y2_ENDSTOP_ADJUSTMENT 0
#endif
#endif
//
// For Z set the number of stepper drivers
//
#define NUM_Z_STEPPER_DRIVERS 1 // (1-4) Z options change based on how many
#if NUM_Z_STEPPER_DRIVERS > 1
// Enable if Z motor direction signals are the opposite of Z1
//#define INVERT_Z2_VS_Z_DIR
//#define INVERT_Z3_VS_Z_DIR
//#define INVERT_Z4_VS_Z_DIR
//#define Z_MULTI_ENDSTOPS
#if ENABLED(Z_MULTI_ENDSTOPS)
#define Z2_USE_ENDSTOP _XMAX_
#define Z2_ENDSTOP_ADJUSTMENT 0
#if NUM_Z_STEPPER_DRIVERS >= 3
#define Z3_USE_ENDSTOP _YMAX_
#define Z3_ENDSTOP_ADJUSTMENT 0
#endif
#if NUM_Z_STEPPER_DRIVERS >= 4
#define Z4_USE_ENDSTOP _ZMAX_
#define Z4_ENDSTOP_ADJUSTMENT 0
#endif
#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
*
@ -816,6 +749,77 @@
//#define EVENT_GCODE_IDEX_AFTER_MODECHANGE "G28X"
#endif
/**
* Multi-Stepper / Multi-Endstop
*
* When X2_DRIVER_TYPE is defined, this indicates that the X and X2 motors work in tandem.
* The following explanations for X also apply to Y and Z multi-stepper setups.
* Endstop offsets may be changed by 'M666 X<offset> Y<offset> Z<offset>' and stored to EEPROM.
*
* - Enable INVERT_X2_VS_X_DIR if the X2 motor requires an opposite DIR signal from X.
*
* - Enable X_DUAL_ENDSTOPS if the second motor has its own endstop, with adjustable offset.
*
* - Extra endstops are included in the output of 'M119'.
*
* - Set X_DUAL_ENDSTOP_ADJUSTMENT to the known error in the X2 endstop.
* Applied to the X2 motor on 'G28' / 'G28 X'.
* Get the offset by homing X and measuring the error.
* Also set with 'M666 X<offset>' and stored to EEPROM with 'M500'.
*
* - Use X2_USE_ENDSTOP to set the endstop plug by name. (_XMIN_, _XMAX_, _YMIN_, _YMAX_, _ZMIN_, _ZMAX_)
*/
#if HAS_X2_STEPPER && DISABLED(DUAL_X_CARRIAGE)
//#define INVERT_X2_VS_X_DIR // X2 direction signal is the opposite of X
//#define X_DUAL_ENDSTOPS // X2 has its own endstop
#if ENABLED(X_DUAL_ENDSTOPS)
#define X2_USE_ENDSTOP _XMAX_ // X2 endstop board plug. Don't forget to enable USE_*_PLUG.
#define X2_ENDSTOP_ADJUSTMENT 0 // X2 offset relative to X endstop
#endif
#endif
#if HAS_DUAL_Y_STEPPERS
//#define INVERT_Y2_VS_Y_DIR // Y2 direction signal is the opposite of Y
//#define Y_DUAL_ENDSTOPS // Y2 has its own endstop
#if ENABLED(Y_DUAL_ENDSTOPS)
#define Y2_USE_ENDSTOP _YMAX_ // Y2 endstop board plug. Don't forget to enable USE_*_PLUG.
#define Y2_ENDSTOP_ADJUSTMENT 0 // Y2 offset relative to Y endstop
#endif
#endif
//
// Multi-Z steppers
//
#ifdef Z2_DRIVER_TYPE
//#define INVERT_Z2_VS_Z_DIR // Z2 direction signal is the opposite of Z
//#define Z_MULTI_ENDSTOPS // Other Z axes have their own endstops
#if ENABLED(Z_MULTI_ENDSTOPS)
#define Z2_USE_ENDSTOP _XMAX_ // Z2 endstop board plug. Don't forget to enable USE_*_PLUG.
#define Z2_ENDSTOP_ADJUSTMENT 0 // Z2 offset relative to Y endstop
#endif
#ifdef Z3_DRIVER_TYPE
//#define INVERT_Z3_VS_Z_DIR // Z3 direction signal is the opposite of Z
#if ENABLED(Z_MULTI_ENDSTOPS)
#define Z3_USE_ENDSTOP _YMAX_ // Z3 endstop board plug. Don't forget to enable USE_*_PLUG.
#define Z3_ENDSTOP_ADJUSTMENT 0 // Z3 offset relative to Y endstop
#endif
#endif
#ifdef Z4_DRIVER_TYPE
//#define INVERT_Z4_VS_Z_DIR // Z4 direction signal is the opposite of Z
#if ENABLED(Z_MULTI_ENDSTOPS)
#define Z4_USE_ENDSTOP _ZMAX_ // Z4 endstop board plug. Don't forget to enable USE_*_PLUG.
#define Z4_ENDSTOP_ADJUSTMENT 0 // Z4 offset relative to Y endstop
#endif
#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 // E direction signals are opposites
#endif
// Activate a solenoid on the active extruder with M380. Disable all with M381.
// Define SOL0_PIN, SOL1_PIN, etc., for each extruder that has a solenoid.
//#define EXT_SOLENOID
@ -828,12 +832,12 @@
* the position of the toolhead relative to the workspace.
*/
//#define SENSORLESS_BACKOFF_MM { 2, 2, 0 } // (mm) Backoff from endstops before sensorless homing
//#define SENSORLESS_BACKOFF_MM { 2, 2, 0 } // (linear=mm, rotational=°) Backoff from endstops before sensorless homing
#define HOMING_BUMP_MM { 5, 5, 2 } // (mm) Backoff from endstops after first bump
#define HOMING_BUMP_MM { 5, 5, 2 } // (linear=mm, rotational=°) Backoff from endstops after first bump
#define HOMING_BUMP_DIVISOR { 2, 2, 4 } // Re-Bump Speed Divisor (Divides the Homing Feedrate)
//#define HOMING_BACKOFF_POST_MM { 2, 2, 2 } // (mm) Backoff from endstops after homing
//#define HOMING_BACKOFF_POST_MM { 2, 2, 2 } // (linear=mm, rotational=°) Backoff from endstops after homing
#define QUICK_HOME // If G28 contains XY do a diagonal move first
//#define HOME_Y_BEFORE_X // If G28 contains XY home Y before X
@ -949,7 +953,7 @@
/**
* Z Stepper positions for more rapid convergence in bed alignment.
* Requires NUM_Z_STEPPER_DRIVERS to be 3 or 4.
* Requires 3 or 4 Z steppers.
*
* Define Stepper XY positions for Z1, Z2, Z3... corresponding to the screw
* positions in the bed carriage, with one position per Z stepper in stepper
@ -1019,6 +1023,9 @@
#define INVERT_I_STEP_PIN false
#define INVERT_J_STEP_PIN false
#define INVERT_K_STEP_PIN false
#define INVERT_U_STEP_PIN false
#define INVERT_V_STEP_PIN false
#define INVERT_W_STEP_PIN false
#define INVERT_E_STEP_PIN false
/**
@ -1033,11 +1040,14 @@
#define DISABLE_INACTIVE_I true
#define DISABLE_INACTIVE_J true
#define DISABLE_INACTIVE_K true
#define DISABLE_INACTIVE_U true
#define DISABLE_INACTIVE_V true
#define DISABLE_INACTIVE_W true
#define DISABLE_INACTIVE_E true
// Default Minimum Feedrates for printing and travel moves
#define DEFAULT_MINIMUMFEEDRATE 0.0 // (mm/s) Minimum feedrate. Set with M205 S.
#define DEFAULT_MINTRAVELFEEDRATE 0.0 // (mm/s) Minimum travel feedrate. Set with M205 T.
#define DEFAULT_MINIMUMFEEDRATE 0.0 // (mm/s. °/s for rotational-only moves) Minimum feedrate. Set with M205 S.
#define DEFAULT_MINTRAVELFEEDRATE 0.0 // (mm/s. °/s for rotational-only moves) Minimum travel feedrate. Set with M205 T.
// Minimum time that a segment needs to take as the buffer gets emptied
#define DEFAULT_MINSEGMENTTIME 20000 // (µs) Set with M205 B.
@ -1073,7 +1083,7 @@
#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) One value for each linear axis
#define BACKLASH_DISTANCE_MM { 0, 0, 0 } // (linear=mm, rotational=°) 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
@ -1150,6 +1160,12 @@
//#define CALIBRATION_MEASURE_JMAX
//#define CALIBRATION_MEASURE_KMIN
//#define CALIBRATION_MEASURE_KMAX
//#define CALIBRATION_MEASURE_UMIN
//#define CALIBRATION_MEASURE_UMAX
//#define CALIBRATION_MEASURE_VMIN
//#define CALIBRATION_MEASURE_VMAX
//#define CALIBRATION_MEASURE_WMIN
//#define CALIBRATION_MEASURE_WMAX
// 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.
@ -1321,6 +1337,7 @@
#if ANY(HAS_DISPLAY, DWIN_LCD_PROUI, DWIN_CREALITY_LCD_JYERSUI)
//#define SOUND_MENU_ITEM // Add a mute option to the LCD menu
#define SOUND_ON_DEFAULT // Buzzer/speaker default enabled state
#endif
#if EITHER(HAS_DISPLAY, DWIN_LCD_PROUI)
@ -1664,14 +1681,25 @@
//#define XYZ_NO_FRAME
#define XYZ_HOLLOW_FRAME
// A bigger font is available for edit items. Costs 3120 bytes of PROGMEM.
// A bigger font is available for edit items. Costs 3120 bytes of flash.
// Western only. Not available for Cyrillic, Kana, Turkish, Greek, or Chinese.
//#define USE_BIG_EDIT_FONT
// A smaller font may be used on the Info Screen. Costs 2434 bytes of PROGMEM.
// A smaller font may be used on the Info Screen. Costs 2434 bytes of flash.
// Western only. Not available for Cyrillic, Kana, Turkish, Greek, or Chinese.
//#define USE_SMALL_INFOFONT
/**
* Graphical Display Sleep
*
* The U8G library provides sleep / wake functions for SH1106, SSD1306,
* SSD1309, and some other DOGM displays.
* Enable this option to save energy and prevent OLED pixel burn-in.
* Adds the menu item Configuration > LCD Timeout (m) to set a wait period
* from 0 (disabled) to 99 minutes.
*/
//#define DISPLAY_SLEEP_MINUTES 2 // (minutes) Timeout before turning off the screen
/**
* ST7920-based LCDs can emulate a 16 x 4 character display using
* the ST7920 character-generator for very fast screen updates.
@ -1713,7 +1741,7 @@
//#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 flash.
// Frivolous Game Options
//#define MARLIN_BRICKOUT
@ -1738,7 +1766,6 @@
// Additional options for DGUS / DWIN displays
//
#if HAS_DGUS_LCD
#define LCD_SERIAL_PORT 3
#define LCD_BAUDRATE 115200
#define DGUS_RX_BUFFER_SIZE 128
@ -1999,6 +2026,21 @@
// @section leveling
/**
* Use Safe Bed Leveling coordinates to move axes to a useful position before bed probing.
* For example, after homing a rotational axis the Z probe might not be perpendicular to the bed.
* Choose values the orient the bed horizontally and the Z-probe vertically.
*/
//#define SAFE_BED_LEVELING_START_X 0.0
//#define SAFE_BED_LEVELING_START_Y 0.0
//#define SAFE_BED_LEVELING_START_Z 0.0
//#define SAFE_BED_LEVELING_START_I 0.0
//#define SAFE_BED_LEVELING_START_J 0.0
//#define SAFE_BED_LEVELING_START_K 0.0
//#define SAFE_BED_LEVELING_START_U 0.0
//#define SAFE_BED_LEVELING_START_V 0.0
//#define SAFE_BED_LEVELING_START_W 0.0
/**
* Points to probe for all 3-point Leveling procedures.
* Override if the automatically selected points are inadequate.
@ -2259,7 +2301,7 @@
#define BUFSIZE 32
// Transmission to Host Buffer Size
// To save 386 bytes of PROGMEM (and TX_BUFFER_SIZE+3 bytes of RAM) set to 0.
// To save 386 bytes of flash (and TX_BUFFER_SIZE+3 bytes of RAM) set to 0.
// To buffer a simple "ok" you need 4 bytes.
// For ADVANCED_OK (M105) you need 32 bytes.
// For debug-echo: 128 bytes for the optimal speed.
@ -2410,7 +2452,7 @@
/**
* 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.
* stepper motor (e.g., I axis 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
@ -2444,12 +2486,16 @@
#define TOOLCHANGE_FS_FAN_SPEED 255 // 0-255
#define TOOLCHANGE_FS_FAN_TIME 10 // (seconds)
// Swap uninitialized extruder (using TOOLCHANGE_FS_PRIME_SPEED feedrate)
// (May break filament if not retracted beforehand.)
//#define TOOLCHANGE_FS_INIT_BEFORE_SWAP
// Use TOOLCHANGE_FS_PRIME_SPEED feedrate the first time each extruder is primed
//#define TOOLCHANGE_FS_SLOW_FIRST_PRIME
// Prime on the first T0 (For other tools use TOOLCHANGE_FS_INIT_BEFORE_SWAP)
// Enable with M217 V1 before printing to avoid unwanted priming on host connect
/**
* Prime T0 the first time T0 is sent to the printer:
* [ Power-On -> T0 { Activate & Prime T0 } -> T1 { Retract T0, Activate & Prime T1 } ]
* If disabled, no priming on T0 until switching back to T0 from another extruder:
* [ Power-On -> T0 { T0 Activated } -> T1 { Activate & Prime T1 } -> T0 { Retract T1, Activate & Prime T0 } ]
* Enable with M217 V1 before printing to avoid unwanted priming on host connect.
*/
//#define TOOLCHANGE_FS_PRIME_FIRST_USED
/**
@ -2613,6 +2659,24 @@
#define K_MICROSTEPS 16
#endif
#if AXIS_DRIVER_TYPE_U(TMC26X)
#define U_MAX_CURRENT 1000
#define U_SENSE_RESISTOR 91
#define U_MICROSTEPS 16
#endif
#if AXIS_DRIVER_TYPE_V(TMC26X)
#define V_MAX_CURRENT 1000
#define V_SENSE_RESISTOR 91
#define V_MICROSTEPS 16
#endif
#if AXIS_DRIVER_TYPE_W(TMC26X)
#define W_MAX_CURRENT 1000
#define W_SENSE_RESISTOR 91
#define W_MICROSTEPS 16
#endif
#if AXIS_DRIVER_TYPE_E0(TMC26X)
#define E0_MAX_CURRENT 1000
#define E0_SENSE_RESISTOR 91
@ -2801,6 +2865,36 @@
//#define K_HOLD_MULTIPLIER 0.5
#endif
#if AXIS_IS_TMC(U)
#define U_CURRENT 800
#define U_CURRENT_HOME U_CURRENT
#define U_MICROSTEPS 8
#define U_RSENSE 0.11
#define U_CHAIN_POS -1
//#define U_INTERPOLATE true
//#define U_HOLD_MULTIPLIER 0.5
#endif
#if AXIS_IS_TMC(V)
#define V_CURRENT 800
#define V_CURRENT_HOME V_CURRENT
#define V_MICROSTEPS 8
#define V_RSENSE 0.11
#define V_CHAIN_POS -1
//#define V_INTERPOLATE true
//#define V_HOLD_MULTIPLIER 0.5
#endif
#if AXIS_IS_TMC(W)
#define W_CURRENT 800
#define W_CURRENT_HOME W_CURRENT
#define W_MICROSTEPS 8
#define W_RSENSE 0.11
#define W_CHAIN_POS -1
//#define W_INTERPOLATE true
//#define W_HOLD_MULTIPLIER 0.5
#endif
#if AXIS_IS_TMC(E0)
#define E0_CURRENT 800
#define E0_MICROSTEPS 16
@ -2888,6 +2982,9 @@
//#define I_CS_PIN -1
//#define J_CS_PIN -1
//#define K_CS_PIN -1
//#define U_CS_PIN -1
//#define V_CS_PIN -1
//#define W_CS_PIN -1
//#define E0_CS_PIN -1
//#define E1_CS_PIN -1
//#define E2_CS_PIN -1
@ -2930,6 +3027,9 @@
//#define I_SLAVE_ADDRESS 0
//#define J_SLAVE_ADDRESS 0
//#define K_SLAVE_ADDRESS 0
//#define U_SLAVE_ADDRESS 0
//#define V_SLAVE_ADDRESS 0
//#define W_SLAVE_ADDRESS 0
//#define E0_SLAVE_ADDRESS 0
//#define E1_SLAVE_ADDRESS 0
//#define E2_SLAVE_ADDRESS 0
@ -2957,6 +3057,9 @@
#define STEALTHCHOP_I
#define STEALTHCHOP_J
#define STEALTHCHOP_K
#define STEALTHCHOP_U
#define STEALTHCHOP_V
#define STEALTHCHOP_W
#define STEALTHCHOP_E
/**
@ -2983,9 +3086,12 @@
//#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_I CHOPPER_TIMING // For I Axis
//#define CHOPPER_TIMING_J CHOPPER_TIMING // For J Axis
//#define CHOPPER_TIMING_K CHOPPER_TIMING // For K Axis
//#define CHOPPER_TIMING_U CHOPPER_TIMING // For U Axis
//#define CHOPPER_TIMING_V CHOPPER_TIMING // For V Axis
//#define CHOPPER_TIMING_W CHOPPER_TIMING // For W Axis
//#define CHOPPER_TIMING_E CHOPPER_TIMING // For Extruders (override below)
//#define CHOPPER_TIMING_E1 CHOPPER_TIMING_E
//#define CHOPPER_TIMING_E2 CHOPPER_TIMING_E
@ -3031,9 +3137,12 @@
#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 I_HYBRID_THRESHOLD 3 // [linear=mm/s, rotational=°/s]
#define J_HYBRID_THRESHOLD 3 // [linear=mm/s, rotational=°/s]
#define K_HYBRID_THRESHOLD 3 // [linear=mm/s, rotational=°/s]
#define U_HYBRID_THRESHOLD 3 // [mm/s]
#define V_HYBRID_THRESHOLD 3
#define W_HYBRID_THRESHOLD 3
#define E0_HYBRID_THRESHOLD 30
#define E1_HYBRID_THRESHOLD 30
#define E2_HYBRID_THRESHOLD 30
@ -3083,6 +3192,9 @@
//#define I_STALL_SENSITIVITY 8
//#define J_STALL_SENSITIVITY 8
//#define K_STALL_SENSITIVITY 8
//#define U_STALL_SENSITIVITY 8
//#define V_STALL_SENSITIVITY 8
//#define W_STALL_SENSITIVITY 8
//#define SPI_ENDSTOPS // TMC2130 only
//#define IMPROVE_HOMING_RELIABILITY
#endif
@ -3250,6 +3362,33 @@
#define K_SLEW_RATE 1
#endif
#if AXIS_IS_L64XX(U)
#define U_MICROSTEPS 128
#define U_OVERCURRENT 2000
#define U_STALLCURRENT 1500
#define U_MAX_VOLTAGE 127
#define U_CHAIN_POS -1
#define U_SLEW_RATE 1
#endif
#if AXIS_IS_L64XX(V)
#define V_MICROSTEPS 128
#define V_OVERCURRENT 2000
#define V_STALLCURRENT 1500
#define V_MAX_VOLTAGE 127
#define V_CHAIN_POS -1
#define V_SLEW_RATE 1
#endif
#if AXIS_IS_L64XX(W)
#define W_MICROSTEPS 128
#define W_OVERCURRENT 2000
#define W_STALLCURRENT 1500
#define W_MAX_VOLTAGE 127
#define W_CHAIN_POS -1
#define W_SLEW_RATE 1
#endif
#if AXIS_IS_L64XX(E0)
#define E0_MICROSTEPS 128
#define E0_OVERCURRENT 2000
@ -3455,6 +3594,9 @@
#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, ESP32, and LPC)
// ESP32: If SPINDLE_LASER_PWM_PIN is onboard then <=78125Hz. For I2S expander
// the frequency determines the PWM resolution. 2500Hz = 0-100, 977Hz = 0-255, ...
// (250000 / SPINDLE_LASER_FREQUENCY) = max value.
#endif
//#define AIR_EVACUATION // Cutter Vacuum / Laser Blower motor control with G-codes M10-M11
@ -3531,6 +3673,16 @@
#define LASER_TEST_PULSE_MIN 1 // Used with Laser Control Menu
#define LASER_TEST_PULSE_MAX 999 // Caution: Menu may not show more than 3 characters
/**
* Laser Safety Timeout
*
* The laser should be turned off when there is no movement for a period of time.
* Consider material flammability, cut rate, and G-code order when setting this
* value. Too low and it could turn off during a very slow move; too high and
* the material could ignite.
*/
#define LASER_SAFETY_TIMEOUT_MS 1000 // (ms)
/**
* Enable inline laser power to be handled in the planner / stepper routines.
* Inline power is specified by the I (inline) flag in an M3 command (e.g., M3 S20 I)
@ -3742,6 +3894,9 @@
* Auto-report temperatures with M155 S<seconds>
*/
#define AUTO_REPORT_TEMPERATURES
#if ENABLED(AUTO_REPORT_TEMPERATURES) && TEMP_SENSOR_REDUNDANT
//#define AUTO_REPORT_REDUNDANT // Include the "R" sensor in the auto-report
#endif
/**
* Auto-report position with M154 S<seconds>
@ -4136,7 +4291,8 @@
#define MAX7219_NUMBER_UNITS 1 // Number of Max7219 units in chain.
#define MAX7219_ROTATE 0 // Rotate the display clockwise (in multiples of +/- 90°)
// connector at: right=0 bottom=-90 top=90 left=180
//#define MAX7219_REVERSE_ORDER // The individual LED matrix units may be in reversed order
//#define MAX7219_REVERSE_ORDER // The order of the LED matrix units may be reversed
//#define MAX7219_REVERSE_EACH // The LEDs in each matrix unit row may be reversed
//#define MAX7219_SIDE_BY_SIDE // Big chip+matrix boards can be chained side-by-side
/**
@ -4144,12 +4300,15 @@
* If you add more debug displays, be careful to avoid conflicts!
*/
#define MAX7219_DEBUG_PRINTER_ALIVE // Blink corner LED of 8x8 matrix to show that the firmware is functioning
#define MAX7219_DEBUG_PLANNER_HEAD 3 // Show the planner queue head position on this and the next LED matrix row
#define MAX7219_DEBUG_PLANNER_TAIL 5 // Show the planner queue tail position on this and the next LED matrix row
#define MAX7219_DEBUG_PLANNER_HEAD 2 // Show the planner queue head position on this and the next LED matrix row
#define MAX7219_DEBUG_PLANNER_TAIL 4 // Show the planner queue tail position on this and the next LED matrix row
#define MAX7219_DEBUG_PLANNER_QUEUE 0 // Show the current planner queue depth on this and the next LED matrix row
// If you experience stuttering, reboots, etc. this option can reveal how
// tweaks made to the configuration are affecting the printer in real-time.
#define MAX7219_DEBUG_PROFILE 6 // Display the fraction of CPU time spent in profiled code on this LED matrix
// row. By default idle() is profiled so this shows how "idle" the processor is.
// See class CodeProfiler.
#endif
/**

4
Marlin/Makefile

@ -109,7 +109,7 @@ LIQUID_TWI2 ?= 0
# This defines if Wire is needed
WIRE ?= 0
# This defines if Tone is needed (i.e SPEAKER is defined in Configuration.h)
# This defines if Tone is needed (i.e., SPEAKER is defined in Configuration.h)
# Disabling this (and SPEAKER) saves approximately 350 bytes of memory.
TONE ?= 1
@ -512,7 +512,7 @@ else ifeq ($(HARDWARE_MOTHERBOARD),1324)
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)
# Malyan M180 Mainboard Version 2 (no display function, direct G-code only)
else ifeq ($(HARDWARE_MOTHERBOARD),1327)
# Geeetech GT2560 Rev B for A20(M/T/D)
else ifeq ($(HARDWARE_MOTHERBOARD),1328)

4
Marlin/Version.h

@ -28,7 +28,7 @@
/**
* Marlin release version identifier
*/
//#define SHORT_BUILD_VERSION "2.0.9.3"
//#define SHORT_BUILD_VERSION "2.1"
/**
* Verbose version identifier which should contain a reference to the location
@ -41,7 +41,7 @@
* here we define this default string as the date where the latest release
* version was tagged.
*/
//#define STRING_DISTRIBUTION_DATE "2021-12-25"
//#define STRING_DISTRIBUTION_DATE "2022-06-04"
/**
* Defines a generic printer name to be output to the LCD after booting Marlin.

57
Marlin/src/HAL/AVR/HAL.cpp

@ -23,6 +23,7 @@
#include "../../inc/MarlinConfig.h"
#include "HAL.h"
#include <avr/wdt.h>
#ifdef USBCON
DefaultSerial1 MSerial0(false, Serial);
@ -88,6 +89,62 @@ void MarlinHAL::reboot() {
#endif
}
// ------------------------
// Watchdog Timer
// ------------------------
#if ENABLED(USE_WATCHDOG)
#include <avr/wdt.h>
#include "../../MarlinCore.h"
// Initialize watchdog with 8s timeout, if possible. Otherwise, make it 4s.
void MarlinHAL::watchdog_init() {
#if ENABLED(WATCHDOG_DURATION_8S) && defined(WDTO_8S)
#define WDTO_NS WDTO_8S
#else
#define WDTO_NS WDTO_4S
#endif
#if ENABLED(WATCHDOG_RESET_MANUAL)
// Enable the watchdog timer, but only for the interrupt.
// Take care, as this requires the correct order of operation, with interrupts disabled.
// See the datasheet of any AVR chip for details.
wdt_reset();
cli();
_WD_CONTROL_REG = _BV(_WD_CHANGE_BIT) | _BV(WDE);
_WD_CONTROL_REG = _BV(WDIE) | (WDTO_NS & 0x07) | ((WDTO_NS & 0x08) << 2); // WDTO_NS directly does not work. bit 0-2 are consecutive in the register but the highest value bit is at bit 5
// So worked for up to WDTO_2S
sei();
wdt_reset();
#else
wdt_enable(WDTO_NS); // The function handles the upper bit correct.
#endif
//delay(10000); // test it!
}
//===========================================================================
//=================================== ISR ===================================
//===========================================================================
// Watchdog timer interrupt, called if main program blocks >4sec and manual reset is enabled.
#if ENABLED(WATCHDOG_RESET_MANUAL)
ISR(WDT_vect) {
sei(); // With the interrupt driven serial we need to allow interrupts.
SERIAL_ERROR_MSG(STR_WATCHDOG_FIRED);
minkill(); // interrupt-safe final kill and infinite loop
}
#endif
// Reset watchdog. MUST be called at least every 4 seconds after the
// first watchdog_init or AVR will go into emergency procedures.
void MarlinHAL::watchdog_refresh() { wdt_reset(); }
#endif // USE_WATCHDOG
// ------------------------
// Free Memory Accessor
// ------------------------
#if ENABLED(SDSUPPORT)
#include "../../sd/SdFatUtil.h"

11
Marlin/src/HAL/AVR/HAL.h

@ -19,10 +19,13 @@
*/
#pragma once
/**
* HAL for Arduino AVR
*/
#include "../shared/Marduino.h"
#include "../shared/HAL_SPI.h"
#include "fastio.h"
#include "watchdog.h"
#include "math.h"
#ifdef USBCON
@ -163,7 +166,7 @@ typedef Servo hal_servo_t;
#define strtof strtod
// ------------------------
// Class Utilities
// Free Memory Accessor
// ------------------------
#pragma GCC diagnostic push
@ -185,6 +188,10 @@ public:
// Earliest possible init, before setup()
MarlinHAL() {}
// Watchdog
static void watchdog_init() IF_DISABLED(USE_WATCHDOG, {});
static void watchdog_refresh() IF_DISABLED(USE_WATCHDOG, {});
static void init(); // Called early in setup()
static void init_board() {} // Called less early in setup()
static void reboot(); // Restart the firmware from 0x0

202
Marlin/src/HAL/AVR/Servo.cpp

@ -66,27 +66,26 @@ static volatile int8_t Channel[_Nbr_16timers]; // counter for the s
/************ static functions common to all instances ***********************/
static inline void handle_interrupts(timer16_Sequence_t timer, volatile uint16_t* TCNTn, volatile uint16_t* OCRnA) {
if (Channel[timer] < 0)
*TCNTn = 0; // channel set to -1 indicated that refresh interval completed so reset the timer
else {
if (SERVO_INDEX(timer, Channel[timer]) < ServoCount && SERVO(timer, Channel[timer]).Pin.isActive)
extDigitalWrite(SERVO(timer, Channel[timer]).Pin.nbr, LOW); // pulse this channel low if activated
}
Channel[timer]++; // increment to the next channel
if (SERVO_INDEX(timer, Channel[timer]) < ServoCount && Channel[timer] < SERVOS_PER_TIMER) {
*OCRnA = *TCNTn + SERVO(timer, Channel[timer]).ticks;
if (SERVO(timer, Channel[timer]).Pin.isActive) // check if activated
extDigitalWrite(SERVO(timer, Channel[timer]).Pin.nbr, HIGH); // it's an active channel so pulse it high
static inline void handle_interrupts(const timer16_Sequence_t timer, volatile uint16_t* TCNTn, volatile uint16_t* OCRnA) {
int8_t cho = Channel[timer]; // Handle the prior Channel[timer] first
if (cho < 0) // Channel -1 indicates the refresh interval completed...
*TCNTn = 0; // ...so reset the timer
else if (SERVO_INDEX(timer, cho) < ServoCount) // prior channel handled?
extDigitalWrite(SERVO(timer, cho).Pin.nbr, LOW); // pulse the prior channel LOW
Channel[timer] = ++cho; // Handle the next channel (or 0)
if (cho < SERVOS_PER_TIMER && SERVO_INDEX(timer, cho) < ServoCount) {
*OCRnA = *TCNTn + SERVO(timer, cho).ticks; // set compare to current ticks plus duration
if (SERVO(timer, cho).Pin.isActive) // activated?
extDigitalWrite(SERVO(timer, cho).Pin.nbr, HIGH); // yes: pulse HIGH
}
else {
// finished all channels so wait for the refresh period to expire before starting over
if (((unsigned)*TCNTn) + 4 < usToTicks(REFRESH_INTERVAL)) // allow a few ticks to ensure the next OCR1A not missed
*OCRnA = (unsigned int)usToTicks(REFRESH_INTERVAL);
else
*OCRnA = *TCNTn + 4; // at least REFRESH_INTERVAL has elapsed
Channel[timer] = -1; // this will get incremented at the end of the refresh period to start again at the first channel
const unsigned int cval = ((unsigned)*TCNTn) + 32 / (SERVO_TIMER_PRESCALER), // allow 32 cycles to ensure the next OCR1A not missed
ival = (unsigned int)usToTicks(REFRESH_INTERVAL); // at least REFRESH_INTERVAL has elapsed
*OCRnA = max(cval, ival);
Channel[timer] = -1; // reset the timer counter to 0 on the next call
}
}
@ -123,91 +122,102 @@ static inline void handle_interrupts(timer16_Sequence_t timer, volatile uint16_t
/****************** end of static functions ******************************/
void initISR(timer16_Sequence_t timer) {
#ifdef _useTimer1
if (timer == _timer1) {
TCCR1A = 0; // normal counting mode
TCCR1B = _BV(CS11); // set prescaler of 8
TCNT1 = 0; // clear the timer count
#if defined(__AVR_ATmega8__) || defined(__AVR_ATmega128__)
SBI(TIFR, OCF1A); // clear any pending interrupts;
SBI(TIMSK, OCIE1A); // enable the output compare interrupt
#else
// here if not ATmega8 or ATmega128
SBI(TIFR1, OCF1A); // clear any pending interrupts;
SBI(TIMSK1, OCIE1A); // enable the output compare interrupt
#endif
#ifdef WIRING
timerAttach(TIMER1OUTCOMPAREA_INT, Timer1Service);
#endif
}
#endif
#ifdef _useTimer3
if (timer == _timer3) {
TCCR3A = 0; // normal counting mode
TCCR3B = _BV(CS31); // set prescaler of 8
TCNT3 = 0; // clear the timer count
#ifdef __AVR_ATmega128__
SBI(TIFR, OCF3A); // clear any pending interrupts;
SBI(ETIMSK, OCIE3A); // enable the output compare interrupt
#else
SBI(TIFR3, OCF3A); // clear any pending interrupts;
SBI(TIMSK3, OCIE3A); // enable the output compare interrupt
#endif
#ifdef WIRING
timerAttach(TIMER3OUTCOMPAREA_INT, Timer3Service); // for Wiring platform only
#endif
}
#endif
#ifdef _useTimer4
if (timer == _timer4) {
TCCR4A = 0; // normal counting mode
TCCR4B = _BV(CS41); // set prescaler of 8
TCNT4 = 0; // clear the timer count
TIFR4 = _BV(OCF4A); // clear any pending interrupts;
TIMSK4 = _BV(OCIE4A); // enable the output compare interrupt
}
#endif
#ifdef _useTimer5
if (timer == _timer5) {
TCCR5A = 0; // normal counting mode
TCCR5B = _BV(CS51); // set prescaler of 8
TCNT5 = 0; // clear the timer count
TIFR5 = _BV(OCF5A); // clear any pending interrupts;
TIMSK5 = _BV(OCIE5A); // enable the output compare interrupt
}
#endif
}
void finISR(timer16_Sequence_t timer) {
// Disable use of the given timer
#ifdef WIRING
if (timer == _timer1) {
CBI(
#if defined(__AVR_ATmega1281__) || defined(__AVR_ATmega2561__)
TIMSK1
void initISR(const timer16_Sequence_t timer_index) {
switch (timer_index) {
default: break;
#ifdef _useTimer1
case _timer1:
TCCR1A = 0; // normal counting mode
TCCR1B = _BV(CS11); // set prescaler of 8
TCNT1 = 0; // clear the timer count
#if defined(__AVR_ATmega8__) || defined(__AVR_ATmega128__)
SBI(TIFR, OCF1A); // clear any pending interrupts;
SBI(TIMSK, OCIE1A); // enable the output compare interrupt
#else
TIMSK
// here if not ATmega8 or ATmega128
SBI(TIFR1, OCF1A); // clear any pending interrupts;
SBI(TIMSK1, OCIE1A); // enable the output compare interrupt
#endif
, OCIE1A); // disable timer 1 output compare interrupt
timerDetach(TIMER1OUTCOMPAREA_INT);
}
else if (timer == _timer3) {
CBI(
#if defined(__AVR_ATmega1281__) || defined(__AVR_ATmega2561__)
TIMSK3
#ifdef WIRING
timerAttach(TIMER1OUTCOMPAREA_INT, Timer1Service);
#endif
break;
#endif
#ifdef _useTimer3
case _timer3:
TCCR3A = 0; // normal counting mode
TCCR3B = _BV(CS31); // set prescaler of 8
TCNT3 = 0; // clear the timer count
#ifdef __AVR_ATmega128__
SBI(TIFR, OCF3A); // clear any pending interrupts;
SBI(ETIMSK, OCIE3A); // enable the output compare interrupt
#else
ETIMSK
SBI(TIFR3, OCF3A); // clear any pending interrupts;
SBI(TIMSK3, OCIE3A); // enable the output compare interrupt
#endif
#ifdef WIRING
timerAttach(TIMER3OUTCOMPAREA_INT, Timer3Service); // for Wiring platform only
#endif
, OCIE3A); // disable the timer3 output compare A interrupt
timerDetach(TIMER3OUTCOMPAREA_INT);
break;
#endif
#ifdef _useTimer4
case _timer4:
TCCR4A = 0; // normal counting mode
TCCR4B = _BV(CS41); // set prescaler of 8
TCNT4 = 0; // clear the timer count
TIFR4 = _BV(OCF4A); // clear any pending interrupts;
TIMSK4 = _BV(OCIE4A); // enable the output compare interrupt
break;
#endif
#ifdef _useTimer5
case _timer5:
TCCR5A = 0; // normal counting mode
TCCR5B = _BV(CS51); // set prescaler of 8
TCNT5 = 0; // clear the timer count
TIFR5 = _BV(OCF5A); // clear any pending interrupts;
TIMSK5 = _BV(OCIE5A); // enable the output compare interrupt
break;
#endif
}
}
void finISR(const timer16_Sequence_t timer_index) {
// Disable use of the given timer
#ifdef WIRING
switch (timer_index) {
default: break;
case _timer1:
CBI(
#if defined(__AVR_ATmega1281__) || defined(__AVR_ATmega2561__)
TIMSK1
#else
TIMSK
#endif
, OCIE1A // disable timer 1 output compare interrupt
);
timerDetach(TIMER1OUTCOMPAREA_INT);
break;
case _timer3:
CBI(
#if defined(__AVR_ATmega1281__) || defined(__AVR_ATmega2561__)
TIMSK3
#else
ETIMSK
#endif
, OCIE3A // disable the timer3 output compare A interrupt
);
timerDetach(TIMER3OUTCOMPAREA_INT);
break;
}
#else // !WIRING
// For arduino - in future: call here to a currently undefined function to reset the timer
UNUSED(timer);
UNUSED(timer_index);
#endif
}

45
Marlin/src/HAL/AVR/endstop_interrupts.h

@ -213,6 +213,51 @@ void setup_endstop_interrupts() {
pciSetup(K_MIN_PIN);
#endif
#endif
#if HAS_U_MAX
#if (digitalPinToInterrupt(U_MAX_PIN) != NOT_AN_INTERRUPT)
_ATTACH(U_MAX_PIN);
#else
static_assert(digitalPinHasPCICR(U_MAX_PIN), "U_MAX_PIN is not interrupt-capable");
pciSetup(U_MAX_PIN);
#endif
#elif HAS_U_MIN
#if (digitalPinToInterrupt(U_MIN_PIN) != NOT_AN_INTERRUPT)
_ATTACH(U_MIN_PIN);
#else
static_assert(digitalPinHasPCICR(U_MIN_PIN), "U_MIN_PIN is not interrupt-capable");
pciSetup(U_MIN_PIN);
#endif
#endif
#if HAS_V_MAX
#if (digitalPinToInterrupt(V_MAX_PIN) != NOT_AN_INTERRUPT)
_ATTACH(V_MAX_PIN);
#else
static_assert(digitalPinHasPCICR(V_MAX_PIN), "V_MAX_PIN is not interrupt-capable");
pciSetup(V_MAX_PIN);
#endif
#elif HAS_V_MIN
#if (digitalPinToInterrupt(V_MIN_PIN) != NOT_AN_INTERRUPT)
_ATTACH(V_MIN_PIN);
#else
static_assert(digitalPinHasPCICR(V_MIN_PIN), "V_MIN_PIN is not interrupt-capable");
pciSetup(V_MIN_PIN);
#endif
#endif
#if HAS_W_MAX
#if (digitalPinToInterrupt(W_MAX_PIN) != NOT_AN_INTERRUPT)
_ATTACH(W_MAX_PIN);
#else
static_assert(digitalPinHasPCICR(W_MAX_PIN), "W_MAX_PIN is not interrupt-capable");
pciSetup(W_MAX_PIN);
#endif
#elif HAS_W_MIN
#if (digitalPinToInterrupt(W_MIN_PIN) != NOT_AN_INTERRUPT)
_ATTACH(W_MIN_PIN);
#else
static_assert(digitalPinHasPCICR(W_MIN_PIN), "W_MIN_PIN is not interrupt-capable");
pciSetup(W_MIN_PIN);
#endif
#endif
#if HAS_X2_MAX
#if (digitalPinToInterrupt(X2_MAX_PIN) != NOT_AN_INTERRUPT)
_ATTACH(X2_MAX_PIN);

6
Marlin/src/HAL/AVR/inc/SanityCheck.h

@ -37,16 +37,16 @@
|| X_ENA_PIN == N || Y_ENA_PIN == N || Z_ENA_PIN == N \
)
#if CONF_SERIAL_IS(0) // D0-D1. No known conflicts.
#endif
#endif
#if CONF_SERIAL_IS(1) && (CHECK_SERIAL_PIN(18) || CHECK_SERIAL_PIN(19))
#error "Serial Port 1 pin D18 and/or D19 conflicts with another pin on the board."
#endif
#endif
#if CONF_SERIAL_IS(2) && (CHECK_SERIAL_PIN(16) || CHECK_SERIAL_PIN(17))
#error "Serial Port 2 pin D16 and/or D17 conflicts with another pin on the board."
#endif
#if CONF_SERIAL_IS(3) && (CHECK_SERIAL_PIN(14) || CHECK_SERIAL_PIN(15))
#error "Serial Port 3 pin D14 and/or D15 conflicts with another pin on the board."
#endif
#endif
#undef CHECK_SERIAL_PIN
/**

2
Marlin/src/HAL/AVR/pinsDebug.h

@ -74,7 +74,7 @@
#define MULTI_NAME_PAD 26 // space needed to be pretty if not first name assigned to a pin
void PRINT_ARRAY_NAME(uint8_t x) {
char *name_mem_pointer = (char*)pgm_read_ptr(&pin_array[x].name);
PGM_P const name_mem_pointer = (PGM_P)pgm_read_ptr(&pin_array[x].name);
LOOP_L_N(y, MAX_NAME_LENGTH) {
char temp_char = pgm_read_byte(name_mem_pointer + y);
if (temp_char != 0)

70
Marlin/src/HAL/AVR/watchdog.cpp

@ -1,70 +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 <https://www.gnu.org/licenses/>.
*
*/
#ifdef __AVR__
#include "../../inc/MarlinConfig.h"
#if ENABLED(USE_WATCHDOG)
#include "watchdog.h"
#include "../../MarlinCore.h"
// Initialize watchdog with 8s timeout, if possible. Otherwise, make it 4s.
void watchdog_init() {
#if ENABLED(WATCHDOG_DURATION_8S) && defined(WDTO_8S)
#define WDTO_NS WDTO_8S
#else
#define WDTO_NS WDTO_4S
#endif
#if ENABLED(WATCHDOG_RESET_MANUAL)
// Enable the watchdog timer, but only for the interrupt.
// Take care, as this requires the correct order of operation, with interrupts disabled.
// See the datasheet of any AVR chip for details.
wdt_reset();
cli();
_WD_CONTROL_REG = _BV(_WD_CHANGE_BIT) | _BV(WDE);
_WD_CONTROL_REG = _BV(WDIE) | (WDTO_NS & 0x07) | ((WDTO_NS & 0x08) << 2); // WDTO_NS directly does not work. bit 0-2 are consecutive in the register but the highest value bit is at bit 5
// So worked for up to WDTO_2S
sei();
wdt_reset();
#else
wdt_enable(WDTO_NS); // The function handles the upper bit correct.
#endif
//delay(10000); // test it!
}
//===========================================================================
//=================================== ISR ===================================
//===========================================================================
// Watchdog timer interrupt, called if main program blocks >4sec and manual reset is enabled.
#if ENABLED(WATCHDOG_RESET_MANUAL)
ISR(WDT_vect) {
sei(); // With the interrupt driven serial we need to allow interrupts.
SERIAL_ERROR_MSG(STR_WATCHDOG_FIRED);
minkill(); // interrupt-safe final kill and infinite loop
}
#endif
#endif // USE_WATCHDOG
#endif // __AVR__

31
Marlin/src/HAL/AVR/watchdog.h

@ -1,31 +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 <https://www.gnu.org/licenses/>.
*
*/
#pragma once
#include <avr/wdt.h>
// Initialize watchdog with a 4 second interrupt time
void watchdog_init();
// Reset watchdog. MUST be called at least every 4 seconds after the
// first watchdog_init or AVR will go into emergency procedures.
inline void HAL_watchdog_refresh() { wdt_reset(); }

110
Marlin/src/HAL/DUE/HAL.cpp

@ -25,7 +25,7 @@
#ifdef ARDUINO_ARCH_SAM
#include "../../inc/MarlinConfig.h"
#include "HAL.h"
#include "../../MarlinCore.h"
#include <Wire.h>
#include "usb/usb_task.h"
@ -40,14 +40,15 @@ 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
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
#endif
usb_task_init();
usb_task_init(); // Initialize the USB stack
TERN_(POSTMORTEM_DEBUGGING, install_min_serial()); // Install the min serial handler
}
@ -72,6 +73,103 @@ uint8_t MarlinHAL::get_reset_source() {
void MarlinHAL::reboot() { rstc_start_software_reset(RSTC); }
// ------------------------
// Watchdog Timer
// ------------------------
#if ENABLED(USE_WATCHDOG)
// Initialize watchdog - On SAM3X, Watchdog was already configured
// and enabled or disabled at startup, so no need to reconfigure it
// here.
void MarlinHAL::watchdog_init() { WDT_Restart(WDT); } // Reset watchdog to start clean
// Reset watchdog. MUST be called at least every 4 seconds after the
// first watchdog_init or AVR will go into emergency procedures.
void MarlinHAL::watchdog_refresh() { watchdogReset(); }
#endif
// Override Arduino runtime to either config or disable the watchdog
//
// We need to configure the watchdog as soon as possible in the boot
// process, because watchdog initialization at hardware reset on SAM3X8E
// is unreliable, and there is risk of unintended resets if we delay
// that initialization to a later time.
void watchdogSetup() {
#if ENABLED(USE_WATCHDOG)
// 4 seconds timeout
uint32_t timeout = TERN(WATCHDOG_DURATION_8S, 8000, 4000);
// Calculate timeout value in WDT counter ticks: This assumes
// the slow clock is running at 32.768 kHz watchdog
// frequency is therefore 32768 / 128 = 256 Hz
timeout = (timeout << 8) / 1000;
if (timeout == 0)
timeout = 1;
else if (timeout > 0xFFF)
timeout = 0xFFF;
// We want to enable the watchdog with the specified timeout
uint32_t value =
WDT_MR_WDV(timeout) | // With the specified timeout
WDT_MR_WDD(timeout) | // and no invalid write window
#if !(SAMV70 || SAMV71 || SAME70 || SAMS70)
WDT_MR_WDRPROC | // WDT fault resets processor only - We want
// to keep PIO controller state
#endif
WDT_MR_WDDBGHLT | // WDT stops in debug state.
WDT_MR_WDIDLEHLT; // WDT stops in idle state.
#if ENABLED(WATCHDOG_RESET_MANUAL)
// We enable the watchdog timer, but only for the interrupt.
// Configure WDT to only trigger an interrupt
value |= WDT_MR_WDFIEN; // Enable WDT fault interrupt.
// Disable WDT interrupt (just in case, to avoid triggering it!)
NVIC_DisableIRQ(WDT_IRQn);
// We NEED memory barriers to ensure Interrupts are actually disabled!
// ( https://dzone.com/articles/nvic-disabling-interrupts-on-arm-cortex-m-and-the )
__DSB();
__ISB();
// Initialize WDT with the given parameters
WDT_Enable(WDT, value);
// Configure and enable WDT interrupt.
NVIC_ClearPendingIRQ(WDT_IRQn);
NVIC_SetPriority(WDT_IRQn, 0); // Use highest priority, so we detect all kinds of lockups
NVIC_EnableIRQ(WDT_IRQn);
#else
// a WDT fault triggers a reset
value |= WDT_MR_WDRSTEN;
// Initialize WDT with the given parameters
WDT_Enable(WDT, value);
#endif
// Reset the watchdog
WDT_Restart(WDT);
#else
// Make sure to completely disable the Watchdog
WDT_Disable(WDT);
#endif
}
// ------------------------
// Free Memory Accessor
// ------------------------
extern "C" {
extern unsigned int _ebss; // end of bss section
}
@ -82,6 +180,10 @@ int freeMemory() {
return (int)&free_memory - (heap_end ?: (int)&_ebss);
}
// ------------------------
// Serial Ports
// ------------------------
// Forward the default serial ports
#if USING_HW_SERIAL0
DefaultSerial1 MSerial0(false, Serial);

11
Marlin/src/HAL/DUE/HAL.h

@ -32,7 +32,6 @@
#include "../shared/math_32bit.h"
#include "../shared/HAL_SPI.h"
#include "fastio.h"
#include "watchdog.h"
#include <stdint.h>
@ -176,9 +175,13 @@ 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
// Watchdog
static void watchdog_init() IF_DISABLED(USE_WATCHDOG, {});
static void watchdog_refresh() IF_DISABLED(USE_WATCHDOG, {});
static void init(); // Called early in setup()
static void init_board(); // Called less early in setup()
static void reboot(); // Restart the firmware
// Interrupts
static bool isr_state() { return !__get_PRIMASK(); }

2
Marlin/src/HAL/DUE/HAL_SPI.cpp

@ -31,8 +31,6 @@
/**
* HAL for Arduino Due and compatible (SAM3X8E)
*
* For ARDUINO_ARCH_SAM
*/
#ifdef ARDUINO_ARCH_SAM

138
Marlin/src/HAL/DUE/Servo.cpp

@ -47,12 +47,12 @@
#include "../shared/servo.h"
#include "../shared/servo_private.h"
static volatile int8_t Channel[_Nbr_16timers]; // counter for the servo being pulsed for each timer (or -1 if refresh interval)
static Flags<_Nbr_16timers> DisablePending; // ISR should disable the timer at the next timer reset
// ------------------------
/// Interrupt handler for the TC0 channel 1.
// ------------------------
void Servo_Handler(timer16_Sequence_t timer, Tc *pTc, uint8_t channel);
void Servo_Handler(const timer16_Sequence_t, Tc*, const uint8_t);
#ifdef _useTimer1
void HANDLER_FOR_TIMER1() { Servo_Handler(_timer1, TC_FOR_TIMER1, CHANNEL_FOR_TIMER1); }
@ -70,88 +70,92 @@ void Servo_Handler(timer16_Sequence_t timer, Tc *pTc, uint8_t channel);
void HANDLER_FOR_TIMER5() { Servo_Handler(_timer5, TC_FOR_TIMER5, CHANNEL_FOR_TIMER5); }
#endif
void Servo_Handler(timer16_Sequence_t timer, Tc *tc, uint8_t channel) {
// clear interrupt
tc->TC_CHANNEL[channel].TC_SR;
if (Channel[timer] < 0)
tc->TC_CHANNEL[channel].TC_CCR |= TC_CCR_SWTRG; // channel set to -1 indicated that refresh interval completed so reset the timer
else if (SERVO_INDEX(timer, Channel[timer]) < ServoCount && SERVO(timer, Channel[timer]).Pin.isActive)
extDigitalWrite(SERVO(timer, Channel[timer]).Pin.nbr, LOW); // pulse this channel low if activated
Channel[timer]++; // increment to the next channel
if (SERVO_INDEX(timer, Channel[timer]) < ServoCount && Channel[timer] < SERVOS_PER_TIMER) {
tc->TC_CHANNEL[channel].TC_RA = tc->TC_CHANNEL[channel].TC_CV + SERVO(timer,Channel[timer]).ticks;
if (SERVO(timer,Channel[timer]).Pin.isActive) // check if activated
extDigitalWrite(SERVO(timer, Channel[timer]).Pin.nbr, HIGH); // its an active channel so pulse it high
void Servo_Handler(const timer16_Sequence_t timer, Tc *tc, const uint8_t channel) {
static int8_t Channel[_Nbr_16timers]; // Servo counters to pulse (or -1 for refresh interval)
int8_t cho = Channel[timer]; // Handle the prior Channel[timer] first
if (cho < 0) { // Channel -1 indicates the refresh interval completed...
tc->TC_CHANNEL[channel].TC_CCR |= TC_CCR_SWTRG; // ...so reset the timer
if (DisablePending[timer]) {
// Disabling only after the full servo period expires prevents
// pulses being too close together if immediately re-enabled.
DisablePending.clear(timer);
TC_Stop(tc, channel);
tc->TC_CHANNEL[channel].TC_SR; // clear interrupt
return;
}
}
else if (SERVO_INDEX(timer, cho) < ServoCount) // prior channel handled?
extDigitalWrite(SERVO(timer, cho).Pin.nbr, LOW); // pulse the prior channel LOW
Channel[timer] = ++cho; // go to the next channel (or 0)
if (cho < SERVOS_PER_TIMER && SERVO_INDEX(timer, cho) < ServoCount) {
tc->TC_CHANNEL[channel].TC_RA = tc->TC_CHANNEL[channel].TC_CV + SERVO(timer, cho).ticks;
if (SERVO(timer, cho).Pin.isActive) // activated?
extDigitalWrite(SERVO(timer, cho).Pin.nbr, HIGH); // yes: pulse HIGH
}
else {
// finished all channels so wait for the refresh period to expire before starting over
tc->TC_CHANNEL[channel].TC_RA =
tc->TC_CHANNEL[channel].TC_CV < usToTicks(REFRESH_INTERVAL) - 4
? (unsigned int)usToTicks(REFRESH_INTERVAL) // allow a few ticks to ensure the next OCR1A not missed
: tc->TC_CHANNEL[channel].TC_CV + 4; // at least REFRESH_INTERVAL has elapsed
Channel[timer] = -1; // this will get incremented at the end of the refresh period to start again at the first channel
const unsigned int cval = tc->TC_CHANNEL[channel].TC_CV + 128 / (SERVO_TIMER_PRESCALER), // allow 128 cycles to ensure the next CV not missed
ival = (unsigned int)usToTicks(REFRESH_INTERVAL); // at least REFRESH_INTERVAL has elapsed
tc->TC_CHANNEL[channel].TC_RA = max(cval, ival);
Channel[timer] = -1; // reset the timer CCR on the next call
}
tc->TC_CHANNEL[channel].TC_SR; // clear interrupt
}
static void _initISR(Tc *tc, uint32_t channel, uint32_t id, IRQn_Type irqn) {
pmc_enable_periph_clk(id);
TC_Configure(tc, channel,
TC_CMR_TCCLKS_TIMER_CLOCK3 | // MCK/32
TC_CMR_WAVE | // Waveform mode
TC_CMR_WAVSEL_UP_RC ); // Counter running up and reset when equals to RC
/* 84MHz, MCK/32, for 1.5ms: 3937 */
TC_SetRA(tc, channel, 2625); // 1ms
/* Configure and enable interrupt */
TC_CMR_WAVE // Waveform mode
| TC_CMR_WAVSEL_UP_RC // Counter running up and reset when equal to RC
| (SERVO_TIMER_PRESCALER == 2 ? TC_CMR_TCCLKS_TIMER_CLOCK1 : 0) // MCK/2
| (SERVO_TIMER_PRESCALER == 8 ? TC_CMR_TCCLKS_TIMER_CLOCK2 : 0) // MCK/8
| (SERVO_TIMER_PRESCALER == 32 ? TC_CMR_TCCLKS_TIMER_CLOCK3 : 0) // MCK/32
| (SERVO_TIMER_PRESCALER == 128 ? TC_CMR_TCCLKS_TIMER_CLOCK4 : 0) // MCK/128
);
// Wait 1ms before the first ISR
TC_SetRA(tc, channel, (F_CPU) / (SERVO_TIMER_PRESCALER) / 1000UL); // 1ms
// Configure and enable interrupt
NVIC_EnableIRQ(irqn);
// TC_IER_CPAS: RA Compare
tc->TC_CHANNEL[channel].TC_IER = TC_IER_CPAS;
tc->TC_CHANNEL[channel].TC_IER = TC_IER_CPAS; // TC_IER_CPAS: RA Compare
// Enables the timer clock and performs a software reset to start the counting
TC_Start(tc, channel);
}
void initISR(timer16_Sequence_t timer) {
#ifdef _useTimer1
if (timer == _timer1)
_initISR(TC_FOR_TIMER1, CHANNEL_FOR_TIMER1, ID_TC_FOR_TIMER1, IRQn_FOR_TIMER1);
#endif
#ifdef _useTimer2
if (timer == _timer2)
_initISR(TC_FOR_TIMER2, CHANNEL_FOR_TIMER2, ID_TC_FOR_TIMER2, IRQn_FOR_TIMER2);
#endif
#ifdef _useTimer3
if (timer == _timer3)
_initISR(TC_FOR_TIMER3, CHANNEL_FOR_TIMER3, ID_TC_FOR_TIMER3, IRQn_FOR_TIMER3);
#endif
#ifdef _useTimer4
if (timer == _timer4)
_initISR(TC_FOR_TIMER4, CHANNEL_FOR_TIMER4, ID_TC_FOR_TIMER4, IRQn_FOR_TIMER4);
#endif
#ifdef _useTimer5
if (timer == _timer5)
_initISR(TC_FOR_TIMER5, CHANNEL_FOR_TIMER5, ID_TC_FOR_TIMER5, IRQn_FOR_TIMER5);
#endif
void initISR(const timer16_Sequence_t timer_index) {
CRITICAL_SECTION_START();
const bool disable_soon = DisablePending[timer_index];
DisablePending.clear(timer_index);
CRITICAL_SECTION_END();
if (!disable_soon) switch (timer_index) {
default: break;
#ifdef _useTimer1
case _timer1: return _initISR(TC_FOR_TIMER1, CHANNEL_FOR_TIMER1, ID_TC_FOR_TIMER1, IRQn_FOR_TIMER1);
#endif
#ifdef _useTimer2
case _timer2: return _initISR(TC_FOR_TIMER2, CHANNEL_FOR_TIMER2, ID_TC_FOR_TIMER2, IRQn_FOR_TIMER2);
#endif
#ifdef _useTimer3
case _timer3: return _initISR(TC_FOR_TIMER3, CHANNEL_FOR_TIMER3, ID_TC_FOR_TIMER3, IRQn_FOR_TIMER3);
#endif
#ifdef _useTimer4
case _timer4: return _initISR(TC_FOR_TIMER4, CHANNEL_FOR_TIMER4, ID_TC_FOR_TIMER4, IRQn_FOR_TIMER4);
#endif
#ifdef _useTimer5
case _timer5: return _initISR(TC_FOR_TIMER5, CHANNEL_FOR_TIMER5, ID_TC_FOR_TIMER5, IRQn_FOR_TIMER5);
#endif
}
}
void finISR(timer16_Sequence_t) {
#ifdef _useTimer1
TC_Stop(TC_FOR_TIMER1, CHANNEL_FOR_TIMER1);
#endif
#ifdef _useTimer2
TC_Stop(TC_FOR_TIMER2, CHANNEL_FOR_TIMER2);
#endif
#ifdef _useTimer3
TC_Stop(TC_FOR_TIMER3, CHANNEL_FOR_TIMER3);
#endif
#ifdef _useTimer4
TC_Stop(TC_FOR_TIMER4, CHANNEL_FOR_TIMER4);
#endif
#ifdef _useTimer5
TC_Stop(TC_FOR_TIMER5, CHANNEL_FOR_TIMER5);
#endif
void finISR(const timer16_Sequence_t timer_index) {
// Timer is disabled from the ISR, to ensure proper final pulse length.
DisablePending.set(timer_index);
}
#endif // HAS_SERVOS

2
Marlin/src/HAL/DUE/ServoTimers.h

@ -37,7 +37,7 @@
#define _useTimer5
#define TRIM_DURATION 2 // compensation ticks to trim adjust for digitalWrite delays
#define SERVO_TIMER_PRESCALER 32 // timer prescaler
#define SERVO_TIMER_PRESCALER 2 // timer prescaler
/*
TC0, chan 0 => TC0_Handler

36
Marlin/src/HAL/DUE/eeprom_flash.cpp

@ -199,8 +199,7 @@ static bool ee_PageWrite(uint16_t page, const void *data) {
for (i = 0; i <PageSize >> 2; i++)
pageContents[i] = (((uint32_t*)data)[i]) | (~(pageContents[i] ^ ((uint32_t*)data)[i]));
DEBUG_ECHO_START();
DEBUG_ECHOLNPGM("EEPROM PageWrite ", page);
DEBUG_ECHO_MSG("EEPROM PageWrite ", page);
DEBUG_ECHOLNPGM(" in FLASH address ", (uint32_t)addrflash);
DEBUG_ECHOLNPGM(" base address ", (uint32_t)getFlashStorage(0));
DEBUG_FLUSH();
@ -245,8 +244,7 @@ static bool ee_PageWrite(uint16_t page, const void *data) {
// Reenable interrupts
__enable_irq();
DEBUG_ECHO_START();
DEBUG_ECHOLNPGM("EEPROM Unlock failure for page ", page);
DEBUG_ECHO_MSG("EEPROM Unlock failure for page ", page);
return false;
}
@ -270,8 +268,7 @@ static bool ee_PageWrite(uint16_t page, const void *data) {
// Reenable interrupts
__enable_irq();
DEBUG_ECHO_START();
DEBUG_ECHOLNPGM("EEPROM Write failure for page ", page);
DEBUG_ECHO_MSG("EEPROM Write failure for page ", page);
return false;
}
@ -286,8 +283,7 @@ static bool ee_PageWrite(uint16_t page, const void *data) {
if (memcmp(getFlashStorage(page),data,PageSize)) {
#ifdef EE_EMU_DEBUG
DEBUG_ECHO_START();
DEBUG_ECHOLNPGM("EEPROM Verify Write failure for page ", page);
DEBUG_ECHO_MSG("EEPROM Verify Write failure for page ", page);
ee_Dump( page, (uint32_t *)addrflash);
ee_Dump(-page, data);
@ -325,8 +321,7 @@ static bool ee_PageErase(uint16_t page) {
uint16_t i;
uint32_t addrflash = uint32_t(getFlashStorage(page));
DEBUG_ECHO_START();
DEBUG_ECHOLNPGM("EEPROM PageErase ", page);
DEBUG_ECHO_MSG("EEPROM PageErase ", page);
DEBUG_ECHOLNPGM(" in FLASH address ", (uint32_t)addrflash);
DEBUG_ECHOLNPGM(" base address ", (uint32_t)getFlashStorage(0));
DEBUG_FLUSH();
@ -370,8 +365,7 @@ static bool ee_PageErase(uint16_t page) {
// Reenable interrupts
__enable_irq();
DEBUG_ECHO_START();
DEBUG_ECHOLNPGM("EEPROM Unlock failure for page ",page);
DEBUG_ECHO_MSG("EEPROM Unlock failure for page ",page);
return false;
}
@ -394,8 +388,7 @@ static bool ee_PageErase(uint16_t page) {
// Reenable interrupts
__enable_irq();
DEBUG_ECHO_START();
DEBUG_ECHOLNPGM("EEPROM Erase failure for page ",page);
DEBUG_ECHO_MSG("EEPROM Erase failure for page ",page);
return false;
}
@ -410,8 +403,7 @@ static bool ee_PageErase(uint16_t page) {
uint32_t * aligned_src = (uint32_t *) addrflash;
for (i = 0; i < PageSize >> 2; i++) {
if (*aligned_src++ != 0xFFFFFFFF) {
DEBUG_ECHO_START();
DEBUG_ECHOLNPGM("EEPROM Verify Erase failure for page ",page);
DEBUG_ECHO_MSG("EEPROM Verify Erase failure for page ",page);
ee_Dump(page, (uint32_t *)addrflash);
return false;
}
@ -921,8 +913,7 @@ static void ee_Init() {
// If all groups seem to be used, default to first group
if (curGroup >= GroupCount) curGroup = 0;
DEBUG_ECHO_START();
DEBUG_ECHOLNPGM("EEPROM Current Group: ",curGroup);
DEBUG_ECHO_MSG("EEPROM Current Group: ",curGroup);
DEBUG_FLUSH();
// Now, validate that all the other group pages are empty
@ -931,8 +922,7 @@ static void ee_Init() {
for (int page = 0; page < PagesPerGroup; page++) {
if (!ee_IsPageClean(grp * PagesPerGroup + page)) {
DEBUG_ECHO_START();
DEBUG_ECHOLNPGM("EEPROM Page ", page, " not clean on group ", grp);
DEBUG_ECHO_MSG("EEPROM Page ", page, " not clean on group ", grp);
DEBUG_FLUSH();
ee_PageErase(grp * PagesPerGroup + page);
}
@ -948,15 +938,13 @@ static void ee_Init() {
}
}
DEBUG_ECHO_START();
DEBUG_ECHOLNPGM("EEPROM Active page: ", curPage);
DEBUG_ECHO_MSG("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_ECHOLNPGM("EEPROM Page ", page, " not clean on active group ", curGroup);
DEBUG_ECHO_MSG("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);

6
Marlin/src/HAL/DUE/endstop_interrupts.h

@ -70,4 +70,10 @@ void setup_endstop_interrupts() {
TERN_(HAS_J_MIN, _ATTACH(J_MIN_PIN));
TERN_(HAS_K_MAX, _ATTACH(K_MAX_PIN));
TERN_(HAS_K_MIN, _ATTACH(K_MIN_PIN));
TERN_(HAS_U_MAX, _ATTACH(U_MAX_PIN));
TERN_(HAS_U_MIN, _ATTACH(U_MIN_PIN));
TERN_(HAS_V_MAX, _ATTACH(V_MAX_PIN));
TERN_(HAS_V_MIN, _ATTACH(V_MIN_PIN));
TERN_(HAS_W_MAX, _ATTACH(W_MAX_PIN));
TERN_(HAS_W_MIN, _ATTACH(W_MIN_PIN));
}

6
Marlin/src/HAL/DUE/inc/SanityCheck.h

@ -37,16 +37,16 @@
|| X_ENA_PIN == N || Y_ENA_PIN == N || Z_ENA_PIN == N \
)
#if CONF_SERIAL_IS(0) // D0-D1. No known conflicts.
#endif
#endif
#if CONF_SERIAL_IS(1) && (CHECK_SERIAL_PIN(18) || CHECK_SERIAL_PIN(19))
#error "Serial Port 1 pin D18 and/or D19 conflicts with another pin on the board."
#endif
#endif
#if CONF_SERIAL_IS(2) && (CHECK_SERIAL_PIN(16) || CHECK_SERIAL_PIN(17))
#error "Serial Port 2 pin D16 and/or D17 conflicts with another pin on the board."
#endif
#if CONF_SERIAL_IS(3) && (CHECK_SERIAL_PIN(14) || CHECK_SERIAL_PIN(15))
#error "Serial Port 3 pin D14 and/or D15 conflicts with another pin on the board."
#endif
#endif
#undef CHECK_SERIAL_PIN
/**

11
Marlin/src/HAL/DUE/timers.cpp

@ -89,10 +89,17 @@ void HAL_timer_start(const uint8_t timer_num, const uint32_t frequency) {
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);
TC_Configure(tc, channel,
TC_CMR_WAVE
| TC_CMR_WAVSEL_UP_RC
| (HAL_TIMER_PRESCALER == 2 ? TC_CMR_TCCLKS_TIMER_CLOCK1 : 0)
| (HAL_TIMER_PRESCALER == 8 ? TC_CMR_TCCLKS_TIMER_CLOCK2 : 0)
| (HAL_TIMER_PRESCALER == 32 ? TC_CMR_TCCLKS_TIMER_CLOCK3 : 0)
| (HAL_TIMER_PRESCALER == 128 ? TC_CMR_TCCLKS_TIMER_CLOCK4 : 0)
);
// Set compare value
TC_SetRC(tc, channel, VARIANT_MCK / 2 / frequency);
TC_SetRC(tc, channel, VARIANT_MCK / (HAL_TIMER_PRESCALER) / frequency);
// And start timer
TC_Start(tc, channel);

3
Marlin/src/HAL/DUE/timers.h

@ -35,7 +35,8 @@
typedef uint32_t hal_timer_t;
#define HAL_TIMER_TYPE_MAX 0xFFFFFFFF
#define HAL_TIMER_RATE ((F_CPU) / 2) // frequency of timers peripherals
#define HAL_TIMER_PRESCALER 2
#define HAL_TIMER_RATE ((F_CPU) / (HAL_TIMER_PRESCALER)) // frequency of timers peripherals
#ifndef MF_TIMER_STEP
#define MF_TIMER_STEP 2 // Timer Index for Stepper

2
Marlin/src/HAL/DUE/usb/compiler.h

@ -1059,7 +1059,7 @@ static inline void convert_64_bit_to_byte_array(uint64_t value, uint8_t *data)
while (val_index < 8)
{
data[val_index++] = value & 0xFF;
value = value >> 8;
value >>= 8;
}
}

114
Marlin/src/HAL/DUE/watchdog.cpp

@ -1,114 +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 <https://www.gnu.org/licenses/>.
*
*/
#ifdef ARDUINO_ARCH_SAM
#include "../../inc/MarlinConfig.h"
#include "../../MarlinCore.h"
#include "watchdog.h"
// Override Arduino runtime to either config or disable the watchdog
//
// We need to configure the watchdog as soon as possible in the boot
// process, because watchdog initialization at hardware reset on SAM3X8E
// is unreliable, and there is risk of unintended resets if we delay
// that initialization to a later time.
void watchdogSetup() {
#if ENABLED(USE_WATCHDOG)
// 4 seconds timeout
uint32_t timeout = TERN(WATCHDOG_DURATION_8S, 8000, 4000);
// Calculate timeout value in WDT counter ticks: This assumes
// the slow clock is running at 32.768 kHz watchdog
// frequency is therefore 32768 / 128 = 256 Hz
timeout = (timeout << 8) / 1000;
if (timeout == 0)
timeout = 1;
else if (timeout > 0xFFF)
timeout = 0xFFF;
// We want to enable the watchdog with the specified timeout
uint32_t value =
WDT_MR_WDV(timeout) | // With the specified timeout
WDT_MR_WDD(timeout) | // and no invalid write window
#if !(SAMV70 || SAMV71 || SAME70 || SAMS70)
WDT_MR_WDRPROC | // WDT fault resets processor only - We want
// to keep PIO controller state
#endif
WDT_MR_WDDBGHLT | // WDT stops in debug state.
WDT_MR_WDIDLEHLT; // WDT stops in idle state.
#if ENABLED(WATCHDOG_RESET_MANUAL)
// We enable the watchdog timer, but only for the interrupt.
// Configure WDT to only trigger an interrupt
value |= WDT_MR_WDFIEN; // Enable WDT fault interrupt.
// Disable WDT interrupt (just in case, to avoid triggering it!)
NVIC_DisableIRQ(WDT_IRQn);
// We NEED memory barriers to ensure Interrupts are actually disabled!
// ( https://dzone.com/articles/nvic-disabling-interrupts-on-arm-cortex-m-and-the )
__DSB();
__ISB();
// Initialize WDT with the given parameters
WDT_Enable(WDT, value);
// Configure and enable WDT interrupt.
NVIC_ClearPendingIRQ(WDT_IRQn);
NVIC_SetPriority(WDT_IRQn, 0); // Use highest priority, so we detect all kinds of lockups
NVIC_EnableIRQ(WDT_IRQn);
#else
// a WDT fault triggers a reset
value |= WDT_MR_WDRSTEN;
// Initialize WDT with the given parameters
WDT_Enable(WDT, value);
#endif
// Reset the watchdog
WDT_Restart(WDT);
#else
// Make sure to completely disable the Watchdog
WDT_Disable(WDT);
#endif
}
#if ENABLED(USE_WATCHDOG)
// Initialize watchdog - On SAM3X, Watchdog was already configured
// and enabled or disabled at startup, so no need to reconfigure it
// here.
void watchdog_init() {
// Reset watchdog to start clean
WDT_Restart(WDT);
}
#endif // USE_WATCHDOG
#endif

33
Marlin/src/HAL/DUE/watchdog.h

@ -1,33 +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 <https://www.gnu.org/licenses/>.
*
*/
#pragma once
// Arduino Due core now has watchdog support
#include "HAL.h"
// Initialize watchdog with a 4 second interrupt time
void watchdog_init();
// Reset watchdog. MUST be called at least every 4 seconds after the
// first watchdog_init or AVR will go into emergency procedures.
inline void HAL_watchdog_refresh() { watchdogReset(); }

101
Marlin/src/HAL/ESP32/HAL.cpp

@ -65,6 +65,7 @@ portMUX_TYPE MarlinHAL::spinlock = portMUX_INITIALIZER_UNLOCKED;
// ------------------------
uint16_t MarlinHAL::adc_result;
pwm_pin_t MarlinHAL::pwm_pin_data[MAX_EXPANDER_BITS];
// ------------------------
// Private Variables
@ -179,6 +180,31 @@ void _delay_ms(int delay_ms) { delay(delay_ms); }
// return free memory between end of heap (or end bss) and whatever is current
int MarlinHAL::freeMemory() { return ESP.getFreeHeap(); }
// ------------------------
// Watchdog Timer
// ------------------------
#if ENABLED(USE_WATCHDOG)
#define WDT_TIMEOUT_US TERN(WATCHDOG_DURATION_8S, 8000000, 4000000) // 4 or 8 second timeout
extern "C" {
esp_err_t esp_task_wdt_reset();
}
void watchdogSetup() {
// do whatever. don't remove this function.
}
void MarlinHAL::watchdog_init() {
// TODO
}
// Reset watchdog.
void MarlinHAL::watchdog_refresh() { esp_task_wdt_reset(); }
#endif
// ------------------------
// ADC
// ------------------------
@ -209,19 +235,19 @@ void MarlinHAL::adc_init() {
adc1_config_width(ADC_WIDTH_12Bit);
// Configure channels only if used as (re-)configuring a pin for ADC that is used elsewhere might have adverse effects
TERN_(HAS_TEMP_ADC_0, adc1_set_attenuation(get_channel(TEMP_0_PIN), ADC_ATTEN_11db));
TERN_(HAS_TEMP_ADC_1, adc1_set_attenuation(get_channel(TEMP_1_PIN), ADC_ATTEN_11db));
TERN_(HAS_TEMP_ADC_2, adc1_set_attenuation(get_channel(TEMP_2_PIN), ADC_ATTEN_11db));
TERN_(HAS_TEMP_ADC_3, adc1_set_attenuation(get_channel(TEMP_3_PIN), ADC_ATTEN_11db));
TERN_(HAS_TEMP_ADC_4, adc1_set_attenuation(get_channel(TEMP_4_PIN), ADC_ATTEN_11db));
TERN_(HAS_TEMP_ADC_5, adc1_set_attenuation(get_channel(TEMP_5_PIN), ADC_ATTEN_11db));
TERN_(HAS_TEMP_ADC_6, adc2_set_attenuation(get_channel(TEMP_6_PIN), ADC_ATTEN_11db));
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_(HAS_TEMP_ADC_0, adc1_set_attenuation(get_channel(TEMP_0_PIN), ADC_ATTEN_11db));
TERN_(HAS_TEMP_ADC_1, adc1_set_attenuation(get_channel(TEMP_1_PIN), ADC_ATTEN_11db));
TERN_(HAS_TEMP_ADC_2, adc1_set_attenuation(get_channel(TEMP_2_PIN), ADC_ATTEN_11db));
TERN_(HAS_TEMP_ADC_3, adc1_set_attenuation(get_channel(TEMP_3_PIN), ADC_ATTEN_11db));
TERN_(HAS_TEMP_ADC_4, adc1_set_attenuation(get_channel(TEMP_4_PIN), ADC_ATTEN_11db));
TERN_(HAS_TEMP_ADC_5, adc1_set_attenuation(get_channel(TEMP_5_PIN), ADC_ATTEN_11db));
TERN_(HAS_TEMP_ADC_6, adc2_set_attenuation(get_channel(TEMP_6_PIN), ADC_ATTEN_11db));
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.
@ -305,21 +331,46 @@ int8_t get_pwm_channel(const pin_t pin, const uint32_t freq, const uint16_t res)
}
void MarlinHAL::set_pwm_duty(const pin_t pin, const uint16_t v, const uint16_t v_size/*=_BV(PWM_RESOLUTION)-1*/, const bool invert/*=false*/) {
const int8_t cid = get_pwm_channel(pin, PWM_FREQUENCY, PWM_RESOLUTION);
if (cid >= 0) {
uint32_t duty = map(invert ? v_size - v : v, 0, v_size, 0, _BV(PWM_RESOLUTION)-1);
ledcWrite(cid, duty);
}
#if ENABLED(I2S_STEPPER_STREAM)
if (pin > 127) {
const uint8_t pinlo = pin & 0x7F;
pwm_pin_t &pindata = pwm_pin_data[pinlo];
const uint32_t duty = map(invert ? v_size - v : v, 0, v_size, 0, pindata.pwm_cycle_ticks);
if (duty == 0 || duty == pindata.pwm_cycle_ticks) { // max or min (i.e., on/off)
pindata.pwm_duty_ticks = 0; // turn off PWM for this pin
duty ? SBI32(i2s_port_data, pinlo) : CBI32(i2s_port_data, pinlo); // set pin level
}
else
pindata.pwm_duty_ticks = duty; // PWM duty count = # of 4µs ticks per full PWM cycle
}
else
#endif
{
const int8_t cid = get_pwm_channel(pin, PWM_FREQUENCY, PWM_RESOLUTION);
if (cid >= 0) {
const uint32_t duty = map(invert ? v_size - v : v, 0, v_size, 0, _BV(PWM_RESOLUTION)-1);
ledcWrite(cid, duty);
}
}
}
int8_t MarlinHAL::set_pwm_frequency(const pin_t pin, const uint32_t f_desired) {
const int8_t cid = channel_for_pin(pin);
if (cid >= 0) {
if (f_desired == ledcReadFreq(cid)) return cid; // no freq change
ledcDetachPin(chan_pin[cid]);
chan_pin[cid] = 0; // remove old freq channel
}
return get_pwm_channel(pin, f_desired, PWM_RESOLUTION); // try for new one
#if ENABLED(I2S_STEPPER_STREAM)
if (pin > 127) {
pwm_pin_data[pin & 0x7F].pwm_cycle_ticks = 1000000UL / f_desired / 4; // # of 4µs ticks per full PWM cycle
return 0;
}
else
#endif
{
const int8_t cid = channel_for_pin(pin);
if (cid >= 0) {
if (f_desired == ledcReadFreq(cid)) return cid; // no freq change
ledcDetachPin(chan_pin[cid]);
chan_pin[cid] = 0; // remove old freq channel
}
return get_pwm_channel(pin, f_desired, PWM_RESOLUTION); // try for new one
}
}
// use hardware PWM if avail, if not then ISR

22
Marlin/src/HAL/ESP32/HAL.h

@ -32,7 +32,6 @@
#include "../shared/HAL_SPI.h"
#include "fastio.h"
#include "watchdog.h"
#include "i2s.h"
#if ENABLED(WIFISUPPORT)
@ -69,6 +68,9 @@
#define PWM_RESOLUTION 10u // Default PWM bit resolution
#define CHANNEL_MAX_NUM 15u // max PWM channel # to allocate (7 to only use low speed, 15 to use low & high)
#define MAX_PWM_IOPIN 33u // hardware pwm pins < 34
#ifndef MAX_EXPANDER_BITS
#define MAX_EXPANDER_BITS 32 // I2S expander bit width (max 32)
#endif
// ------------------------
// Types
@ -77,6 +79,12 @@
typedef double isr_float_t; // FPU ops are used for single-precision, so use double for ISRs.
typedef int16_t pin_t;
typedef struct pwm_pin {
uint32_t pwm_cycle_ticks = 1000000UL / (PWM_FREQUENCY) / 4; // # ticks per pwm cycle
uint32_t pwm_tick_count = 0; // current tick count
uint32_t pwm_duty_ticks = 0; // # of ticks for current duty cycle
} pwm_pin_t;
class Servo;
typedef Servo hal_servo_t;
@ -172,9 +180,13 @@ 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
// Watchdog
static void watchdog_init() IF_DISABLED(USE_WATCHDOG, {});
static void watchdog_refresh() IF_DISABLED(USE_WATCHDOG, {});
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;
@ -194,6 +206,8 @@ public:
// Free SRAM
static int freeMemory();
static pwm_pin_t pwm_pin_data[MAX_EXPANDER_BITS];
//
// ADC Methods
//

6
Marlin/src/HAL/ESP32/endstop_interrupts.h

@ -65,4 +65,10 @@ void setup_endstop_interrupts() {
TERN_(HAS_J_MIN, _ATTACH(J_MIN_PIN));
TERN_(HAS_K_MAX, _ATTACH(K_MAX_PIN));
TERN_(HAS_K_MIN, _ATTACH(K_MIN_PIN));
TERN_(HAS_U_MAX, _ATTACH(U_MAX_PIN));
TERN_(HAS_U_MIN, _ATTACH(U_MIN_PIN));
TERN_(HAS_V_MAX, _ATTACH(V_MAX_PIN));
TERN_(HAS_V_MIN, _ATTACH(V_MIN_PIN));
TERN_(HAS_W_MAX, _ATTACH(W_MAX_PIN));
TERN_(HAS_W_MIN, _ATTACH(W_MIN_PIN));
}

20
Marlin/src/HAL/ESP32/i2s.cpp

@ -337,6 +337,26 @@ uint8_t i2s_state(uint8_t pin) {
}
void i2s_push_sample() {
// Every 4µs (when space in DMA buffer) toggle each expander PWM output using
// the current duty cycle/frequency so they sync with any steps (once
// through the DMA/FIFO buffers). PWM signal inversion handled by other functions
LOOP_L_N(p, MAX_EXPANDER_BITS) {
if (hal.pwm_pin_data[p].pwm_duty_ticks > 0) { // pin has active pwm?
if (hal.pwm_pin_data[p].pwm_tick_count == 0) {
if (TEST32(i2s_port_data, p)) { // hi->lo
CBI32(i2s_port_data, p);
hal.pwm_pin_data[p].pwm_tick_count = hal.pwm_pin_data[p].pwm_cycle_ticks - hal.pwm_pin_data[p].pwm_duty_ticks;
}
else { // lo->hi
SBI32(i2s_port_data, p);
hal.pwm_pin_data[p].pwm_tick_count = hal.pwm_pin_data[p].pwm_duty_ticks;
}
}
else
hal.pwm_pin_data[p].pwm_tick_count--;
}
}
dma.current[dma.rw_pos++] = i2s_port_data;
}

7
Marlin/src/HAL/ESP32/inc/Conditionals_adv.h

@ -20,3 +20,10 @@
*
*/
#pragma once
//
// Board-specific options need to be defined before HAL.h
//
#if MB(MKS_TINYBEE)
#define MAX_EXPANDER_BITS 24 // TinyBee has 3 x HC595
#endif

4
Marlin/src/HAL/ESP32/inc/SanityCheck.h

@ -48,3 +48,7 @@
#if USING_PULLDOWNS
#error "PULLDOWN pin mode is not available on ESP32 boards."
#endif
#if BOTH(I2S_STEPPER_STREAM, LIN_ADVANCE)
#error "I2S stream is currently incompatible with LIN_ADVANCE."
#endif

21
Marlin/src/HAL/ESP32/u8g_esp32_spi.cpp

@ -23,24 +23,17 @@
*/
#ifdef ARDUINO_ARCH_ESP32
#include "../../inc/MarlinConfigPre.h"
#include "../../inc/MarlinConfig.h"
#if EITHER(MKS_MINI_12864, FYSETC_MINI_12864_2_1)
#include <U8glib-HAL.h>
#include "Arduino.h"
#include "../shared/HAL_SPI.h"
#include "HAL.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
@ -61,24 +54,24 @@ uint8_t u8g_eps_hw_spi_fn(u8g_t *u8g, uint8_t msg, uint8_t arg_val, void *arg_pt
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);
OUT_WRITE(DOGLCD_CS, HIGH);
OUT_WRITE(DOGLCD_A0, HIGH);
OUT_WRITE(LCD_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);
WRITE(DOGLCD_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);
WRITE(DOGLCD_CS, arg_val ? LOW : HIGH);
break;
case U8G_COM_MSG_RESET:
WRITE(MLCD_RESET_PIN, arg_val);
WRITE(LCD_RESET_PIN, arg_val);
break;
case U8G_COM_MSG_WRITE_BYTE:

42
Marlin/src/HAL/ESP32/watchdog.cpp

@ -1,42 +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 <https://www.gnu.org/licenses/>.
*
*/
#ifdef ARDUINO_ARCH_ESP32
#include "../../inc/MarlinConfig.h"
#if ENABLED(USE_WATCHDOG)
#define WDT_TIMEOUT_US TERN(WATCHDOG_DURATION_8S, 8000000, 4000000) // 4 or 8 second timeout
#include "watchdog.h"
void watchdogSetup() {
// do whatever. don't remove this function.
}
void watchdog_init() {
// TODO
}
#endif // USE_WATCHDOG
#endif // ARDUINO_ARCH_ESP32

38
Marlin/src/HAL/ESP32/watchdog.h

@ -1,38 +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 <https://www.gnu.org/licenses/>.
*
*/
#pragma once
#ifdef __cplusplus
extern "C" {
#endif
esp_err_t esp_task_wdt_reset();
#ifdef __cplusplus
}
#endif
// Initialize watchdog with a 4 second interrupt time
void watchdog_init();
// Reset watchdog.
inline void HAL_watchdog_refresh() { esp_task_wdt_reset(); }

5
Marlin/src/HAL/HAL.h

@ -28,6 +28,7 @@
#endif
#include HAL_PATH(.,HAL.h)
extern MarlinHAL hal;
#define HAL_ADC_RANGE _BV(HAL_ADC_RESOLUTION)
@ -44,7 +45,3 @@
#ifndef PGMSTR
#define PGMSTR(NAM,STR) const char NAM[] = STR
#endif
inline void watchdog_refresh() {
TERN_(USE_WATCHDOG, HAL_watchdog_refresh());
}

10
Marlin/src/HAL/LINUX/HAL.h

@ -21,6 +21,8 @@
*/
#pragma once
#include "../../inc/MarlinConfigPre.h"
#include <iostream>
#include <stdint.h>
#include <stdarg.h>
@ -29,12 +31,10 @@
#include <algorithm>
#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"
// ------------------------
@ -106,9 +106,13 @@ public:
// Earliest possible init, before setup()
MarlinHAL() {}
// Watchdog
static void watchdog_init() {}
static void watchdog_refresh() {}
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
static void reboot(); // Reset the application state and GPIO
// Interrupts
static bool isr_state() { return true; }

16
Marlin/src/HAL/LINUX/eeprom.cpp

@ -69,12 +69,12 @@ bool PersistentStore::write_data(int &pos, const uint8_t *value, size_t size, ui
std::size_t bytes_written = 0;
for (std::size_t i = 0; i < size; i++) {
buffer[pos+i] = value[i];
bytes_written ++;
buffer[pos + i] = value[i];
bytes_written++;
}
crc16(crc, value, size);
pos = pos + size;
pos += size;
return (bytes_written != size); // return true for any error
}
@ -82,21 +82,21 @@ bool PersistentStore::read_data(int &pos, uint8_t *value, const size_t size, uin
std::size_t bytes_read = 0;
if (writing) {
for (std::size_t i = 0; i < size; i++) {
value[i] = buffer[pos+i];
bytes_read ++;
value[i] = buffer[pos + i];
bytes_read++;
}
crc16(crc, value, size);
}
else {
uint8_t temp[size];
for (std::size_t i = 0; i < size; i++) {
temp[i] = buffer[pos+i];
bytes_read ++;
temp[i] = buffer[pos + i];
bytes_read++;
}
crc16(crc, temp, size);
}
pos = pos + size;
pos += size;
return bytes_read != size; // return true for any error
}

4
Marlin/src/HAL/LINUX/hardware/Heater.h

@ -26,8 +26,8 @@
struct LowpassFilter {
uint64_t data_delay = 0;
uint16_t update(uint16_t value) {
data_delay = data_delay - (data_delay >> 6) + value;
return (uint16_t)(data_delay >> 6);
data_delay += value - (data_delay >> 6);
return uint16_t(data_delay >> 6);
}
};

37
Marlin/src/HAL/LINUX/watchdog.cpp

@ -1,37 +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 <https://www.gnu.org/licenses/>.
*
*/
#ifdef __PLAT_LINUX__
#include "../../inc/MarlinConfig.h"
#if ENABLED(USE_WATCHDOG)
#include "watchdog.h"
#define WDT_TIMEOUT_US TERN(WATCHDOG_DURATION_8S, 8000000, 4000000) // 4 or 8 second timeout
void watchdog_init() {}
void HAL_watchdog_refresh() {}
#endif
#endif // __PLAT_LINUX__

54
Marlin/src/HAL/LPC1768/HAL.cpp

@ -25,10 +25,6 @@
#include "../shared/Delay.h"
#include "../../../gcode/parser.h"
#if ENABLED(USE_WATCHDOG)
#include "watchdog.h"
#endif
DefaultSerial1 USBSerial(false, UsbSerial);
uint32_t MarlinHAL::adc_result = 0;
@ -61,9 +57,7 @@ uint8_t MarlinHAL::get_reset_source() {
return RST_POWER_ON;
}
void MarlinHAL::clear_reset_source() {
TERN_(USE_WATCHDOG, watchdog_clear_timeout_flag());
}
void MarlinHAL::clear_reset_source() { watchdog_clear_timeout_flag(); }
void flashFirmware(const int16_t) {
delay(500); // Give OS time to disconnect
@ -72,6 +66,52 @@ void flashFirmware(const int16_t) {
hal.reboot();
}
#if ENABLED(USE_WATCHDOG)
#include <lpc17xx_wdt.h>
#define WDT_TIMEOUT_US TERN(WATCHDOG_DURATION_8S, 8000000, 4000000) // 4 or 8 second timeout
void MarlinHAL::watchdog_init() {
#if ENABLED(WATCHDOG_RESET_MANUAL)
// We enable the watchdog timer, but only for the interrupt.
// Configure WDT to only trigger an interrupt
// Disable WDT interrupt (just in case, to avoid triggering it!)
NVIC_DisableIRQ(WDT_IRQn);
// We NEED memory barriers to ensure Interrupts are actually disabled!
// ( https://dzone.com/articles/nvic-disabling-interrupts-on-arm-cortex-m-and-the )
__DSB();
__ISB();
// Configure WDT to only trigger an interrupt
// Initialize WDT with the given parameters
WDT_Init(WDT_CLKSRC_IRC, WDT_MODE_INT_ONLY);
// Configure and enable WDT interrupt.
NVIC_ClearPendingIRQ(WDT_IRQn);
NVIC_SetPriority(WDT_IRQn, 0); // Use highest priority, so we detect all kinds of lockups
NVIC_EnableIRQ(WDT_IRQn);
#else
WDT_Init(WDT_CLKSRC_IRC, WDT_MODE_RESET);
#endif
WDT_Start(WDT_TIMEOUT_US);
}
void MarlinHAL::watchdog_refresh() {
WDT_Feed();
#if DISABLED(PINS_DEBUGGING) && PIN_EXISTS(LED)
TOGGLE(LED_PIN); // heartbeat indicator
#endif
}
// Timeout state
bool MarlinHAL::watchdog_timed_out() { return TEST(WDT_ReadTimeOutFlag(), 0); }
void MarlinHAL::watchdog_clear_timeout_flag() { WDT_ClrTimeOutFlag(); }
#endif // USE_WATCHDOG
// 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.

13
Marlin/src/HAL/LPC1768/HAL.h

@ -38,7 +38,6 @@ extern "C" volatile uint32_t _millis;
#include "../shared/math_32bit.h"
#include "../shared/HAL_SPI.h"
#include "fastio.h"
#include "watchdog.h"
#include "MarlinSerial.h"
#include <adc.h>
@ -177,7 +176,7 @@ void flashFirmware(const int16_t);
#define CPU_ST7920_DELAY_3 750
// ------------------------
// Class Utilities
// Free Memory Accessor
// ------------------------
#pragma GCC diagnostic push
@ -199,9 +198,9 @@ public:
// Earliest possible init, before setup()
MarlinHAL() {}
static void init(); // Called early in setup()
static void init(); // Called early in setup()
static void init_board() {} // Called less early in setup()
static void reboot(); // Restart the firmware from 0x0
static void reboot(); // Restart the firmware from 0x0
// Interrupts
static bool isr_state() { return !__get_PRIMASK(); }
@ -210,6 +209,12 @@ public:
static void delay_ms(const int ms) { _delay_ms(ms); }
// Watchdog
static void watchdog_init() IF_DISABLED(USE_WATCHDOG, {});
static void watchdog_refresh() IF_DISABLED(USE_WATCHDOG, {});
static bool watchdog_timed_out() IF_DISABLED(USE_WATCHDOG, { return false; });
static void watchdog_clear_timeout_flag() IF_DISABLED(USE_WATCHDOG, {});
// Tasks, called from idle()
static void idletask();

33
Marlin/src/HAL/LPC1768/endstop_interrupts.h

@ -155,4 +155,37 @@ void setup_endstop_interrupts() {
#endif
_ATTACH(K_MIN_PIN);
#endif
#if HAS_U_MAX
#if !LPC1768_PIN_INTERRUPT_M(U_MAX_PIN)
#error "U_MAX_PIN is not INTERRUPT-capable."
#endif
_ATTACH(U_MAX_PIN);
#elif HAS_U_MIN
#if !LPC1768_PIN_INTERRUPT_M(U_MIN_PIN)
#error "U_MIN_PIN is not INTERRUPT-capable."
#endif
_ATTACH(U_MIN_PIN);
#endif
#if HAS_V_MAX
#if !LPC1768_PIN_INTERRUPT_M(V_MAX_PIN)
#error "V_MAX_PIN is not INTERRUPT-capable."
#endif
_ATTACH(V_MAX_PIN);
#elif HAS_V_MIN
#if !LPC1768_PIN_INTERRUPT_M(V_MIN_PIN)
#error "V_MIN_PIN is not INTERRUPT-capable."
#endif
_ATTACH(V_MIN_PIN);
#endif
#if HAS_W_MAX
#if !LPC1768_PIN_INTERRUPT_M(W_MAX_PIN)
#error "W_MAX_PIN is not INTERRUPT-capable."
#endif
_ATTACH(W_MAX_PIN);
#elif HAS_W_MIN
#if !LPC1768_PIN_INTERRUPT_M(W_MIN_PIN)
#error "W_MIN_PIN is not INTERRUPT-capable."
#endif
_ATTACH(W_MIN_PIN);
#endif
}

72
Marlin/src/HAL/LPC1768/watchdog.cpp

@ -1,72 +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 <https://www.gnu.org/licenses/>.
*
*/
#ifdef TARGET_LPC1768
#include "../../inc/MarlinConfig.h"
#if ENABLED(USE_WATCHDOG)
#include <lpc17xx_wdt.h>
#include "watchdog.h"
#define WDT_TIMEOUT_US TERN(WATCHDOG_DURATION_8S, 8000000, 4000000) // 4 or 8 second timeout
void watchdog_init() {
#if ENABLED(WATCHDOG_RESET_MANUAL)
// We enable the watchdog timer, but only for the interrupt.
// Configure WDT to only trigger an interrupt
// Disable WDT interrupt (just in case, to avoid triggering it!)
NVIC_DisableIRQ(WDT_IRQn);
// We NEED memory barriers to ensure Interrupts are actually disabled!
// ( https://dzone.com/articles/nvic-disabling-interrupts-on-arm-cortex-m-and-the )
__DSB();
__ISB();
// Configure WDT to only trigger an interrupt
// Initialize WDT with the given parameters
WDT_Init(WDT_CLKSRC_IRC, WDT_MODE_INT_ONLY);
// Configure and enable WDT interrupt.
NVIC_ClearPendingIRQ(WDT_IRQn);
NVIC_SetPriority(WDT_IRQn, 0); // Use highest priority, so we detect all kinds of lockups
NVIC_EnableIRQ(WDT_IRQn);
#else
WDT_Init(WDT_CLKSRC_IRC, WDT_MODE_RESET);
#endif
WDT_Start(WDT_TIMEOUT_US);
}
void HAL_watchdog_refresh() {
WDT_Feed();
#if DISABLED(PINS_DEBUGGING) && PIN_EXISTS(LED)
TOGGLE(LED_PIN); // heartbeat indicator
#endif
}
// Timeout state
bool watchdog_timed_out() { return TEST(WDT_ReadTimeOutFlag(), 0); }
void watchdog_clear_timeout_flag() { WDT_ClrTimeOutFlag(); }
#endif // USE_WATCHDOG
#endif // TARGET_LPC1768

28
Marlin/src/HAL/LPC1768/watchdog.h

@ -1,28 +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 <https://www.gnu.org/licenses/>.
*
*/
#pragma once
void watchdog_init();
void HAL_watchdog_refresh();
bool watchdog_timed_out();
void watchdog_clear_timeout_flag();

7
Marlin/src/HAL/NATIVE_SIM/HAL.h

@ -45,7 +45,6 @@ uint8_t _getc();
#include "../shared/math_32bit.h"
#include "../shared/HAL_SPI.h"
#include "fastio.h"
#include "watchdog.h"
#include "serial.h"
// ------------------------
@ -186,7 +185,7 @@ constexpr inline char* strstr_constexpr(char* str, const char* target) {
}
// ------------------------
// Class Utilities
// Free Memory Accessor
// ------------------------
#pragma GCC diagnostic push
@ -208,6 +207,10 @@ public:
// Earliest possible init, before setup()
MarlinHAL() {}
// Watchdog
static void watchdog_init() IF_DISABLED(USE_WATCHDOG, {});
static void watchdog_refresh() IF_DISABLED(USE_WATCHDOG, {});
static void init() {} // Called early in setup()
static void init_board() {} // Called less early in setup()
static void reboot(); // Restart the firmware from 0x0

27
Marlin/src/HAL/NATIVE_SIM/watchdog.h

@ -1,27 +0,0 @@
/**
* 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 <https://www.gnu.org/licenses/>.
*
*/
#pragma once
#define WDT_TIMEOUT 4000000 // 4 second timeout
void watchdog_init();
void HAL_watchdog_refresh();

36
Marlin/src/HAL/SAMD51/HAL.cpp

@ -203,6 +203,40 @@ enum ADCIndex {
ADC_COUNT
};
#if ENABLED(USE_WATCHDOG)
#define WDT_TIMEOUT_REG TERN(WATCHDOG_DURATION_8S, WDT_CONFIG_PER_CYC8192, WDT_CONFIG_PER_CYC4096) // 4 or 8 second timeout
void MarlinHAL::watchdog_init() {
// The low-power oscillator used by the WDT runs at 32,768 Hz with
// a 1:32 prescale, thus 1024 Hz, though probably not super precise.
// Setup WDT clocks
MCLK->APBAMASK.bit.OSC32KCTRL_ = true;
MCLK->APBAMASK.bit.WDT_ = true;
OSC32KCTRL->OSCULP32K.bit.EN1K = true; // Enable out 1K (this is what WDT uses)
WDT->CTRLA.bit.ENABLE = false; // Disable watchdog for config
SYNC(WDT->SYNCBUSY.bit.ENABLE);
WDT->INTENCLR.reg = WDT_INTENCLR_EW; // Disable early warning interrupt
WDT->CONFIG.reg = WDT_TIMEOUT_REG; // Set a 4s or 8s period for chip reset
hal.watchdog_refresh();
WDT->CTRLA.reg = WDT_CTRLA_ENABLE; // Start watchdog now in normal mode
SYNC(WDT->SYNCBUSY.bit.ENABLE);
}
// Reset watchdog. MUST be called at least every 4 seconds after the
// first watchdog_init or SAMD will go into emergency procedures.
void MarlinHAL::watchdog_refresh() {
SYNC(WDT->SYNCBUSY.bit.CLEAR); // Test first if previous is 'ongoing' to save time waiting for command execution
WDT->CLEAR.reg = WDT_CLEAR_CLEAR_KEY;
}
#endif
// ------------------------
// Types
// ------------------------
@ -564,7 +598,7 @@ void MarlinHAL::dma_init() {
void MarlinHAL::init() {
TERN_(DMA_IS_REQUIRED, dma_init());
#if ENABLED(SDSUPPORT)
#if SD_CONNECTION_IS(ONBOARD) && PIN_EXISTS(SD_DETECT)
#if HAS_SD_DETECT && SD_CONNECTION_IS(ONBOARD)
SET_INPUT_PULLUP(SD_DETECT_PIN);
#endif
OUT_WRITE(SDSS, HIGH); // Try to set SDSS inactive before any other SPI users start up

5
Marlin/src/HAL/SAMD51/HAL.h

@ -26,7 +26,6 @@
#include "../shared/math_32bit.h"
#include "../shared/HAL_SPI.h"
#include "fastio.h"
#include "watchdog.h"
#ifdef ADAFRUIT_GRAND_CENTRAL_M4
#include "MarlinSerial_AGCM4.h"
@ -157,6 +156,10 @@ public:
// Earliest possible init, before setup()
MarlinHAL() {}
// Watchdog
static void watchdog_init() IF_DISABLED(USE_WATCHDOG, {});
static void watchdog_refresh() IF_DISABLED(USE_WATCHDOG, {});
static void init(); // Called early in setup()
static void init_board() {} // Called less early in setup()
static void reboot(); // Restart the firmware from 0x0

45
Marlin/src/HAL/SAMD51/Servo.cpp

@ -77,7 +77,8 @@ HAL_SERVO_TIMER_ISR() {
;
const uint8_t tcChannel = TIMER_TCCHANNEL(timer);
if (currentServoIndex[timer] < 0) {
int8_t cho = currentServoIndex[timer]; // Handle the prior servo first
if (cho < 0) { // Servo -1 indicates the refresh interval completed...
#if defined(_useTimer1) && defined(_useTimer2)
if (currentServoIndex[timer ^ 1] >= 0) {
// Wait for both channels
@ -86,45 +87,37 @@ HAL_SERVO_TIMER_ISR() {
return;
}
#endif
tc->COUNT16.COUNT.reg = TC_COUNTER_START_VAL;
tc->COUNT16.COUNT.reg = TC_COUNTER_START_VAL; // ...so reset the timer
SYNC(tc->COUNT16.SYNCBUSY.bit.COUNT);
}
else if (SERVO_INDEX(timer, currentServoIndex[timer]) < ServoCount && SERVO(timer, currentServoIndex[timer]).Pin.isActive)
digitalWrite(SERVO(timer, currentServoIndex[timer]).Pin.nbr, LOW); // pulse this channel low if activated
// Select the next servo controlled by this timer
currentServoIndex[timer]++;
else if (SERVO_INDEX(timer, cho) < ServoCount) // prior channel handled?
digitalWrite(SERVO(timer, cho).Pin.nbr, LOW); // pulse the prior channel LOW
if (SERVO_INDEX(timer, currentServoIndex[timer]) < ServoCount && currentServoIndex[timer] < SERVOS_PER_TIMER) {
if (SERVO(timer, currentServoIndex[timer]).Pin.isActive) // check if activated
digitalWrite(SERVO(timer, currentServoIndex[timer]).Pin.nbr, HIGH); // it's an active channel so pulse it high
currentServoIndex[timer] = ++cho; // go to the next channel (or 0)
if (cho < SERVOS_PER_TIMER && SERVO_INDEX(timer, cho) < ServoCount) {
if (SERVO(timer, cho).Pin.isActive) // activated?
digitalWrite(SERVO(timer, cho).Pin.nbr, HIGH); // yes: pulse HIGH
tc->COUNT16.CC[tcChannel].reg = getTimerCount() - (uint16_t)SERVO(timer, currentServoIndex[timer]).ticks;
tc->COUNT16.CC[tcChannel].reg = getTimerCount() - (uint16_t)SERVO(timer, cho).ticks;
}
else {
// finished all channels so wait for the refresh period to expire before starting over
currentServoIndex[timer] = -1; // this will get incremented at the end of the refresh period to start again at the first channel
const uint16_t tcCounterValue = getTimerCount();
if ((TC_COUNTER_START_VAL - tcCounterValue) + 4UL < usToTicks(REFRESH_INTERVAL)) // allow a few ticks to ensure the next OCR1A not missed
tc->COUNT16.CC[tcChannel].reg = TC_COUNTER_START_VAL - (uint16_t)usToTicks(REFRESH_INTERVAL);
else
tc->COUNT16.CC[tcChannel].reg = (uint16_t)(tcCounterValue - 4UL); // at least REFRESH_INTERVAL has elapsed
currentServoIndex[timer] = -1; // reset the timer COUNT.reg on the next call
const uint16_t cval = getTimerCount() - 256 / (SERVO_TIMER_PRESCALER), // allow 256 cycles to ensure the next CV not missed
ival = (TC_COUNTER_START_VAL) - (uint16_t)usToTicks(REFRESH_INTERVAL); // at least REFRESH_INTERVAL has elapsed
tc->COUNT16.CC[tcChannel].reg = min(cval, ival);
}
if (tcChannel == 0) {
SYNC(tc->COUNT16.SYNCBUSY.bit.CC0);
// Clear the interrupt
tc->COUNT16.INTFLAG.reg = TC_INTFLAG_MC0;
tc->COUNT16.INTFLAG.reg = TC_INTFLAG_MC0; // Clear the interrupt
}
else {
SYNC(tc->COUNT16.SYNCBUSY.bit.CC1);
// Clear the interrupt
tc->COUNT16.INTFLAG.reg = TC_INTFLAG_MC1;
tc->COUNT16.INTFLAG.reg = TC_INTFLAG_MC1; // Clear the interrupt
}
}
void initISR(timer16_Sequence_t timer) {
void initISR(const timer16_Sequence_t timer) {
Tc * const tc = timer_config[SERVO_TC].pTc;
const uint8_t tcChannel = TIMER_TCCHANNEL(timer);
@ -201,9 +194,9 @@ void initISR(timer16_Sequence_t timer) {
}
}
void finISR(timer16_Sequence_t timer) {
void finISR(const timer16_Sequence_t timer_index) {
Tc * const tc = timer_config[SERVO_TC].pTc;
const uint8_t tcChannel = TIMER_TCCHANNEL(timer);
const uint8_t tcChannel = TIMER_TCCHANNEL(timer_index);
// Disable the match channel interrupt request
tc->COUNT16.INTENCLR.reg = (tcChannel == 0) ? TC_INTENCLR_MC0 : TC_INTENCLR_MC1;

45
Marlin/src/HAL/SAMD51/endstop_interrupts.h

@ -60,6 +60,12 @@
#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_U_MAX_EILINE(P) TERN0(HAS_U_MAX, DEFER4(MATCH_EILINE)(P, U_MAX_PIN))
#define MATCH_U_MIN_EILINE(P) TERN0(HAS_U_MIN, DEFER4(MATCH_EILINE)(P, U_MIN_PIN))
#define MATCH_V_MAX_EILINE(P) TERN0(HAS_V_MAX, DEFER4(MATCH_EILINE)(P, V_MAX_PIN))
#define MATCH_V_MIN_EILINE(P) TERN0(HAS_V_MIN, DEFER4(MATCH_EILINE)(P, V_MIN_PIN))
#define MATCH_W_MAX_EILINE(P) TERN0(HAS_W_MAX, DEFER4(MATCH_EILINE)(P, W_MAX_PIN))
#define MATCH_W_MIN_EILINE(P) TERN0(HAS_W_MIN, DEFER4(MATCH_EILINE)(P, W_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))
@ -75,6 +81,9 @@
&& !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_U_MAX_EILINE(P) && !MATCH_U_MIN_EILINE(P) \
&& !MATCH_V_MAX_EILINE(P) && !MATCH_V_MIN_EILINE(P) \
&& !MATCH_W_MAX_EILINE(P) && !MATCH_W_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) \
@ -199,4 +208,40 @@ void setup_endstop_interrupts() {
#endif
attachInterrupt(K_MIN_PIN, endstop_ISR, CHANGE);
#endif
#if HAS_U_MAX
#if !AVAILABLE_EILINE(U_MAX_PIN)
#error "U_MAX_PIN has no EXTINT line available."
#endif
attachInterrupt(U_MAX_PIN, endstop_ISR, CHANGE);
#endif
#if HAS_U_MIN
#if !AVAILABLE_EILINE(U_MIN_PIN)
#error "U_MIN_PIN has no EXTINT line available."
#endif
attachInterrupt(U_MIN_PIN, endstop_ISR, CHANGE);
#endif
#if HAS_V_MAX
#if !AVAILABLE_EILINE(V_MAX_PIN)
#error "V_MAX_PIN has no EXTINT line available."
#endif
attachInterrupt(V_MAX_PIN, endstop_ISR, CHANGE);
#endif
#if HAS_V_MIN
#if !AVAILABLE_EILINE(V_MIN_PIN)
#error "V_MIN_PIN has no EXTINT line available."
#endif
attachInterrupt(V_MIN_PIN, endstop_ISR, CHANGE);
#endif
#if HAS_W_MAX
#if !AVAILABLE_EILINE(W_MAX_PIN)
#error "W_MAX_PIN has no EXTINT line available."
#endif
attachInterrupt(W_MAX_PIN, endstop_ISR, CHANGE);
#endif
#if HAS_W_MIN
#if !AVAILABLE_EILINE(W_MIN_PIN)
#error "W_MIN_PIN has no EXTINT line available."
#endif
attachInterrupt(W_MIN_PIN, endstop_ISR, CHANGE);
#endif
}

54
Marlin/src/HAL/SAMD51/watchdog.cpp

@ -1,54 +0,0 @@
/**
* Marlin 3D Printer Firmware
*
* Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
* 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 <https://www.gnu.org/licenses/>.
*
*/
#ifdef __SAMD51__
#include "../../inc/MarlinConfig.h"
#if ENABLED(USE_WATCHDOG)
#include "watchdog.h"
#define WDT_TIMEOUT_REG TERN(WATCHDOG_DURATION_8S, WDT_CONFIG_PER_CYC8192, WDT_CONFIG_PER_CYC4096) // 4 or 8 second timeout
void watchdog_init() {
// The low-power oscillator used by the WDT runs at 32,768 Hz with
// a 1:32 prescale, thus 1024 Hz, though probably not super precise.
// Setup WDT clocks
MCLK->APBAMASK.bit.OSC32KCTRL_ = true;
MCLK->APBAMASK.bit.WDT_ = true;
OSC32KCTRL->OSCULP32K.bit.EN1K = true; // Enable out 1K (this is what WDT uses)
WDT->CTRLA.bit.ENABLE = false; // Disable watchdog for config
SYNC(WDT->SYNCBUSY.bit.ENABLE);
WDT->INTENCLR.reg = WDT_INTENCLR_EW; // Disable early warning interrupt
WDT->CONFIG.reg = WDT_TIMEOUT_REG; // Set a 4s or 8s period for chip reset
HAL_watchdog_refresh();
WDT->CTRLA.reg = WDT_CTRLA_ENABLE; // Start watchdog now in normal mode
SYNC(WDT->SYNCBUSY.bit.ENABLE);
}
#endif // USE_WATCHDOG
#endif // __SAMD51__

31
Marlin/src/HAL/SAMD51/watchdog.h

@ -1,31 +0,0 @@
/**
* Marlin 3D Printer Firmware
*
* Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
* 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 <https://www.gnu.org/licenses/>.
*
*/
#pragma once
// Initialize watchdog with a 4 second interrupt time
void watchdog_init();
// Reset watchdog. MUST be called at least every 4 seconds after the
// first watchdog_init or SAMD will go into emergency procedures.
inline void HAL_watchdog_refresh() {
SYNC(WDT->SYNCBUSY.bit.CLEAR); // Test first if previous is 'ongoing' to save time waiting for command execution
WDT->CLEAR.reg = WDT_CLEAR_CLEAR_KEY;
}

28
Marlin/src/HAL/STM32/HAL.cpp

@ -24,12 +24,11 @@
#ifdef HAL_STM32
#include "HAL.h"
#include "usb_serial.h"
#include "../../inc/MarlinConfig.h"
#include "../shared/Delay.h"
#include "usb_serial.h"
#ifdef USBCON
DefaultSerial1 MSerialUSB(false, SerialUSB);
#endif
@ -141,6 +140,29 @@ uint8_t MarlinHAL::get_reset_source() {
void MarlinHAL::clear_reset_source() { __HAL_RCC_CLEAR_RESET_FLAGS(); }
// ------------------------
// Watchdog Timer
// ------------------------
#if ENABLED(USE_WATCHDOG)
#define WDT_TIMEOUT_US TERN(WATCHDOG_DURATION_8S, 8000000, 4000000) // 4 or 8 second timeout
#include <IWatchdog.h>
void MarlinHAL::watchdog_init() {
IF_DISABLED(DISABLE_WATCHDOG_INIT, IWatchdog.begin(WDT_TIMEOUT_US));
}
void MarlinHAL::watchdog_refresh() {
IWatchdog.reload();
#if DISABLED(PINS_DEBUGGING) && PIN_EXISTS(LED)
TOGGLE(LED_PIN); // heartbeat indicator
#endif
}
#endif
extern "C" {
extern unsigned int _ebss; // end of bss section
}

9
Marlin/src/HAL/STM32/HAL.h

@ -30,7 +30,6 @@
#include "../shared/HAL_SPI.h"
#include "fastio.h"
#include "Servo.h"
#include "watchdog.h"
#include "MarlinSerial.h"
#include "../../inc/MarlinConfigPre.h"
@ -218,9 +217,13 @@ public:
// Earliest possible init, before setup()
MarlinHAL() {}
static void init(); // Called early in setup()
// Watchdog
static void watchdog_init() IF_DISABLED(USE_WATCHDOG, {});
static void watchdog_refresh() IF_DISABLED(USE_WATCHDOG, {});
static void init(); // Called early in setup()
static void init_board() {} // Called less early in setup()
static void reboot(); // Restart the firmware from 0x0
static void reboot(); // Restart the firmware from 0x0
// Interrupts
static bool isr_state() { return !__get_PRIMASK(); }

3
Marlin/src/HAL/STM32/MinSerial.cpp

@ -29,7 +29,6 @@
#if ENABLED(POSTMORTEM_DEBUGGING)
#include "../shared/MinSerial.h"
#include "watchdog.h"
/* Instruction Synchronization Barrier */
#define isb() __asm__ __volatile__ ("isb" : : : "memory")
@ -120,7 +119,7 @@ static void TX(char c) {
#if WITHIN(SERIAL_PORT, 1, 6)
constexpr uint32_t usart_sr_txe = _BV(7);
while (!(regs->SR & usart_sr_txe)) {
TERN_(USE_WATCHDOG, HAL_watchdog_refresh());
hal.watchdog_refresh();
sw_barrier();
}
regs->DR = c;

6
Marlin/src/HAL/STM32/endstop_interrupts.h

@ -52,4 +52,10 @@ void setup_endstop_interrupts() {
TERN_(HAS_J_MIN, _ATTACH(J_MIN_PIN));
TERN_(HAS_K_MAX, _ATTACH(K_MAX_PIN));
TERN_(HAS_K_MIN, _ATTACH(K_MIN_PIN));
TERN_(HAS_U_MAX, _ATTACH(U_MAX_PIN));
TERN_(HAS_U_MIN, _ATTACH(U_MIN_PIN));
TERN_(HAS_V_MAX, _ATTACH(V_MAX_PIN));
TERN_(HAS_V_MIN, _ATTACH(V_MIN_PIN));
TERN_(HAS_W_MAX, _ATTACH(W_MAX_PIN));
TERN_(HAS_W_MIN, _ATTACH(W_MIN_PIN));
}

8
Marlin/src/HAL/STM32/msc_sd.cpp

@ -57,7 +57,7 @@ public:
auto sd2card = diskIODriver();
// single block
if (blkLen == 1) {
watchdog_refresh();
hal.watchdog_refresh();
sd2card->writeBlock(blkAddr, pBuf);
return true;
}
@ -65,7 +65,7 @@ public:
// multi block optimization
sd2card->writeStart(blkAddr, blkLen);
while (blkLen--) {
watchdog_refresh();
hal.watchdog_refresh();
sd2card->writeData(pBuf);
pBuf += BLOCK_SIZE;
}
@ -77,7 +77,7 @@ public:
auto sd2card = diskIODriver();
// single block
if (blkLen == 1) {
watchdog_refresh();
hal.watchdog_refresh();
sd2card->readBlock(blkAddr, pBuf);
return true;
}
@ -85,7 +85,7 @@ public:
// multi block optimization
sd2card->readStart(blkAddr);
while (blkLen--) {
watchdog_refresh();
hal.watchdog_refresh();
sd2card->readData(pBuf);
pBuf += BLOCK_SIZE;
}

378
Marlin/src/HAL/STM32/sdio.cpp

@ -35,42 +35,15 @@
// use local drivers
#if defined(STM32F103xE) || defined(STM32F103xG)
#include <stm32f1xx_hal_rcc_ex.h>
#include <stm32f1xx_hal_sd.h>
#include <stm32f1xx.h>
#elif defined(STM32F4xx)
#include <stm32f4xx_hal_rcc.h>
#include <stm32f4xx_hal_dma.h>
#include <stm32f4xx_hal_gpio.h>
#include <stm32f4xx_hal_sd.h>
#include <stm32f4xx.h>
#elif defined(STM32F7xx)
#include <stm32f7xx_hal_rcc.h>
#include <stm32f7xx_hal_dma.h>
#include <stm32f7xx_hal_gpio.h>
#include <stm32f7xx_hal_sd.h>
#include <stm32f7xx.h>
#elif defined(STM32H7xx)
#include <stm32h7xx.h>
#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
#error "SDIO only supported with STM32F103xE, STM32F103xG, STM32F4xx, STM32F7xx, or STM32H7xx."
#endif
// Target Clock, configurable. Default is 18MHz, from STM32F1
@ -78,223 +51,209 @@ DMA_HandleTypeDef hdma_sdio;
#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
#define SD_TIMEOUT 1000 // ms
// 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;
#if defined(STM32F1xx)
DMA_HandleTypeDef hdma_sdio;
extern "C" void DMA2_Channel4_5_IRQHandler(void) {
HAL_DMA_IRQHandler(&hdma_sdio);
}
#elif defined(STM32F4xx)
DMA_HandleTypeDef hdma_sdio_rx;
DMA_HandleTypeDef hdma_sdio_tx;
extern "C" void DMA2_Stream3_IRQHandler(void) {
HAL_DMA_IRQHandler(&hdma_sdio_rx);
}
__HAL_RCC_GPIOC_CLK_ENABLE(); //enable GPIO clocks
__HAL_RCC_GPIOD_CLK_ENABLE(); //enable GPIO clocks
extern "C" void DMA2_Stream6_IRQHandler(void) {
HAL_DMA_IRQHandler(&hdma_sdio_tx);
}
#elif defined(STM32H7xx)
#define __HAL_RCC_SDIO_FORCE_RESET __HAL_RCC_SDMMC1_FORCE_RESET
#define __HAL_RCC_SDIO_RELEASE_RESET __HAL_RCC_SDMMC1_RELEASE_RESET
#define __HAL_RCC_SDIO_CLK_ENABLE __HAL_RCC_SDMMC1_CLK_ENABLE
#define SDIO SDMMC1
#define SDIO_IRQn SDMMC1_IRQn
#define SDIO_IRQHandler SDMMC1_IRQHandler
#define SDIO_CLOCK_EDGE_RISING SDMMC_CLOCK_EDGE_RISING
#define SDIO_CLOCK_POWER_SAVE_DISABLE SDMMC_CLOCK_POWER_SAVE_DISABLE
#define SDIO_BUS_WIDE_1B SDMMC_BUS_WIDE_1B
#define SDIO_BUS_WIDE_4B SDMMC_BUS_WIDE_4B
#define SDIO_HARDWARE_FLOW_CONTROL_DISABLE SDMMC_HARDWARE_FLOW_CONTROL_DISABLE
#endif
GPIO_InitTypeDef GPIO_InitStruct;
uint8_t waitingRxCplt = 0;
uint8_t waitingTxCplt = 0;
SD_HandleTypeDef hsd;
GPIO_InitStruct.Mode = GPIO_MODE_AF_PP;
GPIO_InitStruct.Pull = 1; //GPIO_NOPULL;
GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_HIGH;
extern "C" void SDIO_IRQHandler(void) {
HAL_SD_IRQHandler(&hsd);
}
#if DISABLED(STM32F1xx)
GPIO_InitStruct.Alternate = GPIO_AF12_SDIO;
#endif
void HAL_SD_TxCpltCallback(SD_HandleTypeDef *hsdio) {
waitingTxCplt = 0;
}
GPIO_InitStruct.Pin = GPIO_PIN_8 | GPIO_PIN_12; // D0 & SCK
HAL_GPIO_Init(GPIOC, &GPIO_InitStruct);
void HAL_SD_RxCpltCallback(SD_HandleTypeDef *hsdio) {
waitingRxCplt = 0;
}
void HAL_SD_MspInit(SD_HandleTypeDef *hsd) {
pinmap_pinout(PC_12, PinMap_SD);
pinmap_pinout(PD_2, PinMap_SD);
pinmap_pinout(PC_8, PinMap_SD);
#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);
// D1-D3
pinmap_pinout(PC_9, PinMap_SD);
pinmap_pinout(PC_10, PinMap_SD);
pinmap_pinout(PC_11, PinMap_SD);
#endif
// Configure PD.02 CMD line
GPIO_InitStruct.Pin = GPIO_PIN_2;
HAL_GPIO_Init(GPIOD, &GPIO_InitStruct);
__HAL_RCC_SDIO_CLK_ENABLE();
HAL_NVIC_EnableIRQ(SDIO_IRQn);
// Setup DMA
// DMA Config
#if defined(STM32F1xx)
hdma_sdio.Init.Mode = DMA_NORMAL;
hdma_sdio.Instance = DMA2_Channel4;
__HAL_RCC_DMA2_CLK_ENABLE();
HAL_NVIC_EnableIRQ(DMA2_Channel4_5_IRQn);
hdma_sdio.Instance = DMA2_Channel4;
hdma_sdio.Init.Direction = DMA_PERIPH_TO_MEMORY;
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.Mode = DMA_NORMAL;
hdma_sdio.Init.Priority = DMA_PRIORITY_LOW;
HAL_DMA_Init(&hdma_sdio);
__HAL_LINKDMA(hsd, hdmarx ,hdma_sdio);
__HAL_LINKDMA(hsd, hdmatx, hdma_sdio);
#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_RCC_DMA2_CLK_ENABLE();
HAL_NVIC_EnableIRQ(DMA2_Stream3_IRQn);
HAL_NVIC_EnableIRQ(DMA2_Stream6_IRQn);
hdma_sdio_rx.Instance = DMA2_Stream3;
hdma_sdio_rx.Init.Channel = DMA_CHANNEL_4;
hdma_sdio_rx.Init.Direction = DMA_PERIPH_TO_MEMORY;
hdma_sdio_rx.Init.PeriphInc = DMA_PINC_DISABLE;
hdma_sdio_rx.Init.MemInc = DMA_MINC_ENABLE;
hdma_sdio_rx.Init.PeriphDataAlignment = DMA_PDATAALIGN_WORD;
hdma_sdio_rx.Init.MemDataAlignment = DMA_MDATAALIGN_WORD;
hdma_sdio_rx.Init.Mode = DMA_PFCTRL;
hdma_sdio_rx.Init.Priority = DMA_PRIORITY_LOW;
hdma_sdio_rx.Init.FIFOMode = DMA_FIFOMODE_ENABLE;
hdma_sdio_rx.Init.FIFOThreshold = DMA_FIFO_THRESHOLD_FULL;
hdma_sdio_rx.Init.MemBurst = DMA_MBURST_INC4;
hdma_sdio_rx.Init.PeriphBurst = DMA_PBURST_INC4;
HAL_DMA_Init(&hdma_sdio_rx);
__HAL_LINKDMA(hsd,hdmarx,hdma_sdio_rx);
hdma_sdio_tx.Instance = DMA2_Stream6;
hdma_sdio_tx.Init.Channel = DMA_CHANNEL_4;
hdma_sdio_tx.Init.Direction = DMA_MEMORY_TO_PERIPH;
hdma_sdio_tx.Init.PeriphInc = DMA_PINC_DISABLE;
hdma_sdio_tx.Init.MemInc = DMA_MINC_ENABLE;
hdma_sdio_tx.Init.PeriphDataAlignment = DMA_PDATAALIGN_WORD;
hdma_sdio_tx.Init.MemDataAlignment = DMA_MDATAALIGN_WORD;
hdma_sdio_tx.Init.Mode = DMA_PFCTRL;
hdma_sdio_tx.Init.Priority = DMA_PRIORITY_LOW;
hdma_sdio_tx.Init.FIFOMode = DMA_FIFOMODE_ENABLE;
hdma_sdio_tx.Init.FIFOThreshold = DMA_FIFO_THRESHOLD_FULL;
hdma_sdio_tx.Init.MemBurst = DMA_MBURST_INC4;
hdma_sdio_tx.Init.PeriphBurst = DMA_PBURST_INC4;
HAL_DMA_Init(&hdma_sdio_tx);
__HAL_LINKDMA(hsd,hdmatx,hdma_sdio_tx);
#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
void HAL_SD_MspDeInit(SD_HandleTypeDef *hsd) {
#if !defined(STM32F1xx)
__HAL_RCC_SDIO_FORCE_RESET();
delay(2);
delay(10);
__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();
delay(10);
#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
static uint32_t clock_to_divider(uint32_t clk) {
#if defined(STM32H7xx)
// SDMMC_CK frequency = sdmmc_ker_ck / [2 * CLKDIV].
uint32_t sdmmc_clk = HAL_RCCEx_GetPeriphCLKFreq(RCC_PERIPHCLK_SDMMC);
return sdmmc_clk / (2U * SDIO_CLOCK) + (sdmmc_clk % (2U * SDIO_CLOCK) != 0);
#else
// 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;
#endif
}
bool SDIO_Init() {
uint8_t retryCnt = SDIO_READ_RETRIES;
HAL_StatusTypeDef sd_state = HAL_OK;
if (hsd.Instance == SDIO)
HAL_SD_DeInit(&hsd);
bool status;
/* HAL SD initialization */
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();
hsd.Init.ClockEdge = SDIO_CLOCK_EDGE_RISING;
hsd.Init.ClockPowerSave = SDIO_CLOCK_POWER_SAVE_DISABLE;
hsd.Init.BusWide = SDIO_BUS_WIDE_1B;
hsd.Init.HardwareFlowControl = SDIO_HARDWARE_FLOW_CONTROL_DISABLE;
hsd.Init.ClockDiv = clock_to_divider(SDIO_CLOCK);
sd_state = HAL_SD_Init(&hsd);
#if PINS_EXIST(SDIO_D1, SDIO_D2, SDIO_D3)
if (sd_state == HAL_OK) {
sd_state = HAL_SD_ConfigWideBusOperation(&hsd, SDIO_BUS_WIDE_4B);
}
#endif
return true;
return (sd_state == HAL_OK) ? true : false;
}
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());
bool SDIO_ReadBlock(uint32_t block, uint8_t *dst) {
uint32_t timeout = HAL_GetTick() + SD_TIMEOUT;
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);
while (HAL_SD_GetCardState(&hsd) != HAL_SD_CARD_TRANSFER) {
if (HAL_GetTick() >= timeout) return false;
}
if (ret != HAL_OK) {
HAL_DMA_Abort_IT(&hdma_sdio);
HAL_DMA_DeInit(&hdma_sdio);
waitingRxCplt = 1;
if (HAL_SD_ReadBlocks_DMA(&hsd, (uint8_t *)dst, block, 1) != HAL_OK)
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;
}
}
timeout = HAL_GetTick() + SD_TIMEOUT;
while (waitingRxCplt)
if (HAL_GetTick() >= timeout) 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 */ }
return true;
}
HAL_DMA_Abort_IT(&hdma_sdio);
HAL_DMA_DeInit(&hdma_sdio);
bool SDIO_WriteBlock(uint32_t block, const uint8_t *src) {
uint32_t timeout = HAL_GetTick() + SD_TIMEOUT;
timeout = millis() + 500;
while (HAL_SD_GetCardState(&hsd) != HAL_SD_CARD_TRANSFER) if (ELAPSED(millis(), timeout)) return false;
while (HAL_SD_GetCardState(&hsd) != HAL_SD_CARD_TRANSFER)
if (HAL_GetTick() >= timeout) return false;
return true;
}
waitingTxCplt = 1;
if (HAL_SD_WriteBlocks_DMA(&hsd, (uint8_t *)src, block, 1) != HAL_OK)
return false;
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;
}
timeout = HAL_GetTick() + SD_TIMEOUT;
while (waitingTxCplt)
if (HAL_GetTick() >= timeout) 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;
return true;
}
bool SDIO_IsReady() {
@ -305,16 +264,5 @@ 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

16
Marlin/src/HAL/STM32/tft/gt911.cpp

@ -159,24 +159,28 @@ void GT911::read_reg(uint16_t reg, uint8_t reg_len, uint8_t* r_data, uint8_t r_l
void GT911::Init() {
OUT_WRITE(GT911_RST_PIN, LOW);
OUT_WRITE(GT911_INT_PIN, LOW);
delay(20);
delay(11);
WRITE(GT911_INT_PIN, HIGH);
delayMicroseconds(110);
WRITE(GT911_RST_PIN, HIGH);
delay(6);
WRITE(GT911_INT_PIN, LOW);
delay(55);
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
uint8_t clear_reg = 0x00;
write_reg(0x814E, 2, &clear_reg, 1); // Reset to 0 for start
}
bool GT911::getFirstTouchPoint(int16_t *x, int16_t *y) {
read_reg(0x814E, 2, &reg.REG.status, 1);
if (reg.REG.status & 0x80) {
if (reg.REG.status >= 0x80 && reg.REG.status <= 0x85) {
read_reg(0x8150, 2, reg.map + 2, 38);
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;

2
Marlin/src/HAL/STM32/tft/gt911.h

@ -23,7 +23,7 @@
#include "../../../inc/MarlinConfig.h"
#define GT911_SLAVE_ADDRESS 0xBA
#define GT911_SLAVE_ADDRESS 0x28
#if !PIN_EXISTS(GT911_RST)
#error "GT911_RST_PIN is not defined."

52
Marlin/src/HAL/STM32/watchdog.cpp

@ -1,52 +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 <https://www.gnu.org/licenses/>.
*
*/
#include "../platforms.h"
#ifdef HAL_STM32
#include "../../inc/MarlinConfigPre.h"
#if ENABLED(USE_WATCHDOG)
#define WDT_TIMEOUT_US TERN(WATCHDOG_DURATION_8S, 8000000, 4000000) // 4 or 8 second timeout
#include "../../inc/MarlinConfig.h"
#include "watchdog.h"
#include <IWatchdog.h>
void watchdog_init() {
#if DISABLED(DISABLE_WATCHDOG_INIT)
IWatchdog.begin(WDT_TIMEOUT_US);
#endif
}
void HAL_watchdog_refresh() {
IWatchdog.reload();
#if DISABLED(PINS_DEBUGGING) && PIN_EXISTS(LED)
TOGGLE(LED_PIN); // heartbeat indicator
#endif
}
#endif // USE_WATCHDOG
#endif // HAL_STM32

50
Marlin/src/HAL/STM32F1/HAL.cpp

@ -83,6 +83,7 @@
// ------------------------
#if defined(SERIAL_USB) && !HAS_SD_HOST_DRIVE
USBSerial SerialUSB;
DefaultSerial1 MSerial0(true, SerialUSB);
@ -112,6 +113,47 @@
#endif
#endif
// ------------------------
// Watchdog Timer
// ------------------------
#if ENABLED(USE_WATCHDOG)
#include <libmaple/iwdg.h>
void watchdogSetup() {
// do whatever. don't remove this function.
}
/**
* The watchdog clock is 40Khz. So for a 4s or 8s interval use a /256 preescaler and 625 or 1250 reload value (counts down to 0).
*/
#define STM32F1_WD_RELOAD TERN(WATCHDOG_DURATION_8S, 1250, 625) // 4 or 8 second timeout
/**
* @brief Initialize the independent hardware watchdog.
*
* @return No return
*
* @details The watchdog clock is 40Khz. So for a 4s or 8s interval use a /256 preescaler and 625 or 1250 reload value (counts down to 0).
*/
void MarlinHAL::watchdog_init() {
#if DISABLED(DISABLE_WATCHDOG_INIT)
iwdg_init(IWDG_PRE_256, STM32F1_WD_RELOAD);
#endif
}
// Reset watchdog. MUST be called every 4 or 8 seconds after the
// first watchdog_init or the STM32F1 will reset.
void MarlinHAL::watchdog_refresh() {
#if DISABLED(PINS_DEBUGGING) && PIN_EXISTS(LED)
TOGGLE(LED_PIN); // heartbeat indicator
#endif
iwdg_feed();
}
#endif // USE_WATCHDOG
// ------------------------
// ADC
// ------------------------
@ -211,6 +253,10 @@ void MarlinHAL::idletask() {
void MarlinHAL::reboot() { nvic_sys_reset(); }
// ------------------------
// Free Memory Accessor
// ------------------------
extern "C" {
extern unsigned int _ebss; // end of bss section
}
@ -243,9 +289,9 @@ extern "C" {
}
*/
//
// ------------------------
// ADC
//
// ------------------------
enum ADCIndex : uint8_t {
OPTITEM(HAS_TEMP_ADC_0, TEMP_0)

5
Marlin/src/HAL/STM32F1/HAL.h

@ -34,7 +34,6 @@
#include "../shared/HAL_SPI.h"
#include "fastio.h"
#include "watchdog.h"
#include <stdint.h>
#include <util/atomic.h>
@ -247,6 +246,10 @@ public:
// Earliest possible init, before setup()
MarlinHAL() {}
// Watchdog
static void watchdog_init() IF_DISABLED(USE_WATCHDOG, {});
static void watchdog_refresh() IF_DISABLED(USE_WATCHDOG, {});
static void init(); // Called early in setup()
static void init_board() {} // Called less early in setup()
static void reboot(); // Restart the firmware from 0x0

3
Marlin/src/HAL/STM32F1/MinSerial.cpp

@ -27,7 +27,6 @@
#if ENABLED(POSTMORTEM_DEBUGGING)
#include "../shared/MinSerial.h"
#include "watchdog.h"
#include <libmaple/usart.h>
#include <libmaple/rcc.h>
@ -82,7 +81,7 @@ static void TX(char c) {
#if WITHIN(SERIAL_PORT, 1, 6)
struct usart_dev* dev = MYSERIAL1.c_dev();
while (!(dev->regs->SR & USART_SR_TXE)) {
TERN_(USE_WATCHDOG, HAL_watchdog_refresh());
hal.watchdog_refresh();
sw_barrier();
}
dev->regs->DR = c;

16
Marlin/src/HAL/STM32F1/Servo.cpp

@ -147,17 +147,17 @@ void libServo::move(const int32_t value) {
uint16_t SR = timer_get_status(tdev);
if (SR & TIMER_SR_CC1IF) { // channel 1 off
#ifdef SERVO0_PWM_OD
OUT_WRITE_OD(SERVO0_PIN, 1); // off
OUT_WRITE_OD(SERVO0_PIN, HIGH); // off
#else
OUT_WRITE(SERVO0_PIN, 0);
OUT_WRITE(SERVO0_PIN, LOW);
#endif
timer_reset_status_bit(tdev, TIMER_SR_CC1IF_BIT);
}
if (SR & TIMER_SR_CC2IF) { // channel 2 resume
#ifdef SERVO0_PWM_OD
OUT_WRITE_OD(SERVO0_PIN, 0); // on
OUT_WRITE_OD(SERVO0_PIN, LOW); // on
#else
OUT_WRITE(SERVO0_PIN, 1);
OUT_WRITE(SERVO0_PIN, HIGH);
#endif
timer_reset_status_bit(tdev, TIMER_SR_CC2IF_BIT);
}
@ -167,9 +167,9 @@ void libServo::move(const int32_t value) {
timer_dev *tdev = HAL_get_timer_dev(MF_TIMER_SERVO0);
if (!tdev) return false;
#ifdef SERVO0_PWM_OD
OUT_WRITE_OD(inPin, 1);
OUT_WRITE_OD(inPin, HIGH);
#else
OUT_WRITE(inPin, 0);
OUT_WRITE(inPin, LOW);
#endif
timer_pause(tdev);
@ -200,9 +200,9 @@ void libServo::move(const int32_t value) {
timer_disable_irq(tdev, 1);
timer_disable_irq(tdev, 2);
#ifdef SERVO0_PWM_OD
OUT_WRITE_OD(pin, 1); // off
OUT_WRITE_OD(pin, HIGH); // off
#else
OUT_WRITE(pin, 0);
OUT_WRITE(pin, LOW);
#endif
}
}

6
Marlin/src/HAL/STM32F1/endstop_interrupts.h

@ -77,4 +77,10 @@ void setup_endstop_interrupts() {
TERN_(HAS_J_MIN, _ATTACH(J_MIN_PIN));
TERN_(HAS_K_MAX, _ATTACH(K_MAX_PIN));
TERN_(HAS_K_MIN, _ATTACH(K_MIN_PIN));
TERN_(HAS_U_MAX, _ATTACH(U_MAX_PIN));
TERN_(HAS_U_MIN, _ATTACH(U_MIN_PIN));
TERN_(HAS_V_MAX, _ATTACH(V_MAX_PIN));
TERN_(HAS_V_MIN, _ATTACH(V_MIN_PIN));
TERN_(HAS_W_MAX, _ATTACH(W_MAX_PIN));
TERN_(HAS_W_MIN, _ATTACH(W_MIN_PIN));
}

2
Marlin/src/HAL/STM32F1/tft/tft_spi.cpp

@ -30,7 +30,7 @@ SPIClass TFT_SPI::SPIx(1);
void TFT_SPI::Init() {
#if PIN_EXISTS(TFT_RESET)
OUT_WRITE(TFT_RST_PIN, HIGH);
OUT_WRITE(TFT_RESET_PIN, HIGH);
delay(100);
#endif

66
Marlin/src/HAL/STM32F1/watchdog.cpp

@ -1,66 +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 <https://www.gnu.org/licenses/>.
*
*/
/**
* HAL for stm32duino.com based on Libmaple and compatible (STM32F1)
*/
#ifdef __STM32F1__
#include "../../inc/MarlinConfig.h"
#if ENABLED(USE_WATCHDOG)
#include <libmaple/iwdg.h>
#include "watchdog.h"
/**
* The watchdog clock is 40Khz. So for a 4s or 8s interval use a /256 preescaler and 625 or 1250 reload value (counts down to 0).
*/
#define STM32F1_WD_RELOAD TERN(WATCHDOG_DURATION_8S, 1250, 625) // 4 or 8 second timeout
void HAL_watchdog_refresh() {
#if DISABLED(PINS_DEBUGGING) && PIN_EXISTS(LED)
TOGGLE(LED_PIN); // heartbeat indicator
#endif
iwdg_feed();
}
void watchdogSetup() {
// do whatever. don't remove this function.
}
/**
* @brief Initialized the independent hardware watchdog.
*
* @return No return
*
* @details The watchdog clock is 40Khz. So for a 4s or 8s interval use a /256 preescaler and 625 or 1250 reload value (counts down to 0).
*/
void watchdog_init() {
#if DISABLED(DISABLE_WATCHDOG_INIT)
iwdg_init(IWDG_PRE_256, STM32F1_WD_RELOAD);
#endif
}
#endif // USE_WATCHDOG
#endif // __STM32F1__

35
Marlin/src/HAL/STM32F1/watchdog.h

@ -1,35 +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 <https://www.gnu.org/licenses/>.
*
*/
#pragma once
/**
* HAL for stm32duino.com based on Libmaple and compatible (STM32F1)
*/
#include <libmaple/iwdg.h>
// Initialize watchdog with a 4 or 8 second countdown time
void watchdog_init();
// Reset watchdog. MUST be called every 4 or 8 seconds after the
// first watchdog_init or the STM32F1 will reset.
void HAL_watchdog_refresh();

62
Marlin/src/HAL/TEENSY31_32/HAL.cpp

@ -44,25 +44,6 @@
#endif
USBSerialType USBSerial(false, SerialUSB);
// ------------------------
// Class Utilities
// ------------------------
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;
}
}
// ------------------------
// MarlinHAL Class
// ------------------------
@ -81,7 +62,31 @@ uint8_t MarlinHAL::get_reset_source() {
return 0;
}
// ------------------------
// Watchdog Timer
// ------------------------
#if ENABLED(USE_WATCHDOG)
#define WDT_TIMEOUT_MS TERN(WATCHDOG_DURATION_8S, 8000, 4000) // 4 or 8 second timeout
void MarlinHAL::watchdog_init() {
WDOG_TOVALH = 0;
WDOG_TOVALL = WDT_TIMEOUT_MS;
WDOG_STCTRLH = WDOG_STCTRLH_WDOGEN;
}
void MarlinHAL::watchdog_refresh() {
// Watchdog refresh sequence
WDOG_REFRESH = 0xA602;
WDOG_REFRESH = 0xB480;
}
#endif
// ------------------------
// ADC
// ------------------------
void MarlinHAL::adc_init() {
analog_init();
@ -102,4 +107,23 @@ void MarlinHAL::adc_start(const pin_t pin) {
uint16_t MarlinHAL::adc_value() { return ADC0_RA; }
// ------------------------
// Free Memory Accessor
// ------------------------
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 // __MK20DX256__

5
Marlin/src/HAL/TEENSY31_32/HAL.h

@ -32,7 +32,6 @@
#include "../shared/HAL_SPI.h"
#include "fastio.h"
#include "watchdog.h"
#include <stdint.h>
@ -135,6 +134,10 @@ public:
// Earliest possible init, before setup()
MarlinHAL() {}
// Watchdog
static void watchdog_init() IF_DISABLED(USE_WATCHDOG, {});
static void watchdog_refresh() IF_DISABLED(USE_WATCHDOG, {});
static void init() {} // Called early in setup()
static void init_board() {} // Called less early in setup()
static void reboot(); // Restart the firmware from 0x0

6
Marlin/src/HAL/TEENSY31_32/endstop_interrupts.h

@ -70,4 +70,10 @@ void setup_endstop_interrupts() {
TERN_(HAS_J_MIN, _ATTACH(J_MIN_PIN));
TERN_(HAS_K_MAX, _ATTACH(K_MAX_PIN));
TERN_(HAS_K_MIN, _ATTACH(K_MIN_PIN));
TERN_(HAS_U_MAX, _ATTACH(U_MAX_PIN));
TERN_(HAS_U_MIN, _ATTACH(U_MIN_PIN));
TERN_(HAS_V_MAX, _ATTACH(V_MAX_PIN));
TERN_(HAS_V_MIN, _ATTACH(V_MIN_PIN));
TERN_(HAS_W_MAX, _ATTACH(W_MAX_PIN));
TERN_(HAS_W_MIN, _ATTACH(W_MIN_PIN));
}

40
Marlin/src/HAL/TEENSY31_32/watchdog.cpp

@ -1,40 +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 <https://www.gnu.org/licenses/>.
*
*/
#ifdef __MK20DX256__
#include "../../inc/MarlinConfig.h"
#if ENABLED(USE_WATCHDOG)
#include "watchdog.h"
#define WDT_TIMEOUT_MS TERN(WATCHDOG_DURATION_8S, 8000, 4000) // 4 or 8 second timeout
void watchdog_init() {
WDOG_TOVALH = 0;
WDOG_TOVALL = WDT_TIMEOUT_MS;
WDOG_STCTRLH = WDOG_STCTRLH_WDOGEN;
}
#endif // USE_WATCHDOG
#endif // __MK20DX256__

34
Marlin/src/HAL/TEENSY31_32/watchdog.h

@ -1,34 +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 <https://www.gnu.org/licenses/>.
*
*/
#pragma once
#include "HAL.h"
// Arduino Due core now has watchdog support
void watchdog_init();
inline void HAL_watchdog_refresh() {
// Watchdog refresh sequence
WDOG_REFRESH = 0xA602;
WDOG_REFRESH = 0xB480;
}

64
Marlin/src/HAL/TEENSY35_36/HAL.cpp

@ -43,33 +43,12 @@
USBSerialType USBSerial(false, SerialUSB);
// ------------------------
// Class Utilities
// ------------------------
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;
}
}
// ------------------------
// MarlinHAL Class
// ------------------------
void MarlinHAL::reboot() { _reboot_Teensyduino_(); }
// Reset
uint8_t MarlinHAL::get_reset_source() {
switch (RCM_SRS0) {
case 128: return RST_POWER_ON; break;
@ -82,7 +61,31 @@ uint8_t MarlinHAL::get_reset_source() {
return 0;
}
// ------------------------
// Watchdog Timer
// ------------------------
#if ENABLED(USE_WATCHDOG)
#define WDT_TIMEOUT_MS TERN(WATCHDOG_DURATION_8S, 8000, 4000) // 4 or 8 second timeout
void MarlinHAL::watchdog_init() {
WDOG_TOVALH = 0;
WDOG_TOVALL = WDT_TIMEOUT_MS;
WDOG_STCTRLH = WDOG_STCTRLH_WDOGEN;
}
void MarlinHAL::watchdog_refresh() {
// Watchdog refresh sequence
WDOG_REFRESH = 0xA602;
WDOG_REFRESH = 0xB480;
}
#endif
// ------------------------
// ADC
// ------------------------
int8_t MarlinHAL::adc_select;
@ -131,4 +134,23 @@ uint16_t MarlinHAL::adc_value() {
return 0;
}
// ------------------------
// Free Memory Accessor
// ------------------------
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 // __MK64FX512__ || __MK66FX1M0__

7
Marlin/src/HAL/TEENSY35_36/HAL.h

@ -32,7 +32,6 @@
#include "../shared/HAL_SPI.h"
#include "fastio.h"
#include "watchdog.h"
#include <stdint.h>
#include <util/atomic.h>
@ -118,7 +117,7 @@ typedef int8_t pin_t;
#define PARSED_PIN_INDEX(code, dval) parser.intval(code, dval)
// ------------------------
// Class Utilities
// Free Memory Accessor
// ------------------------
#pragma GCC diagnostic push
@ -140,6 +139,10 @@ public:
// Earliest possible init, before setup()
MarlinHAL() {}
// Watchdog
static void watchdog_init() IF_DISABLED(USE_WATCHDOG, {});
static void watchdog_refresh() IF_DISABLED(USE_WATCHDOG, {});
static void init() {} // Called early in setup()
static void init_board() {} // Called less early in setup()
static void reboot(); // Restart the firmware from 0x0

6
Marlin/src/HAL/TEENSY35_36/endstop_interrupts.h

@ -69,4 +69,10 @@ void setup_endstop_interrupts() {
TERN_(HAS_J_MIN, _ATTACH(J_MIN_PIN));
TERN_(HAS_K_MAX, _ATTACH(K_MAX_PIN));
TERN_(HAS_K_MIN, _ATTACH(K_MIN_PIN));
TERN_(HAS_U_MAX, _ATTACH(U_MAX_PIN));
TERN_(HAS_U_MIN, _ATTACH(U_MIN_PIN));
TERN_(HAS_V_MAX, _ATTACH(V_MAX_PIN));
TERN_(HAS_V_MIN, _ATTACH(V_MIN_PIN));
TERN_(HAS_W_MAX, _ATTACH(W_MAX_PIN));
TERN_(HAS_W_MIN, _ATTACH(W_MIN_PIN));
}

40
Marlin/src/HAL/TEENSY35_36/watchdog.cpp

@ -1,40 +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 <https://www.gnu.org/licenses/>.
*
*/
#if defined(__MK64FX512__) || defined(__MK66FX1M0__)
#include "../../inc/MarlinConfig.h"
#if ENABLED(USE_WATCHDOG)
#include "watchdog.h"
#define WDT_TIMEOUT_MS TERN(WATCHDOG_DURATION_8S, 8000, 4000) // 4 or 8 second timeout
void watchdog_init() {
WDOG_TOVALH = 0;
WDOG_TOVALL = WDT_TIMEOUT_MS;
WDOG_STCTRLH = WDOG_STCTRLH_WDOGEN;
}
#endif // USE_WATCHDOG
#endif // __MK64FX512__ || __MK66FX1M0__

30
Marlin/src/HAL/TEENSY35_36/watchdog.h

@ -1,30 +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 <https://www.gnu.org/licenses/>.
*
*/
#pragma once
void watchdog_init();
inline void HAL_watchdog_refresh() {
// Watchdog refresh sequence
WDOG_REFRESH = 0xA602;
WDOG_REFRESH = 0xB480;
}

65
Marlin/src/HAL/TEENSY40_41/HAL.cpp

@ -44,25 +44,6 @@
#endif
USBSerialType USBSerial(false, SerialUSB);
// ------------------------
// 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;
}
}
// ------------------------
// FastIO
// ------------------------
@ -97,7 +78,34 @@ void MarlinHAL::clear_reset_source() {
SRC_SRSR = reset_source;
}
// ------------------------
// Watchdog Timer
// ------------------------
#if ENABLED(USE_WATCHDOG)
#define WDT_TIMEOUT TERN(WATCHDOG_DURATION_8S, 8, 4) // 4 or 8 second timeout
constexpr uint8_t timeoutval = (WDT_TIMEOUT - 0.5f) / 0.5f;
void MarlinHAL::watchdog_init() {
CCM_CCGR3 |= CCM_CCGR3_WDOG1(3); // enable WDOG1 clocks
WDOG1_WMCR = 0; // disable power down PDE
WDOG1_WCR |= WDOG_WCR_SRS | WDOG_WCR_WT(timeoutval);
WDOG1_WCR |= WDOG_WCR_WDE | WDOG_WCR_WDT | WDOG_WCR_SRE;
}
void MarlinHAL::watchdog_refresh() {
// Watchdog refresh sequence
WDOG1_WSR = 0x5555;
WDOG1_WSR = 0xAAAA;
}
#endif
// ------------------------
// ADC
// ------------------------
int8_t MarlinHAL::adc_select;
@ -180,4 +188,23 @@ uint16_t MarlinHAL::adc_value() {
return 0;
}
// ------------------------
// Free Memory Accessor
// ------------------------
#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;
}
}
#endif // __IMXRT1062__

7
Marlin/src/HAL/TEENSY40_41/HAL.h

@ -32,7 +32,6 @@
#include "../shared/HAL_SPI.h"
#include "fastio.h"
#include "watchdog.h"
#include <stdint.h>
#include <util/atomic.h>
@ -140,7 +139,7 @@ typedef int8_t pin_t;
bool is_output(pin_t pin);
// ------------------------
// Class Utilities
// Free Memory Accessor
// ------------------------
#pragma GCC diagnostic push
@ -162,6 +161,10 @@ public:
// Earliest possible init, before setup()
MarlinHAL() {}
// Watchdog
static void watchdog_init() IF_DISABLED(USE_WATCHDOG, {});
static void watchdog_refresh() IF_DISABLED(USE_WATCHDOG, {});
static void init() {} // Called early in setup()
static void init_board() {} // Called less early in setup()
static void reboot(); // Restart the firmware from 0x0

52
Marlin/src/HAL/TEENSY40_41/watchdog.cpp

@ -1,52 +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 <https://www.gnu.org/licenses/>.
*
*/
#ifdef __IMXRT1062__
/**
* HAL Watchdog for Teensy 4.0 (IMXRT1062DVL6A) / 4.1 (IMXRT1062DVJ6A)
*/
#include "../../inc/MarlinConfig.h"
#if ENABLED(USE_WATCHDOG)
#include "watchdog.h"
#define WDT_TIMEOUT TERN(WATCHDOG_DURATION_8S, 8, 4) // 4 or 8 second timeout
constexpr uint8_t timeoutval = (WDT_TIMEOUT - 0.5f) / 0.5f;
void watchdog_init() {
CCM_CCGR3 |= CCM_CCGR3_WDOG1(3); // enable WDOG1 clocks
WDOG1_WMCR = 0; // disable power down PDE
WDOG1_WCR |= WDOG_WCR_SRS | WDOG_WCR_WT(timeoutval);
WDOG1_WCR |= WDOG_WCR_WDE | WDOG_WCR_WDT | WDOG_WCR_SRE;
}
void HAL_watchdog_refresh() {
// Watchdog refresh sequence
WDOG1_WSR = 0x5555;
WDOG1_WSR = 0xAAAA;
}
#endif // USE_WATCHDOG
#endif // __IMXRT1062__

30
Marlin/src/HAL/TEENSY40_41/watchdog.h

@ -1,30 +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 <https://www.gnu.org/licenses/>.
*
*/
#pragma once
/**
* HAL Watchdog for Teensy 4.0 (IMXRT1062DVL6A) / 4.1 (IMXRT1062DVJ6A)
*/
void watchdog_init();
void HAL_watchdog_refresh();

4
Marlin/src/HAL/shared/backtrace/unwarmbytab.cpp

@ -135,11 +135,11 @@ static UnwResult UnwTabExecuteInstructions(const UnwindCallbacks *cb, UnwTabStat
while ((instruction = UnwTabGetNextInstruction(cb, ucb)) != -1) {
if ((instruction & 0xC0) == 0x00) { // ARM_EXIDX_CMD_DATA_POP
/* vsp = vsp + (xxxxxx << 2) + 4 */
/* vsp += (xxxxxx << 2) + 4 */
ucb->vrs[13] += ((instruction & 0x3F) << 2) + 4;
}
else if ((instruction & 0xC0) == 0x40) { // ARM_EXIDX_CMD_DATA_PUSH
/* vsp = vsp - (xxxxxx << 2) - 4 */
/* vsp -= (xxxxxx << 2) - 4 */
ucb->vrs[13] -= ((instruction & 0x3F) << 2) - 4;
}
else if ((instruction & 0xF0) == 0x80) {

2
Marlin/src/HAL/shared/cpu_exception/exception_arm.cpp

@ -221,7 +221,7 @@ bool resume_from_fault() {
// So we'll just need to refresh the watchdog for a while and then stop for the system to reboot
uint32_t last = start;
while (PENDING(last, end)) {
watchdog_refresh();
hal.watchdog_refresh();
while (millis() == last) { /* nada */ }
last = millis();
MinSerial::TX('.');

13
Marlin/src/HAL/shared/servo.cpp

@ -65,7 +65,7 @@ uint8_t ServoCount = 0; // the total number of attached
/************ static functions common to all instances ***********************/
static boolean isTimerActive(timer16_Sequence_t timer) {
static bool anyTimerChannelActive(const timer16_Sequence_t timer) {
// returns true if any servo is active on this timer
LOOP_L_N(channel, SERVOS_PER_TIMER) {
if (SERVO(timer, channel).Pin.isActive)
@ -101,17 +101,18 @@ int8_t Servo::attach(const int inPin, const int inMin, const int inMax) {
max = (MAX_PULSE_WIDTH - inMax) / 4;
// initialize the timer if it has not already been initialized
timer16_Sequence_t timer = SERVO_INDEX_TO_TIMER(servoIndex);
if (!isTimerActive(timer)) initISR(timer);
servo_info[servoIndex].Pin.isActive = true; // this must be set after the check for isTimerActive
const timer16_Sequence_t timer = SERVO_INDEX_TO_TIMER(servoIndex);
if (!anyTimerChannelActive(timer)) initISR(timer);
servo_info[servoIndex].Pin.isActive = true; // this must be set after the check for anyTimerChannelActive
return servoIndex;
}
void Servo::detach() {
servo_info[servoIndex].Pin.isActive = false;
timer16_Sequence_t timer = SERVO_INDEX_TO_TIMER(servoIndex);
if (!isTimerActive(timer)) finISR(timer);
const timer16_Sequence_t timer = SERVO_INDEX_TO_TIMER(servoIndex);
if (!anyTimerChannelActive(timer)) finISR(timer);
//pinMode(servo_info[servoIndex].Pin.nbr, INPUT); // set servo pin to input
}
void Servo::write(int value) {

12
Marlin/src/HAL/shared/servo_private.h

@ -70,10 +70,10 @@
#define ticksToUs(_ticks) (unsigned(_ticks) * (SERVO_TIMER_PRESCALER) / clockCyclesPerMicrosecond())
// convenience macros
#define SERVO_INDEX_TO_TIMER(_servo_nbr) ((timer16_Sequence_t)(_servo_nbr / (SERVOS_PER_TIMER))) // returns the timer controlling this servo
#define SERVO_INDEX_TO_CHANNEL(_servo_nbr) (_servo_nbr % (SERVOS_PER_TIMER)) // returns the index of the servo on this timer
#define SERVO_INDEX(_timer,_channel) ((_timer*(SERVOS_PER_TIMER)) + _channel) // macro to access servo index by timer and channel
#define SERVO(_timer,_channel) (servo_info[SERVO_INDEX(_timer,_channel)]) // macro to access servo class by timer and channel
#define SERVO_INDEX_TO_TIMER(_servo_nbr) timer16_Sequence_t(_servo_nbr / (SERVOS_PER_TIMER)) // the timer controlling this servo
#define SERVO_INDEX_TO_CHANNEL(_servo_nbr) (_servo_nbr % (SERVOS_PER_TIMER)) // the index of the servo on this timer
#define SERVO_INDEX(_timer,_channel) ((_timer*(SERVOS_PER_TIMER)) + _channel) // servo index by timer and channel
#define SERVO(_timer,_channel) servo_info[SERVO_INDEX(_timer,_channel)] // servo class by timer and channel
// Types
@ -94,5 +94,5 @@ extern ServoInfo_t servo_info[MAX_SERVOS];
// Public functions
extern void initISR(timer16_Sequence_t timer);
extern void finISR(timer16_Sequence_t timer);
void initISR(const timer16_Sequence_t timer_index);
void finISR(const timer16_Sequence_t timer_index);

111
Marlin/src/MarlinCore.cpp

@ -97,7 +97,7 @@
#include "feature/host_actions.h"
#endif
#if USE_BEEPER
#if HAS_BEEPER
#include "libs/buzzer.h"
#endif
@ -417,38 +417,44 @@ inline void manage_inactivity(const bool no_stepper_sleep=false) {
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);
SERIAL_ERROR_START();
SERIAL_ECHOPGM(STR_KILL_PRE);
SERIAL_ECHOLNPGM(STR_KILL_INACTIVE_TIME, parser.command_ptr);
kill();
}
// M18 / M84 : Handle steppers inactive time timeout
if (gcode.stepper_inactive_time) {
static bool already_shutdown_steppers; // = false
const bool has_blocks = planner.has_blocks_queued(); // Any moves in the planner?
if (has_blocks) gcode.reset_stepper_timeout(ms); // Reset timeout for M18/M84, M85 max 'kill', and laser.
// Any moves in the planner? Resets both the M18/M84
// activity timeout and the M85 max 'kill' timeout
if (planner.has_blocks_queued())
gcode.reset_stepper_timeout(ms);
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
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());
// M18 / M84 : Handle steppers inactive time timeout
#if HAS_DISABLE_INACTIVE_AXIS
if (gcode.stepper_inactive_time) {
static bool already_shutdown_steppers; // = false
if (!has_blocks && !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
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_U, stepper.disable_axis(U_AXIS));
TERN_(DISABLE_INACTIVE_V, stepper.disable_axis(V_AXIS));
TERN_(DISABLE_INACTIVE_W, stepper.disable_axis(W_AXIS));
TERN_(DISABLE_INACTIVE_E, stepper.disable_e_steppers());
TERN_(AUTO_BED_LEVELING_UBL, bedlevel.steppers_were_disabled());
}
}
else
already_shutdown_steppers = false;
}
else
already_shutdown_steppers = false;
}
#endif
#if ENABLED(PHOTO_GCODE) && PIN_EXISTS(CHDK)
// Check if CHDK should be set to LOW (after M240 set it HIGH)
@ -475,7 +481,9 @@ inline void manage_inactivity(const bool no_stepper_sleep=false) {
// KILL the machine
// ----------------------------------------------------------------
if (killCount >= KILL_DELAY) {
SERIAL_ERROR_MSG(STR_KILL_BUTTON);
SERIAL_ERROR_START();
SERIAL_ECHOPGM(STR_KILL_PRE);
SERIAL_ECHOLNPGM(STR_KILL_BUTTON);
kill();
}
#endif
@ -768,6 +776,10 @@ inline void manage_inactivity(const bool no_stepper_sleep=false) {
* - Handle Joystick jogging
*/
void idle(bool no_stepper_sleep/*=false*/) {
#ifdef MAX7219_DEBUG_PROFILE
CodeProfiler idle_profiler;
#endif
#if ENABLED(MARLIN_DEV_MODE)
static uint16_t idle_depth = 0;
if (++idle_depth > 5) SERIAL_ECHOLNPGM("idle() call depth: ", idle_depth);
@ -824,7 +836,7 @@ void idle(bool no_stepper_sleep/*=false*/) {
TERN_(PRINTCOUNTER, print_job_timer.tick());
// Update the Beeper queue
TERN_(USE_BEEPER, buzzer.tick());
TERN_(HAS_BEEPER, buzzer.tick());
// Handle UI input / draw events
TERN(DWIN_CREALITY_LCD, DWIN_Update(), ui.update());
@ -927,18 +939,18 @@ void minkill(const bool steppers_off/*=false*/) {
// 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();
hal.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();
hal.watchdog_refresh();
// Reboot the board
hal.reboot();
#else
for (;;) watchdog_refresh(); // Wait for RESET button or power-cycle
for (;;) hal.watchdog_refresh(); // Wait for RESET button or power-cycle
#endif
}
@ -998,6 +1010,15 @@ inline void tmc_standby_setup() {
#if PIN_EXISTS(K_STDBY)
SET_INPUT_PULLDOWN(K_STDBY_PIN);
#endif
#if PIN_EXISTS(U_STDBY)
SET_INPUT_PULLDOWN(U_STDBY_PIN);
#endif
#if PIN_EXISTS(V_STDBY)
SET_INPUT_PULLDOWN(V_STDBY_PIN);
#endif
#if PIN_EXISTS(W_STDBY)
SET_INPUT_PULLDOWN(W_STDBY_PIN);
#endif
#if PIN_EXISTS(E0_STDBY)
SET_INPUT_PULLDOWN(E0_STDBY_PIN);
#endif
@ -1225,6 +1246,17 @@ void setup() {
SETUP_RUN(tmc_serial_begin());
#endif
#if HAS_TMC_SPI
#if DISABLED(TMC_USE_SW_SPI)
SETUP_RUN(SPI.begin());
#endif
SETUP_RUN(tmc_init_cs_pins());
#endif
#if HAS_L64XX
SETUP_RUN(L64xxManager.init()); // Set up SPI, init drivers
#endif
#if ENABLED(PSU_CONTROL)
SETUP_LOG("PSU_CONTROL");
powerManager.init();
@ -1234,21 +1266,10 @@ void setup() {
SETUP_RUN(recovery.setup());
#endif
#if HAS_L64XX
SETUP_RUN(L64xxManager.init()); // Set up SPI, init drivers
#endif
#if HAS_STEPPER_RESET
SETUP_RUN(disableStepperDrivers());
#endif
#if HAS_TMC_SPI
#if DISABLED(TMC_USE_SW_SPI)
SETUP_RUN(SPI.begin());
#endif
SETUP_RUN(tmc_init_cs_pins());
#endif
SETUP_RUN(hal.init_board());
SETUP_RUN(esp_wifi_init());
@ -1275,7 +1296,7 @@ void setup() {
calibrate_delay_loop();
// Init buzzer pin(s)
#if USE_BEEPER
#if HAS_BEEPER
SETUP_RUN(buzzer.init());
#endif
@ -1545,7 +1566,7 @@ void setup() {
#endif
#if ENABLED(USE_WATCHDOG)
SETUP_RUN(watchdog_init()); // Reinit watchdog after hal.get_reset_source call
SETUP_RUN(hal.watchdog_init()); // Reinit watchdog after hal.get_reset_source call
#endif
#if ENABLED(EXTERNAL_CLOSED_LOOP_CONTROLLER)

44
Marlin/src/core/boards.h

@ -161,7 +161,7 @@
#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_MALYAN_M180 1327 // Malyan M180 Mainboard Version 2 (no display function, direct G-code 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
#define BOARD_WEEDO_62A 1330 // WEEDO 62A board (TINA2, Monoprice Cadet, etc.)
@ -228,33 +228,33 @@
#define BOARD_RAMPS_14_RE_ARM_EFF 2002 // Re-ARM with RAMPS 1.4 (Power outputs: Hotend, Fan0, Fan1)
#define BOARD_RAMPS_14_RE_ARM_EEF 2003 // Re-ARM with RAMPS 1.4 (Power outputs: Hotend0, Hotend1, Fan)
#define BOARD_RAMPS_14_RE_ARM_SF 2004 // Re-ARM with RAMPS 1.4 (Power outputs: Spindle, Controller Fan)
#define BOARD_MKS_SBASE 2005 // MKS-Sbase (Power outputs: Hotend0, Hotend1, Bed, Fan)
#define BOARD_MKS_SBASE 2005 // MKS-Sbase
#define BOARD_AZSMZ_MINI 2006 // AZSMZ Mini
#define BOARD_BIQU_BQ111_A4 2007 // BIQU BQ111-A4 (Power outputs: Hotend, Fan, Bed)
#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_BIQU_BQ111_A4 2007 // BIQU BQ111-A4
#define BOARD_SELENA_COMPACT 2008 // Selena Compact
#define BOARD_BIQU_B300_V1_0 2009 // BIQU B300_V1.0
#define BOARD_MKS_SGEN_L 2010 // MKS-SGen-L
#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)
#define BOARD_BTT_SKR_V1_1 2012 // BigTreeTech SKR v1.1
#define BOARD_BTT_SKR_V1_3 2013 // BigTreeTech SKR v1.3
#define BOARD_BTT_SKR_V1_4 2014 // BigTreeTech SKR v1.4
//
// LPC1769 ARM Cortex M3
//
#define BOARD_MKS_SGEN 2500 // MKS-SGen (Power outputs: Hotend0, Hotend1, Bed, Fan)
#define BOARD_AZTEEG_X5_GT 2501 // Azteeg X5 GT (Power outputs: Hotend0, Hotend1, Bed, Fan)
#define BOARD_AZTEEG_X5_MINI 2502 // Azteeg X5 Mini (Power outputs: Hotend0, Bed, Fan)
#define BOARD_AZTEEG_X5_MINI_WIFI 2503 // Azteeg X5 Mini Wifi (Power outputs: Hotend0, Bed, Fan)
#define BOARD_MKS_SGEN 2500 // MKS-SGen
#define BOARD_AZTEEG_X5_GT 2501 // Azteeg X5 GT
#define BOARD_AZTEEG_X5_MINI 2502 // Azteeg X5 Mini
#define BOARD_AZTEEG_X5_MINI_WIFI 2503 // Azteeg X5 Mini Wifi
#define BOARD_COHESION3D_REMIX 2504 // Cohesion3D ReMix
#define BOARD_COHESION3D_MINI 2505 // Cohesion3D Mini
#define BOARD_SMOOTHIEBOARD 2506 // Smoothieboard
#define BOARD_TH3D_EZBOARD 2507 // TH3D EZBoard v1.0
#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 // FLYmaker FLY CDY (Power outputs: Hotend0, Hotend1, Hotend2, Bed, Fan0, Fan1, Fan2)
#define BOARD_BTT_SKR_V1_4_TURBO 2508 // BigTreeTech SKR v1.4 TURBO
#define BOARD_MKS_SGEN_L_V2 2509 // MKS SGEN_L V2
#define BOARD_BTT_SKR_E3_TURBO 2510 // BigTreeTech SKR E3 Turbo
#define BOARD_FLY_CDY 2511 // FLYmaker FLY CDY
//
// SAM3X8E ARM Cortex M3
@ -280,8 +280,8 @@
#define BOARD_RAMPS4DUE_EFF 3017 // RAMPS4DUE (Power outputs: Hotend, Fan0, Fan1)
#define BOARD_RAMPS4DUE_EEF 3018 // RAMPS4DUE (Power outputs: Hotend0, Hotend1, Fan)
#define BOARD_RAMPS4DUE_SF 3019 // RAMPS4DUE (Power outputs: Spindle, Controller Fan)
#define BOARD_RURAMPS4D_11 3020 // RuRAMPS4Duo v1.1 (Power outputs: Hotend0, Hotend1, Hotend2, Fan0, Fan1, Bed)
#define BOARD_RURAMPS4D_13 3021 // RuRAMPS4Duo v1.3 (Power outputs: Hotend0, Hotend1, Hotend2, Fan0, Fan1, Bed)
#define BOARD_RURAMPS4D_11 3020 // RuRAMPS4Duo v1.1
#define BOARD_RURAMPS4D_13 3021 // RuRAMPS4Duo v1.3
#define BOARD_ULTRATRONICS_PRO 3022 // ReprapWorld Ultratronics Pro V1.0
#define BOARD_ARCHIM1 3023 // UltiMachine Archim1 (with DRV8825 drivers)
#define BOARD_ARCHIM2 3024 // UltiMachine Archim2 (with TMC2130 drivers)
@ -293,7 +293,7 @@
// SAM3X8C ARM Cortex M3
//
#define BOARD_PRINTRBOARD_G2 3100 // PRINTRBOARD G2
#define BOARD_PRINTRBOARD_G2 3100 // Printrboard G2
#define BOARD_ADSK 3101 // Arduino DUE Shield Kit (ADSK)
//
@ -413,7 +413,7 @@
#define BOARD_ANET_ET4P 4232 // ANET ET4P V1.x (STM32F407VG)
#define BOARD_FYSETC_CHEETAH_V20 4233 // FYSETC Cheetah V2.0
#define BOARD_TH3D_EZBOARD_V2 4234 // TH3D EZBoard v2.0
#define BOARD_INDEX_REV03 4235 // Index PnP Controller REV03 (STM32F407VE/VG)
#define BOARD_OPULO_LUMEN_REV3 4235 // Opulo Lumen PnP Controller REV3 (STM32F407VE/VG)
#define BOARD_MKS_ROBIN_NANO_V1_3_F4 4236 // MKS Robin Nano V1.3 and MKS Robin Nano-S V1.3 (STM32F407VE)
#define BOARD_MKS_EAGLE 4237 // MKS Eagle (STM32F407VE)
#define BOARD_ARTILLERY_RUBY 4238 // Artillery Ruby (STM32F401RC)
@ -429,6 +429,8 @@
#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)
#define BOARD_BTT_SKR_V3_0 5005 // BigTreeTech SKR V3.0 (STM32H743VG)
#define BOARD_BTT_SKR_V3_0_EZ 5006 // BigTreeTech SKR V3.0 EZ (STM32H743VG)
//
// Espressif ESP32 WiFi

15
Marlin/src/core/drivers.h

@ -63,12 +63,15 @@
#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_U(T) _AXIS_DRIVER_TYPE(U,T)
#define AXIS_DRIVER_TYPE_V(T) _AXIS_DRIVER_TYPE(V,T)
#define AXIS_DRIVER_TYPE_W(T) _AXIS_DRIVER_TYPE(W,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))
#define AXIS_DRIVER_TYPE_Z2(T) (NUM_Z_STEPPER_DRIVERS >= 2 && _AXIS_DRIVER_TYPE(Z2,T))
#define AXIS_DRIVER_TYPE_Z3(T) (NUM_Z_STEPPER_DRIVERS >= 3 && _AXIS_DRIVER_TYPE(Z3,T))
#define AXIS_DRIVER_TYPE_Z4(T) (NUM_Z_STEPPER_DRIVERS >= 4 && _AXIS_DRIVER_TYPE(Z4,T))
#define AXIS_DRIVER_TYPE_X2(T) (HAS_X2_STEPPER && _AXIS_DRIVER_TYPE(X2,T))
#define AXIS_DRIVER_TYPE_Y2(T) (HAS_DUAL_Y_STEPPERS && _AXIS_DRIVER_TYPE(Y2,T))
#define AXIS_DRIVER_TYPE_Z2(T) (NUM_Z_STEPPERS >= 2 && _AXIS_DRIVER_TYPE(Z2,T))
#define AXIS_DRIVER_TYPE_Z3(T) (NUM_Z_STEPPERS >= 3 && _AXIS_DRIVER_TYPE(Z3,T))
#define AXIS_DRIVER_TYPE_Z4(T) (NUM_Z_STEPPERS >= 4 && _AXIS_DRIVER_TYPE(Z4,T))
#define AXIS_DRIVER_TYPE_E(N,T) (E_STEPPERS > N && _AXIS_DRIVER_TYPE(E##N,T))
#define AXIS_DRIVER_TYPE_E0(T) AXIS_DRIVER_TYPE_E(0,T)
@ -87,6 +90,7 @@
#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_U(T) || AXIS_DRIVER_TYPE_V(T) || AXIS_DRIVER_TYPE_W(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) )
@ -161,6 +165,7 @@
|| 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) \
|| AXIS_HAS_##T(U) || AXIS_HAS_##T(V) || AXIS_HAS_##T(W) \
|| E_AXIS_HAS(T) )
#if ANY_AXIS_HAS(STEALTHCHOP)

79
Marlin/src/core/language.h

@ -199,16 +199,20 @@
#define STR_FILAMENT_CHANGE_INSERT_M108 "Insert filament and send M108"
#define STR_FILAMENT_CHANGE_WAIT_M108 "Send M108 to resume"
#define STR_STOP_BLTOUCH "!! STOP called because of BLTouch error - restart with M999"
#define STR_STOP_UNHOMED "!! STOP called because of unhomed error - restart with M999"
#define STR_KILL_INACTIVE_TIME "!! KILL caused by too much inactive time - current command: "
#define STR_KILL_BUTTON "!! KILL caused by KILL button/pin"
#define STR_STOP_PRE "!! STOP called because of "
#define STR_STOP_POST " error - restart with M999"
#define STR_STOP_BLTOUCH "BLTouch"
#define STR_STOP_UNHOMED "unhomed"
#define STR_KILL_PRE "!! KILL caused by "
#define STR_KILL_INACTIVE_TIME "too much inactive time - current command: "
#define STR_KILL_BUTTON "KILL button/pin"
// temperature.cpp strings
#define STR_PID_AUTOTUNE_START "PID Autotune start"
#define STR_PID_BAD_HEATER_ID "PID Autotune failed! Bad heater id"
#define STR_PID_TEMP_TOO_HIGH "PID Autotune failed! Temperature too high"
#define STR_PID_TIMEOUT "PID Autotune failed! timeout"
#define STR_PID_AUTOTUNE "PID Autotune"
#define STR_PID_AUTOTUNE_START " start"
#define STR_PID_BAD_HEATER_ID " failed! Bad heater id"
#define STR_PID_TEMP_TOO_HIGH " failed! Temperature too high"
#define STR_PID_TIMEOUT " failed! timeout"
#define STR_BIAS " bias: "
#define STR_D_COLON " d: "
#define STR_T_MIN " min: "
@ -219,7 +223,7 @@
#define STR_KP " Kp: "
#define STR_KI " Ki: "
#define STR_KD " Kd: "
#define STR_PID_AUTOTUNE_FINISHED "PID Autotune finished! Put the last Kp, Ki and Kd constants from below into Configuration.h"
#define STR_PID_AUTOTUNE_FINISHED " finished! Put the last Kp, Ki and Kd constants from below into Configuration.h"
#define STR_PID_DEBUG " PID_DEBUG "
#define STR_PID_DEBUG_INPUT ": Input "
#define STR_PID_DEBUG_OUTPUT " Output "
@ -228,6 +232,14 @@
#define STR_PID_DEBUG_DTERM " dTerm "
#define STR_PID_DEBUG_CTERM " cTerm "
#define STR_INVALID_EXTRUDER_NUM " - Invalid extruder number !"
#define STR_MPC_AUTOTUNE "MPC Autotune"
#define STR_MPC_AUTOTUNE_START " start for " STR_E
#define STR_MPC_AUTOTUNE_INTERRUPTED " interrupted!"
#define STR_MPC_AUTOTUNE_FINISHED " finished! Put the constants below into Configuration.h"
#define STR_MPC_COOLING_TO_AMBIENT "Cooling to ambient"
#define STR_MPC_HEATING_PAST_200 "Heating to over 200C"
#define STR_MPC_MEASURING_AMBIENT "Measuring ambient heatloss at "
#define STR_MPC_TEMPERATURE_ERROR "Temperature error"
#define STR_HEATER_BED "bed"
#define STR_HEATER_CHAMBER "chamber"
@ -303,6 +315,7 @@
#define STR_MATERIAL_HEATUP "Material heatup parameters"
#define STR_LCD_CONTRAST "LCD Contrast"
#define STR_LCD_BRIGHTNESS "LCD Brightness"
#define STR_DISPLAY_SLEEP "Display Sleep"
#define STR_UI_LANGUAGE "UI Language"
#define STR_Z_PROBE_OFFSET "Z-Probe Offset"
#define STR_TEMPERATURE_UNITS "Temperature Units"
@ -444,6 +457,54 @@
#define STR_K ""
#endif
#if HAS_U_AXIS
#if AXIS7_NAME == 'U'
#define STR_U "U"
#define STR_U_MIN "u_min"
#define STR_U_MAX "u_max"
#elif AXIS7_NAME == 'V'
#define STR_U "V"
#define STR_U_MIN "v_min"
#define STR_U_MAX "v_max"
#elif AXIS7_NAME == 'W'
#define STR_U "W"
#define STR_U_MIN "w_min"
#define STR_U_MAX "w_max"
#else
#error "AXIS7_NAME can only be one of 'U', 'V', or 'W'."
#endif
#else
#define STR_U ""
#endif
#if HAS_V_AXIS
#if AXIS8_NAME == 'V'
#define STR_V "V"
#define STR_V_MIN "v_min"
#define STR_V_MAX "v_max"
#elif AXIS8_NAME == 'W'
#define STR_V "W"
#define STR_V_MIN "w_min"
#define STR_V_MAX "w_max"
#else
#error "AXIS8_NAME can only be one of 'V', or 'W'."
#endif
#else
#define STR_V ""
#endif
#if HAS_W_AXIS
#if AXIS9_NAME == 'W'
#define STR_W "W"
#define STR_W_MIN "w_min"
#define STR_W_MAX "w_max"
#else
#error "AXIS9_NAME can only be 'W'."
#endif
#else
#define STR_W ""
#endif
#if EITHER(HAS_MARLINUI_HD44780, IS_TFTGLCD_PANEL)
// Custom characters defined in the first 8 characters of the LCD

48
Marlin/src/core/macros.h

@ -39,24 +39,36 @@
#define _ISTOP_ 0x04
#define _JSTOP_ 0x05
#define _KSTOP_ 0x06
#define _USTOP_ 0x07
#define _VSTOP_ 0x08
#define _WSTOP_ 0x09
#define _XMIN_ 0x11
#define _YMIN_ 0x12
#define _ZMIN_ 0x13
#define _IMIN_ 0x14
#define _JMIN_ 0x15
#define _KMIN_ 0x16
#define _UMIN_ 0x17
#define _VMIN_ 0x18
#define _WMIN_ 0x19
#define _XMAX_ 0x21
#define _YMAX_ 0x22
#define _ZMAX_ 0x23
#define _IMAX_ 0x24
#define _JMAX_ 0x25
#define _KMAX_ 0x26
#define _UMAX_ 0x27
#define _VMAX_ 0x28
#define _WMAX_ 0x29
#define _XDIAG_ 0x31
#define _YDIAG_ 0x32
#define _ZDIAG_ 0x33
#define _IDIAG_ 0x34
#define _JDIAG_ 0x35
#define _KDIAG_ 0x36
#define _UDIAG_ 0x37
#define _VDIAG_ 0x38
#define _WDIAG_ 0x39
#define _E0DIAG_ 0xE0
#define _E1DIAG_ 0xE1
#define _E2DIAG_ 0xE2
@ -350,7 +362,7 @@
#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 LIST_N_1(N,K) _LIST_N(N,K,K,K,K,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) }
@ -632,8 +644,8 @@
#define IS_PROBE(V...) SECOND(V, 0) // Get the second item passed, or 0
#define PROBE() ~, 1 // Second item will be 1 if this is passed
#define _NOT_0 PROBE()
#define NOT(x) IS_PROBE(_CAT(_NOT_, x)) // NOT('0') gets '1'. Anything else gets '0'.
#define _BOOL(x) NOT(NOT(x)) // NOT('0') gets '0'. Anything else gets '1'.
#define NOT(x) IS_PROBE(_CAT(_NOT_, x)) // NOT('0') gets '1'. Anything else gets '0'.
#define _BOOL(x) NOT(NOT(x)) // _BOOL('0') gets '0'. Anything else gets '1'.
#define IF_ELSE(TF) _IF_ELSE(_BOOL(TF))
#define _IF_ELSE(TF) _CAT(_IF_, TF)
@ -647,7 +659,6 @@
#define HAS_ARGS(V...) _BOOL(FIRST(_END_OF_ARGUMENTS_ V)())
#define _END_OF_ARGUMENTS_() 0
// Simple Inline IF Macros, friendly to use in other macro definitions
#define IF(O, A, B) ((O) ? (A) : (B))
#define IF_0(O, A) IF(O, A, 0)
@ -700,13 +711,22 @@
#define RREPEAT2_S(S,N,OP,V...) EVAL1024(_RREPEAT2(S,SUB##S(N),OP,V))
#define RREPEAT2(N,OP,V...) RREPEAT2_S(0,N,OP,V)
// See https://github.com/swansontec/map-macro
#define MAP_OUT
#define MAP_END(...)
#define MAP_GET_END() 0, MAP_END
#define MAP_NEXT0(test, next, ...) next MAP_OUT
#define MAP_NEXT1(test, next) MAP_NEXT0 (test, next, 0)
#define MAP_NEXT(test, next) MAP_NEXT1 (MAP_GET_END test, next)
#define MAP0(f, x, peek, ...) f(x) MAP_NEXT (peek, MAP1) (f, peek, __VA_ARGS__)
#define MAP1(f, x, peek, ...) f(x) MAP_NEXT (peek, MAP0) (f, peek, __VA_ARGS__)
#define MAP(f, ...) EVAL512 (MAP1 (f, __VA_ARGS__, (), 0))
// Call OP(A) with each item as an argument
#define _MAP(_MAP_OP,A,V...) \
_MAP_OP(A) \
IF_ELSE(HAS_ARGS(V)) \
( DEFER2(__MAP)()(_MAP_OP,V) ) \
( /* Do nothing */ )
#define __MAP() _MAP
#define MAP(OP,V...) EVAL(_MAP(OP,V))
// Emit a list of OP(A) with the given items
#define _MAPLIST(_MAP_OP,A,V...) \
_MAP_OP(A) \
IF_ELSE(HAS_ARGS(V)) \
( , DEFER2(__MAPLIST)()(_MAP_OP,V) ) \
( /* Do nothing */ )
#define __MAPLIST() _MAPLIST
#define MAPLIST(OP,V...) EVAL(_MAPLIST(OP,V))

1
Marlin/src/core/multi_language.h

@ -45,6 +45,7 @@ typedef const char Language_Str[];
// Set unused languages equal to each other so the
// compiler can optimize away the conditionals.
#define LCD_LANGUAGE_1 LCD_LANGUAGE
#ifndef LCD_LANGUAGE_2
#define LCD_LANGUAGE_2 LCD_LANGUAGE
#endif

27
Marlin/src/core/serial.cpp

@ -30,16 +30,15 @@
uint8_t marlin_debug_flags = MARLIN_DEBUG_NONE;
// Commonly-used strings in serial output
PGMSTR(NUL_STR, ""); PGMSTR(SP_P_STR, " P"); PGMSTR(SP_T_STR, " T");
PGMSTR(X_STR, "X"); PGMSTR(Y_STR, "Y"); PGMSTR(Z_STR, "Z"); PGMSTR(E_STR, "E");
PGMSTR(X_LBL, "X:"); PGMSTR(Y_LBL, "Y:"); PGMSTR(Z_LBL, "Z:"); PGMSTR(E_LBL, "E:");
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 ":");
PGMSTR(SP_A_STR, " A"); PGMSTR(SP_B_STR, " B"); PGMSTR(SP_C_STR, " C");
PGMSTR(SP_P_STR, " P"); PGMSTR(SP_T_STR, " T"); PGMSTR(NUL_STR, "");
#define _N_STR(N) PGMSTR(N##_STR, STR_##N);
#define _N_LBL(N) PGMSTR(N##_LBL, STR_##N ":");
#define _SP_N_STR(N) PGMSTR(SP_##N##_STR, " " STR_##N);
#define _SP_N_LBL(N) PGMSTR(SP_##N##_LBL, " " STR_##N ":");
MAP(_N_STR, LOGICAL_AXIS_NAMES); MAP(_SP_N_STR, LOGICAL_AXIS_NAMES);
MAP(_N_LBL, LOGICAL_AXIS_NAMES); MAP(_SP_N_LBL, LOGICAL_AXIS_NAMES);
// Hook Meatpack if it's enabled on the first leaf
#if ENABLED(MEATPACK_ON_SERIAL_PORT_1)
@ -73,8 +72,8 @@ 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:"); serial_print_P(echomagic); }
void serial_error_start() { static PGMSTR(errormagic, "Error:"); serial_print_P(errormagic); }
void serial_echo_start() { serial_print(F("echo:")); }
void serial_error_start() { serial_print(F("Error:")); }
void serial_spaces(uint8_t count) { count *= (PROPORTIONAL_FONT_RATIO); while (count--) SERIAL_CHAR(' '); }
@ -102,10 +101,10 @@ void print_bin(uint16_t val) {
}
}
void print_pos(LINEAR_AXIS_ARGS(const_float_t), FSTR_P const prefix/*=nullptr*/, FSTR_P const suffix/*=nullptr*/) {
void print_pos(NUM_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)
LIST_N(DOUBLE(NUM_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, SP_U_STR, u, SP_V_STR, v, SP_W_STR, w)
);
if (suffix) serial_print(suffix); else SERIAL_EOL();
}

46
Marlin/src/core/serial.h

@ -28,19 +28,6 @@
#include "../feature/meatpack.h"
#endif
// Commonly-used strings in serial output
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[],
I_LBL[], J_LBL[], K_LBL[];
//
// Debugging flags for use by M111
//
@ -348,11 +335,40 @@ void serial_spaces(uint8_t count);
void serial_offset(const_float_t v, const uint8_t sp=0); // For v==0 draw space (sp==1) or plus (sp==2)
void print_bin(const uint16_t val);
void print_pos(LINEAR_AXIS_ARGS(const_float_t), FSTR_P const prefix=nullptr, FSTR_P const suffix=nullptr);
void print_pos(NUM_AXIS_ARGS(const_float_t), FSTR_P const prefix=nullptr, FSTR_P const suffix=nullptr);
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);
print_pos(NUM_AXIS_ELEM(xyz), prefix, suffix);
}
#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)
//
// Commonly-used strings in serial output
//
#define _N_STR(N) N##_STR
#define _N_LBL(N) N##_LBL
#define _N_STR_A(N) _N_STR(N)[]
#define _N_LBL_A(N) _N_LBL(N)[]
#define _SP_N_STR(N) SP_##N##_STR
#define _SP_N_LBL(N) SP_##N##_LBL
#define _SP_N_STR_A(N) _SP_N_STR(N)[]
#define _SP_N_LBL_A(N) _SP_N_LBL(N)[]
extern const char SP_A_STR[], SP_B_STR[], SP_C_STR[], SP_P_STR[], SP_T_STR[], NUL_STR[],
MAPLIST(_N_STR_A, LOGICAL_AXIS_NAMES), MAPLIST(_SP_N_STR_A, LOGICAL_AXIS_NAMES),
MAPLIST(_N_LBL_A, LOGICAL_AXIS_NAMES), MAPLIST(_SP_N_LBL_A, LOGICAL_AXIS_NAMES);
PGM_P const SP_AXIS_LBL[] PROGMEM = { MAPLIST(_SP_N_LBL, LOGICAL_AXIS_NAMES) };
PGM_P const SP_AXIS_STR[] PROGMEM = { MAPLIST(_SP_N_STR, LOGICAL_AXIS_NAMES) };
#undef _N_STR
#undef _N_LBL
#undef _N_STR_A
#undef _N_LBL_A
#undef _SP_N_STR
#undef _SP_N_LBL
#undef _SP_N_STR_A
#undef _SP_N_LBL_A

4
Marlin/src/core/serial_hook.h

@ -44,7 +44,9 @@ public:
}
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
constexpr SerialMask(const SerialMask &rs) : mask(rs.mask) {} // Can't use = default here since not all frameworks support this
SerialMask& operator=(const SerialMask &rs) { mask = rs.mask; return *this; }
static constexpr uint8_t All = 0xFF;
};

409
Marlin/src/core/types.h

@ -36,23 +36,41 @@ struct IF { typedef R type; };
template <class L, class R>
struct IF<true, L, R> { 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 NUM_AXIS_GANG(V...) GANG_N(NUM_AXES, V)
#define NUM_AXIS_CODE(V...) CODE_N(NUM_AXES, V)
#define NUM_AXIS_LIST(V...) LIST_N(NUM_AXES, V)
#define NUM_AXIS_LIST_1(V) LIST_N_1(NUM_AXES, V)
#define NUM_AXIS_ARRAY(V...) { NUM_AXIS_LIST(V) }
#define NUM_AXIS_ARRAY_1(V) { NUM_AXIS_LIST_1(V) }
#define NUM_AXIS_ARGS(T...) NUM_AXIS_LIST(T x, T y, T z, T i, T j, T k, T u, T v, T w)
#define NUM_AXIS_ELEM(O) NUM_AXIS_LIST(O.x, O.y, O.z, O.i, O.j, O.k, O.u, O.v, O.w)
#define NUM_AXIS_DEFS(T,V) NUM_AXIS_LIST(T x=V, T y=V, T z=V, T i=V, T j=V, T k=V, T u=V, T v=V, T w=V)
#define MAIN_AXIS_NAMES NUM_AXIS_LIST(X, Y, Z, I, J, K, U, V, W)
#define MAIN_AXIS_MAP(F) MAP(F, MAIN_AXIS_NAMES)
#define STR_AXES_MAIN NUM_AXIS_GANG("X", "Y", "Z", STR_I, STR_J, STR_K, STR_U, STR_V, STR_W)
#define LOGICAL_AXIS_GANG(E,V...) NUM_AXIS_GANG(V) GANG_ITEM_E(E)
#define LOGICAL_AXIS_CODE(E,V...) NUM_AXIS_CODE(V) CODE_ITEM_E(E)
#define LOGICAL_AXIS_LIST(E,V...) NUM_AXIS_LIST(V) LIST_ITEM_E(E)
#define LOGICAL_AXIS_LIST_1(V) NUM_AXIS_LIST_1(V) LIST_ITEM_E(V)
#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)
#define LOGICAL_AXIS_ARRAY_1(V) { LOGICAL_AXIS_LIST_1(V) }
#define LOGICAL_AXIS_ARGS(T...) LOGICAL_AXIS_LIST(T e, T x, T y, T z, T i, T j, T k, T u, T v, T w)
#define LOGICAL_AXIS_ELEM(O) LOGICAL_AXIS_LIST(O.e, O.x, O.y, O.z, O.i, O.j, O.k, O.u, O.v, O.w)
#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, T u=V, T v=V, T w=V)
#define LOGICAL_AXIS_NAMES LOGICAL_AXIS_LIST(E, X, Y, Z, I, J, K, U, V, W)
#define LOGICAL_AXIS_MAP(F) MAP(F, LOGICAL_AXIS_NAMES)
#define STR_AXES_LOGICAL LOGICAL_AXIS_GANG("E", "X", "Y", "Z", STR_I, STR_J, STR_K, STR_U, STR_V, STR_W)
#define XYZ_GANG(V...) GANG_N(PRIMARY_LINEAR_AXES, V)
#define XYZ_CODE(V...) CODE_N(PRIMARY_LINEAR_AXES, V)
#define SECONDARY_AXIS_GANG(V...) GANG_N(SECONDARY_AXES, V)
#define SECONDARY_AXIS_CODE(V...) CODE_N(SECONDARY_AXES, V)
#if HAS_ROTATIONAL_AXES
#define ROTATIONAL_AXIS_GANG(V...) GANG_N(ROTATIONAL_AXES, V)
#endif
#if HAS_EXTRUDERS
#define LIST_ITEM_E(N) , N
@ -64,7 +82,7 @@ struct IF<true, L, R> { typedef L type; };
#define GANG_ITEM_E(N)
#endif
#define AXIS_COLLISION(L) (AXIS4_NAME == L || AXIS5_NAME == L || AXIS6_NAME == L)
#define AXIS_COLLISION(L) (AXIS4_NAME == L || AXIS5_NAME == L || AXIS6_NAME == L || AXIS7_NAME == L || AXIS8_NAME == L || AXIS9_NAME == L)
// General Flags for some number of states
template<size_t N>
@ -81,9 +99,9 @@ struct Flags {
void set(const int n) { b |= (bits_t)_BV(n); }
void clear(const int n) { b &= ~(bits_t)_BV(n); }
bool test(const int n) const { return TEST(b, n); }
bool operator[](const int n) { return test(n); }
const bool operator[](const int n) { return test(n); }
const bool operator[](const int n) const { return test(n); }
const int size() const { return sizeof(b); }
int size() const { return sizeof(b); }
};
// Specialization for a single bool flag
@ -95,9 +113,9 @@ struct Flags<1> {
void set(const int) { b = true; }
void clear(const int) { b = false; }
bool test(const int) const { return b; }
bool operator[](const int) { return b; }
const bool operator[](const int) const { return b; }
const int size() const { return sizeof(b); }
bool& operator[](const int) { return b; }
bool operator[](const int) const { return b; }
int size() const { return sizeof(b); }
};
typedef Flags<8> flags_8_t;
@ -114,9 +132,9 @@ typedef struct AxisFlags {
void set(const int n, const bool onoff) { flags.set(n, onoff); }
void clear(const int n) { flags.clear(n); }
bool test(const int n) const { return flags.test(n); }
bool operator[](const int n) { return flags[n]; }
const bool operator[](const int n) const { return flags[n]; }
const int size() const { return sizeof(flags); }
bool operator[](const int n) { return flags[n]; }
bool operator[](const int n) const { return flags[n]; }
int size() const { return sizeof(flags); }
} axis_flags_t;
//
@ -129,7 +147,7 @@ typedef struct AxisFlags {
enum AxisEnum : uint8_t {
// Linear axes may be controlled directly or indirectly
LINEAR_AXIS_LIST(X_AXIS, Y_AXIS, Z_AXIS, I_AXIS, J_AXIS, K_AXIS)
NUM_AXIS_LIST(X_AXIS, Y_AXIS, Z_AXIS, I_AXIS, J_AXIS, K_AXIS, U_AXIS, V_AXIS, W_AXIS)
// Extruder axes may be considered distinctly
#define _EN_ITEM(N) , E##N##_AXIS
@ -168,7 +186,7 @@ typedef IF<(NUM_AXIS_ENUMS > 8), uint16_t, uint8_t>::type axis_bits_t;
// Loop over axes
//
#define LOOP_ABC(VAR) LOOP_S_LE_N(VAR, A_AXIS, C_AXIS)
#define LOOP_LINEAR_AXES(VAR) LOOP_S_L_N(VAR, X_AXIS, LINEAR_AXES)
#define LOOP_NUM_AXES(VAR) LOOP_S_L_N(VAR, X_AXIS, NUM_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)
#define LOOP_DISTINCT_E(VAR) LOOP_L_N(VAR, DISTINCT_E)
@ -313,10 +331,10 @@ struct XYval {
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
#if LINEAR_AXES > XY
FI void set(const T (&arr)[LINEAR_AXES]) { x = arr[0]; y = arr[1]; }
#if NUM_AXES > XY
FI void set(const T (&arr)[NUM_AXES]) { x = arr[0]; y = arr[1]; }
#endif
#if LOGICAL_AXES > LINEAR_AXES
#if LOGICAL_AXES > NUM_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]; }
@ -438,60 +456,69 @@ struct XYval {
template<typename T>
struct XYZval {
union {
struct { T LINEAR_AXIS_ARGS(); };
struct { T LINEAR_AXIS_LIST(a, b, c, u, v, w); };
T pos[LINEAR_AXES];
struct { T NUM_AXIS_ARGS(); };
struct { T NUM_AXIS_LIST(a, b, c, _i, _j, _k, _u, _v, _w); };
T pos[NUM_AXES];
};
// Set all to 0
FI void reset() { LINEAR_AXIS_GANG(x =, y =, z =, i =, j =, k =) 0; }
FI void reset() { NUM_AXIS_GANG(x =, y =, z =, i =, j =, k =, u =, v =, w =) 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 XYval<T> pxy) { x = pxy.x; y = pxy.y; }
FI void set(const XYval<T> pxy, const T pz) { LINEAR_AXIS_CODE(x = pxy.x, y = pxy.y, z = pz, NOOP, NOOP, NOOP); }
FI void set(const XYval<T> pxy, const T pz) { NUM_AXIS_CODE(x = pxy.x, y = pxy.y, z = pz, NOOP, NOOP, NOOP, NOOP, NOOP, NOOP); }
FI void set(const T (&arr)[XY]) { x = arr[0]; y = arr[1]; }
#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 ); }
FI void set(const T (&arr)[NUM_AXES]) { NUM_AXIS_CODE(x = arr[0], y = arr[1], z = arr[2], i = arr[3], j = arr[4], k = arr[5], u = arr[6], v = arr[7], w = arr[8]); }
FI void set(NUM_AXIS_ARGS(const T)) { NUM_AXIS_CODE(a = x, b = y, c = z, _i = i, _j = j, _k = k, _u = u, _v = v, _w = w ); }
#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 LOGICAL_AXES > NUM_AXES
FI void set(const T (&arr)[LOGICAL_AXES]) { NUM_AXIS_CODE(x = arr[0], y = arr[1], z = arr[2], i = arr[3], j = arr[4], k = arr[5], u = arr[6], v = arr[7], w = arr[8]); }
FI void set(LOGICAL_AXIS_ARGS(const T)) { NUM_AXIS_CODE(a = x, b = y, c = z, _i = i, _j = j, _k = k, _u = u, _v = v, _w = w ); }
#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]); }
FI void set(const T (&arr)[DISTINCT_AXES]) { NUM_AXIS_CODE(x = arr[0], y = arr[1], z = arr[2], i = arr[3], j = arr[4], k = arr[5], u = arr[6], v = arr[7], w = arr[8]); }
#endif
#endif
#if HAS_I_AXIS
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) { 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; }
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
#if HAS_U_AXIS
FI void set(const T px, const T py, const T pz, const T pi, const T pj, const T pk) { x = px; y = py; z = pz; i = pi; j = pj; k = pk; }
#endif
#if HAS_V_AXIS
FI void set(const T px, const T py, const T pz, const T pi, const T pj, const T pk, const T pm) { x = px; y = py; z = pz; i = pi; j = pj; k = pk; u = pu; }
#endif
#if HAS_W_AXIS
FI void set(const T px, const T py, const T pz, const T pi, const T pj, const T pk, const T pm, const T po) { x = px; y = py; z = pz; i = pi; j = pj; k = pk; u = pu; v = pv; }
#endif
// 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)); }
FI T magnitude() const { return (T)sqrtf(NUM_AXIS_GANG(x*x, + y*y, + z*z, + i*i, + j*j, + k*k, + u*u, + v*v, + w*w)); }
// 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 LINEAR_AXIS_GANG(x, || y, || z, || i, || j, || k); }
FI operator bool() { return NUM_AXIS_GANG(x, || y, || z, || i, || j, || k, || u, || v, || w); }
// Explicit copy and copies with conversion
FI XYZval<T> copy() const { XYZval<T> o = *this; return o; }
FI XYZval<T> 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<int16_t> 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<int16_t> 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<int32_t> 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<int32_t> 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<int32_t> 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<int32_t> 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<float> asFloat() { return LINEAR_AXIS_ARRAY(static_cast<float>(x), static_cast<float>(y), static_cast<float>(z), static_cast<float>(i), static_cast<float>(j), static_cast<float>(k)); }
FI XYZval<float> asFloat() const { return LINEAR_AXIS_ARRAY(static_cast<float>(x), static_cast<float>(y), static_cast<float>(z), static_cast<float>(i), static_cast<float>(j), static_cast<float>(k)); }
FI XYZval<float> reciprocal() const { return LINEAR_AXIS_ARRAY(_RECIP(x), _RECIP(y), _RECIP(z), _RECIP(i), _RECIP(j), _RECIP(k)); }
FI XYZval<T> ABS() const { return NUM_AXIS_ARRAY(T(_ABS(x)), T(_ABS(y)), T(_ABS(z)), T(_ABS(i)), T(_ABS(j)), T(_ABS(k)), T(_ABS(u)), T(_ABS(v)), T(_ABS(w))); }
FI XYZval<int16_t> asInt() { return NUM_AXIS_ARRAY(int16_t(x), int16_t(y), int16_t(z), int16_t(i), int16_t(j), int16_t(k), int16_t(u), int16_t(v), int16_t(w)); }
FI XYZval<int16_t> asInt() const { return NUM_AXIS_ARRAY(int16_t(x), int16_t(y), int16_t(z), int16_t(i), int16_t(j), int16_t(k), int16_t(u), int16_t(v), int16_t(w)); }
FI XYZval<int32_t> asLong() { return NUM_AXIS_ARRAY(int32_t(x), int32_t(y), int32_t(z), int32_t(i), int32_t(j), int32_t(k), int32_t(u), int32_t(v), int32_t(w)); }
FI XYZval<int32_t> asLong() const { return NUM_AXIS_ARRAY(int32_t(x), int32_t(y), int32_t(z), int32_t(i), int32_t(j), int32_t(k), int32_t(u), int32_t(v), int32_t(w)); }
FI XYZval<int32_t> ROUNDL() { return NUM_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)), int32_t(LROUND(u)), int32_t(LROUND(v)), int32_t(LROUND(w))); }
FI XYZval<int32_t> ROUNDL() const { return NUM_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)), int32_t(LROUND(u)), int32_t(LROUND(v)), int32_t(LROUND(w))); }
FI XYZval<float> asFloat() { return NUM_AXIS_ARRAY(static_cast<float>(x), static_cast<float>(y), static_cast<float>(z), static_cast<float>(i), static_cast<float>(j), static_cast<float>(k), static_cast<float>(u), static_cast<float>(v), static_cast<float>(w)); }
FI XYZval<float> asFloat() const { return NUM_AXIS_ARRAY(static_cast<float>(x), static_cast<float>(y), static_cast<float>(z), static_cast<float>(i), static_cast<float>(j), static_cast<float>(k), static_cast<float>(u), static_cast<float>(v), static_cast<float>(w)); }
FI XYZval<float> reciprocal() const { return NUM_AXIS_ARRAY(_RECIP(x), _RECIP(y), _RECIP(z), _RECIP(i), _RECIP(j), _RECIP(k), _RECIP(u), _RECIP(v), _RECIP(w)); }
// Marlin workspace shifting is done with G92 and M206
FI XYZval<float> asLogical() const { XYZval<float> o = asFloat(); toLogical(o); return o; }
@ -502,78 +529,78 @@ struct XYZval {
FI operator const XYval<T>&() const { return *(const XYval<T>*)this; }
// Cast to a type with more fields by making a new object
FI operator XYZEval<T>() const { return LINEAR_AXIS_ARRAY(x, y, z, i, j, k); }
FI operator XYZEval<T>() const { return NUM_AXIS_ARRAY(x, y, z, i, j, k, u, v, w); }
// 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<T>& operator= (const T v) { set(ARRAY_N_1(LINEAR_AXES, v)); return *this; }
FI XYZval<T>& operator= (const T v) { set(ARRAY_N_1(NUM_AXES, v)); return *this; }
FI XYZval<T>& operator= (const XYval<T> &rs) { set(rs.x, rs.y ); return *this; }
FI XYZval<T>& operator= (const XYZEval<T> &rs) { set(LINEAR_AXIS_ELEM(rs)); return *this; }
FI XYZval<T>& operator= (const XYZEval<T> &rs) { set(NUM_AXIS_ELEM(rs)); return *this; }
// Override other operators to get intuitive behaviors
FI XYZval<T> operator+ (const XYval<T> &rs) const { XYZval<T> ls = *this; LINEAR_AXIS_CODE(ls.x += rs.x, ls.y += rs.y, NOOP , NOOP , NOOP , NOOP ); return ls; }
FI XYZval<T> operator+ (const XYval<T> &rs) { XYZval<T> ls = *this; LINEAR_AXIS_CODE(ls.x += rs.x, ls.y += rs.y, NOOP , NOOP , NOOP , NOOP ); return ls; }
FI XYZval<T> operator- (const XYval<T> &rs) const { XYZval<T> ls = *this; LINEAR_AXIS_CODE(ls.x -= rs.x, ls.y -= rs.y, NOOP , NOOP , NOOP , NOOP ); return ls; }
FI XYZval<T> operator- (const XYval<T> &rs) { XYZval<T> ls = *this; LINEAR_AXIS_CODE(ls.x -= rs.x, ls.y -= rs.y, NOOP , NOOP , NOOP , NOOP ); return ls; }
FI XYZval<T> operator* (const XYval<T> &rs) const { XYZval<T> ls = *this; LINEAR_AXIS_CODE(ls.x *= rs.x, ls.y *= rs.y, NOOP , NOOP , NOOP , NOOP ); return ls; }
FI XYZval<T> operator* (const XYval<T> &rs) { XYZval<T> ls = *this; LINEAR_AXIS_CODE(ls.x *= rs.x, ls.y *= rs.y, NOOP , NOOP , NOOP , NOOP ); return ls; }
FI XYZval<T> operator/ (const XYval<T> &rs) const { XYZval<T> ls = *this; LINEAR_AXIS_CODE(ls.x /= rs.x, ls.y /= rs.y, NOOP , NOOP , NOOP , NOOP ); return ls; }
FI XYZval<T> operator/ (const XYval<T> &rs) { XYZval<T> ls = *this; LINEAR_AXIS_CODE(ls.x /= rs.x, ls.y /= rs.y, NOOP , NOOP , NOOP , NOOP ); return ls; }
FI XYZval<T> operator+ (const XYZval<T> &rs) const { XYZval<T> 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<T> operator+ (const XYZval<T> &rs) { XYZval<T> 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<T> operator- (const XYZval<T> &rs) const { XYZval<T> 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<T> operator- (const XYZval<T> &rs) { XYZval<T> 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<T> operator* (const XYZval<T> &rs) const { XYZval<T> 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<T> operator* (const XYZval<T> &rs) { XYZval<T> 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<T> operator/ (const XYZval<T> &rs) const { XYZval<T> 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<T> operator/ (const XYZval<T> &rs) { XYZval<T> 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<T> operator+ (const XYZEval<T> &rs) const { XYZval<T> 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<T> operator+ (const XYZEval<T> &rs) { XYZval<T> 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<T> operator- (const XYZEval<T> &rs) const { XYZval<T> 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<T> operator- (const XYZEval<T> &rs) { XYZval<T> 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<T> operator* (const XYZEval<T> &rs) const { XYZval<T> 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<T> operator* (const XYZEval<T> &rs) { XYZval<T> 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<T> operator/ (const XYZEval<T> &rs) const { XYZval<T> 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<T> operator/ (const XYZEval<T> &rs) { XYZval<T> 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<T> operator* (const float &v) const { XYZval<T> 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<T> operator* (const float &v) { XYZval<T> 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<T> operator* (const int &v) const { XYZval<T> 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<T> operator* (const int &v) { XYZval<T> 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<T> operator/ (const float &v) const { XYZval<T> 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<T> operator/ (const float &v) { XYZval<T> 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<T> operator/ (const int &v) const { XYZval<T> 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<T> operator/ (const int &v) { XYZval<T> 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<T> operator>>(const int &v) const { XYZval<T> 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<T> operator>>(const int &v) { XYZval<T> 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<T> operator<<(const int &v) const { XYZval<T> 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<T> operator<<(const int &v) { XYZval<T> 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<T> operator-() const { XYZval<T> 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<T> operator-() { XYZval<T> 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<T> operator+ (const XYval<T> &rs) const { XYZval<T> ls = *this; NUM_AXIS_CODE(ls.x += rs.x, ls.y += rs.y, NOOP , NOOP , NOOP , NOOP , NOOP , NOOP , NOOP ); return ls; }
FI XYZval<T> operator+ (const XYval<T> &rs) { XYZval<T> ls = *this; NUM_AXIS_CODE(ls.x += rs.x, ls.y += rs.y, NOOP , NOOP , NOOP , NOOP , NOOP , NOOP , NOOP ); return ls; }
FI XYZval<T> operator- (const XYval<T> &rs) const { XYZval<T> ls = *this; NUM_AXIS_CODE(ls.x -= rs.x, ls.y -= rs.y, NOOP , NOOP , NOOP , NOOP , NOOP , NOOP , NOOP ); return ls; }
FI XYZval<T> operator- (const XYval<T> &rs) { XYZval<T> ls = *this; NUM_AXIS_CODE(ls.x -= rs.x, ls.y -= rs.y, NOOP , NOOP , NOOP , NOOP , NOOP , NOOP , NOOP ); return ls; }
FI XYZval<T> operator* (const XYval<T> &rs) const { XYZval<T> ls = *this; NUM_AXIS_CODE(ls.x *= rs.x, ls.y *= rs.y, NOOP , NOOP , NOOP , NOOP , NOOP , NOOP , NOOP ); return ls; }
FI XYZval<T> operator* (const XYval<T> &rs) { XYZval<T> ls = *this; NUM_AXIS_CODE(ls.x *= rs.x, ls.y *= rs.y, NOOP , NOOP , NOOP , NOOP , NOOP , NOOP , NOOP ); return ls; }
FI XYZval<T> operator/ (const XYval<T> &rs) const { XYZval<T> ls = *this; NUM_AXIS_CODE(ls.x /= rs.x, ls.y /= rs.y, NOOP , NOOP , NOOP , NOOP , NOOP , NOOP , NOOP ); return ls; }
FI XYZval<T> operator/ (const XYval<T> &rs) { XYZval<T> ls = *this; NUM_AXIS_CODE(ls.x /= rs.x, ls.y /= rs.y, NOOP , NOOP , NOOP , NOOP , NOOP , NOOP , NOOP ); return ls; }
FI XYZval<T> operator+ (const XYZval<T> &rs) const { XYZval<T> ls = *this; NUM_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, ls.u += rs.u, ls.v += rs.v, ls.w += rs.w); return ls; }
FI XYZval<T> operator+ (const XYZval<T> &rs) { XYZval<T> ls = *this; NUM_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, ls.u += rs.u, ls.v += rs.v, ls.w += rs.w); return ls; }
FI XYZval<T> operator- (const XYZval<T> &rs) const { XYZval<T> ls = *this; NUM_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, ls.u -= rs.u, ls.v -= rs.v, ls.w -= rs.w); return ls; }
FI XYZval<T> operator- (const XYZval<T> &rs) { XYZval<T> ls = *this; NUM_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, ls.u -= rs.u, ls.v -= rs.v, ls.w -= rs.w); return ls; }
FI XYZval<T> operator* (const XYZval<T> &rs) const { XYZval<T> ls = *this; NUM_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, ls.u *= rs.u, ls.v *= rs.v, ls.w *= rs.w); return ls; }
FI XYZval<T> operator* (const XYZval<T> &rs) { XYZval<T> ls = *this; NUM_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, ls.u *= rs.u, ls.v *= rs.v, ls.w *= rs.w); return ls; }
FI XYZval<T> operator/ (const XYZval<T> &rs) const { XYZval<T> ls = *this; NUM_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, ls.u /= rs.u, ls.v /= rs.v, ls.w /= rs.w); return ls; }
FI XYZval<T> operator/ (const XYZval<T> &rs) { XYZval<T> ls = *this; NUM_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, ls.u /= rs.u, ls.v /= rs.v, ls.w /= rs.w); return ls; }
FI XYZval<T> operator+ (const XYZEval<T> &rs) const { XYZval<T> ls = *this; NUM_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, ls.u += rs.u, ls.v += rs.v, ls.w += rs.w); return ls; }
FI XYZval<T> operator+ (const XYZEval<T> &rs) { XYZval<T> ls = *this; NUM_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, ls.u += rs.u, ls.v += rs.v, ls.w += rs.w); return ls; }
FI XYZval<T> operator- (const XYZEval<T> &rs) const { XYZval<T> ls = *this; NUM_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, ls.u -= rs.u, ls.v -= rs.v, ls.w -= rs.w); return ls; }
FI XYZval<T> operator- (const XYZEval<T> &rs) { XYZval<T> ls = *this; NUM_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, ls.u -= rs.u, ls.v -= rs.v, ls.w -= rs.w); return ls; }
FI XYZval<T> operator* (const XYZEval<T> &rs) const { XYZval<T> ls = *this; NUM_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, ls.u *= rs.u, ls.v *= rs.v, ls.w *= rs.w); return ls; }
FI XYZval<T> operator* (const XYZEval<T> &rs) { XYZval<T> ls = *this; NUM_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, ls.u *= rs.u, ls.v *= rs.v, ls.w *= rs.w); return ls; }
FI XYZval<T> operator/ (const XYZEval<T> &rs) const { XYZval<T> ls = *this; NUM_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, ls.u /= rs.u, ls.v /= rs.v, ls.w /= rs.w); return ls; }
FI XYZval<T> operator/ (const XYZEval<T> &rs) { XYZval<T> ls = *this; NUM_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, ls.u /= rs.u, ls.v /= rs.v, ls.w /= rs.w); return ls; }
FI XYZval<T> operator* (const float &v) const { XYZval<T> ls = *this; NUM_AXIS_CODE(ls.x *= v, ls.y *= v, ls.z *= v, ls.i *= v, ls.j *= v, ls.k *= v, ls.u *= v, ls.v *= v, ls.w *= v ); return ls; }
FI XYZval<T> operator* (const float &v) { XYZval<T> ls = *this; NUM_AXIS_CODE(ls.x *= v, ls.y *= v, ls.z *= v, ls.i *= v, ls.j *= v, ls.k *= v, ls.u *= v, ls.v *= v, ls.w *= v ); return ls; }
FI XYZval<T> operator* (const int &v) const { XYZval<T> ls = *this; NUM_AXIS_CODE(ls.x *= v, ls.y *= v, ls.z *= v, ls.i *= v, ls.j *= v, ls.k *= v, ls.u *= v, ls.v *= v, ls.w *= v ); return ls; }
FI XYZval<T> operator* (const int &v) { XYZval<T> ls = *this; NUM_AXIS_CODE(ls.x *= v, ls.y *= v, ls.z *= v, ls.i *= v, ls.j *= v, ls.k *= v, ls.u *= v, ls.v *= v, ls.w *= v ); return ls; }
FI XYZval<T> operator/ (const float &v) const { XYZval<T> ls = *this; NUM_AXIS_CODE(ls.x /= v, ls.y /= v, ls.z /= v, ls.i /= v, ls.j /= v, ls.k /= v, ls.u /= v, ls.v /= v, ls.w /= v ); return ls; }
FI XYZval<T> operator/ (const float &v) { XYZval<T> ls = *this; NUM_AXIS_CODE(ls.x /= v, ls.y /= v, ls.z /= v, ls.i /= v, ls.j /= v, ls.k /= v, ls.u /= v, ls.v /= v, ls.w /= v ); return ls; }
FI XYZval<T> operator/ (const int &v) const { XYZval<T> ls = *this; NUM_AXIS_CODE(ls.x /= v, ls.y /= v, ls.z /= v, ls.i /= v, ls.j /= v, ls.k /= v, ls.u /= v, ls.v /= v, ls.w /= v ); return ls; }
FI XYZval<T> operator/ (const int &v) { XYZval<T> ls = *this; NUM_AXIS_CODE(ls.x /= v, ls.y /= v, ls.z /= v, ls.i /= v, ls.j /= v, ls.k /= v, ls.u /= v, ls.v /= v, ls.w /= v ); return ls; }
FI XYZval<T> operator>>(const int &v) const { XYZval<T> ls = *this; NUM_AXIS_CODE(_RS(ls.x), _RS(ls.y), _RS(ls.z), _RS(ls.i), _RS(ls.j), _RS(ls.k), _RS(ls.u), _RS(ls.v), _RS(ls.w) ); return ls; }
FI XYZval<T> operator>>(const int &v) { XYZval<T> ls = *this; NUM_AXIS_CODE(_RS(ls.x), _RS(ls.y), _RS(ls.z), _RS(ls.i), _RS(ls.j), _RS(ls.k), _RS(ls.u), _RS(ls.v), _RS(ls.w) ); return ls; }
FI XYZval<T> operator<<(const int &v) const { XYZval<T> ls = *this; NUM_AXIS_CODE(_LS(ls.x), _LS(ls.y), _LS(ls.z), _LS(ls.i), _LS(ls.j), _LS(ls.k), _LS(ls.u), _LS(ls.v), _LS(ls.w) ); return ls; }
FI XYZval<T> operator<<(const int &v) { XYZval<T> ls = *this; NUM_AXIS_CODE(_LS(ls.x), _LS(ls.y), _LS(ls.z), _LS(ls.i), _LS(ls.j), _LS(ls.k), _LS(ls.u), _LS(ls.v), _LS(ls.w) ); return ls; }
FI const XYZval<T> operator-() const { XYZval<T> o = *this; NUM_AXIS_CODE(o.x = -x, o.y = -y, o.z = -z, o.i = -i, o.j = -j, o.k = -k, o.u = -u, o.v = -v, o.w = -w); return o; }
FI XYZval<T> operator-() { XYZval<T> o = *this; NUM_AXIS_CODE(o.x = -x, o.y = -y, o.z = -z, o.i = -i, o.j = -j, o.k = -k, o.u = -u, o.v = -v, o.w = -w); return o; }
// Modifier operators
FI XYZval<T>& operator+=(const XYval<T> &rs) { LINEAR_AXIS_CODE(x += rs.x, y += rs.y, NOOP, NOOP, NOOP, NOOP ); return *this; }
FI XYZval<T>& operator-=(const XYval<T> &rs) { LINEAR_AXIS_CODE(x -= rs.x, y -= rs.y, NOOP, NOOP, NOOP, NOOP ); return *this; }
FI XYZval<T>& operator*=(const XYval<T> &rs) { LINEAR_AXIS_CODE(x *= rs.x, y *= rs.y, NOOP, NOOP, NOOP, NOOP ); return *this; }
FI XYZval<T>& operator/=(const XYval<T> &rs) { LINEAR_AXIS_CODE(x /= rs.x, y /= rs.y, NOOP, NOOP, NOOP, NOOP ); return *this; }
FI XYZval<T>& operator+=(const XYZval<T> &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<T>& operator-=(const XYZval<T> &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<T>& operator*=(const XYZval<T> &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<T>& operator/=(const XYZval<T> &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<T>& operator+=(const XYZEval<T> &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<T>& operator-=(const XYZEval<T> &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<T>& operator*=(const XYZEval<T> &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<T>& operator/=(const XYZEval<T> &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<T>& operator*=(const float &v) { LINEAR_AXIS_CODE(x *= v, y *= v, z *= v, i *= v, j *= v, k *= v); return *this; }
FI XYZval<T>& operator*=(const int &v) { LINEAR_AXIS_CODE(x *= v, y *= v, z *= v, i *= v, j *= v, k *= v); return *this; }
FI XYZval<T>& operator>>=(const int &v) { LINEAR_AXIS_CODE(_RS(x), _RS(y), _RS(z), _RS(i), _RS(j), _RS(k)); return *this; }
FI XYZval<T>& operator<<=(const int &v) { LINEAR_AXIS_CODE(_LS(x), _LS(y), _LS(z), _LS(i), _LS(j), _LS(k)); return *this; }
FI XYZval<T>& operator+=(const XYval<T> &rs) { NUM_AXIS_CODE(x += rs.x, y += rs.y, NOOP, NOOP, NOOP, NOOP, NOOP, NOOP, NOOP ); return *this; }
FI XYZval<T>& operator-=(const XYval<T> &rs) { NUM_AXIS_CODE(x -= rs.x, y -= rs.y, NOOP, NOOP, NOOP, NOOP, NOOP, NOOP, NOOP ); return *this; }
FI XYZval<T>& operator*=(const XYval<T> &rs) { NUM_AXIS_CODE(x *= rs.x, y *= rs.y, NOOP, NOOP, NOOP, NOOP, NOOP, NOOP, NOOP ); return *this; }
FI XYZval<T>& operator/=(const XYval<T> &rs) { NUM_AXIS_CODE(x /= rs.x, y /= rs.y, NOOP, NOOP, NOOP, NOOP, NOOP, NOOP, NOOP ); return *this; }
FI XYZval<T>& operator+=(const XYZval<T> &rs) { NUM_AXIS_CODE(x += rs.x, y += rs.y, z += rs.z, i += rs.i, j += rs.j, k += rs.k, u += rs.u, v += rs.v, w += rs.w); return *this; }
FI XYZval<T>& operator-=(const XYZval<T> &rs) { NUM_AXIS_CODE(x -= rs.x, y -= rs.y, z -= rs.z, i -= rs.i, j -= rs.j, k -= rs.k, u -= rs.u, v -= rs.v, w -= rs.w); return *this; }
FI XYZval<T>& operator*=(const XYZval<T> &rs) { NUM_AXIS_CODE(x *= rs.x, y *= rs.y, z *= rs.z, i *= rs.i, j *= rs.j, k *= rs.k, u *= rs.u, v *= rs.v, w *= rs.w); return *this; }
FI XYZval<T>& operator/=(const XYZval<T> &rs) { NUM_AXIS_CODE(x /= rs.x, y /= rs.y, z /= rs.z, i /= rs.i, j /= rs.j, k /= rs.k, u /= rs.u, v /= rs.v, w /= rs.w); return *this; }
FI XYZval<T>& operator+=(const XYZEval<T> &rs) { NUM_AXIS_CODE(x += rs.x, y += rs.y, z += rs.z, i += rs.i, j += rs.j, k += rs.k, u += rs.u, v += rs.v, w += rs.w); return *this; }
FI XYZval<T>& operator-=(const XYZEval<T> &rs) { NUM_AXIS_CODE(x -= rs.x, y -= rs.y, z -= rs.z, i -= rs.i, j -= rs.j, k -= rs.k, u -= rs.u, v -= rs.v, w -= rs.w); return *this; }
FI XYZval<T>& operator*=(const XYZEval<T> &rs) { NUM_AXIS_CODE(x *= rs.x, y *= rs.y, z *= rs.z, i *= rs.i, j *= rs.j, k *= rs.k, u *= rs.u, v *= rs.v, w *= rs.w); return *this; }
FI XYZval<T>& operator/=(const XYZEval<T> &rs) { NUM_AXIS_CODE(x /= rs.x, y /= rs.y, z /= rs.z, i /= rs.i, j /= rs.j, k /= rs.k, u /= rs.u, v /= rs.v, w /= rs.w); return *this; }
FI XYZval<T>& operator*=(const float &v) { NUM_AXIS_CODE(x *= v, y *= v, z *= v, i *= v, j *= v, k *= v, u *= v, v *= v, w *= v); return *this; }
FI XYZval<T>& operator*=(const int &v) { NUM_AXIS_CODE(x *= v, y *= v, z *= v, i *= v, j *= v, k *= v, u *= v, v *= v, w *= v); return *this; }
FI XYZval<T>& operator>>=(const int &v) { NUM_AXIS_CODE(_RS(x), _RS(y), _RS(z), _RS(i), _RS(j), _RS(k), _RS(u), _RS(v), _RS(w)); return *this; }
FI XYZval<T>& operator<<=(const int &v) { NUM_AXIS_CODE(_LS(x), _LS(y), _LS(z), _LS(i), _LS(j), _LS(k), _LS(u), _LS(v), _LS(w)); return *this; }
// Exact comparisons. For floats a "NEAR" operation may be better.
FI bool operator==(const XYZEval<T> &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<T> &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<T> &rs) { return true NUM_AXIS_GANG(&& x == rs.x, && y == rs.y, && z == rs.z, && i == rs.i, && j == rs.j, && k == rs.k, && u == rs.u, && v == rs.v, && w == rs.w); }
FI bool operator==(const XYZEval<T> &rs) const { return true NUM_AXIS_GANG(&& x == rs.x, && y == rs.y, && z == rs.z, && i == rs.i, && j == rs.j, && k == rs.k, && u == rs.u, && v == rs.v, && w == rs.w); }
FI bool operator!=(const XYZEval<T> &rs) { return !operator==(rs); }
FI bool operator!=(const XYZEval<T> &rs) const { return !operator==(rs); }
};
@ -585,56 +612,66 @@ template<typename T>
struct XYZEval {
union {
struct { T LOGICAL_AXIS_ARGS(); };
struct { T LOGICAL_AXIS_LIST(_e, a, b, c, u, v, w); };
struct { T LOGICAL_AXIS_LIST(_e, a, b, c, _i, _j, _k, _u, _v, _w); };
T pos[LOGICAL_AXES];
};
// Reset all to 0
FI void reset() { LOGICAL_AXIS_GANG(e =, x =, y =, z =, i =, j =, k =) 0; }
FI void reset() { LOGICAL_AXIS_GANG(e =, x =, y =, z =, i =, j =, k =, u =, v =, w =) 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; }
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; }
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; }
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; }
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
#if HAS_U_AXIS
FI void set(const T px, const T py, const T pz, const T pi, const T pj, const T pk) { x = px; y = py; z = pz; i = pi; j = pj; k = pk; }
#endif
#if HAS_V_AXIS
FI void set(const T px, const T py, const T pz, const T pi, const T pj, const T pk, const T pm) { x = px; y = py; z = pz; i = pi; j = pj; k = pk; u = pu; }
#endif
#if HAS_W_AXIS
FI void set(const T px, const T py, const T pz, const T pi, const T pj, const T pk, const T pm, const T po) { x = px; y = py; z = pz; i = pi; j = pj; k = pk; u = pm; v = pv; }
#endif
// Setters taking struct types and arrays
FI void set(const XYval<T> pxy) { x = pxy.x; y = pxy.y; }
FI void set(const XYZval<T> pxyz) { set(LINEAR_AXIS_ELEM(pxyz)); }
FI void set(const XYval<T> pxy) { x = pxy.x; y = pxy.y; }
FI void set(const XYZval<T> pxyz) { set(NUM_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); }
FI void set(NUM_AXIS_ARGS(const T)) { NUM_AXIS_CODE(a = x, b = y, c = z, _i = i, _j = j, _k = k, _u = u, _v = v, _w = w); }
#endif
FI void set(const XYval<T> pxy, const T pz) { set(pxy); TERN_(HAS_Z_AXIS, z = pz); }
#if LOGICAL_AXES > LINEAR_AXES
FI void set(const XYval<T> pxy, const T pz) { set(pxy); TERN_(HAS_Z_AXIS, z = pz); }
#if LOGICAL_AXES > NUM_AXES
FI void set(const XYval<T> pxy, const T pz, const T pe) { set(pxy, pz); e = pe; }
FI void set(const XYZval<T> 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); }
FI void set(const XYZval<T> 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, _i = i, _j = j, _k = k, _u = u, _v = v, _w = w); }
#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)); }
FI T magnitude() const { return (T)sqrtf(LOGICAL_AXIS_GANG(+ e*e, + x*x, + y*y, + z*z, + i*i, + j*j, + k*k, + u*u, + v*v, + w*w)); }
// 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); }
FI operator bool() { return 0 LOGICAL_AXIS_GANG(|| e, || x, || y, || z, || i, || j, || k, || u, || v, || w); }
// Explicit copy and copies with conversion
FI XYZEval<T> copy() const { XYZEval<T> o = *this; return o; }
FI XYZEval<T> 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<int16_t> 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<int16_t> 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<int32_t> 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<int32_t> 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<int32_t> 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<int32_t> 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<float> asFloat() { return LOGICAL_AXIS_ARRAY(static_cast<float>(e), static_cast<float>(x), static_cast<float>(y), static_cast<float>(z), static_cast<float>(i), static_cast<float>(j), static_cast<float>(k)); }
FI XYZEval<float> asFloat() const { return LOGICAL_AXIS_ARRAY(static_cast<float>(e), static_cast<float>(x), static_cast<float>(y), static_cast<float>(z), static_cast<float>(i), static_cast<float>(j), static_cast<float>(k)); }
FI XYZEval<float> reciprocal() const { return LOGICAL_AXIS_ARRAY(_RECIP(e), _RECIP(x), _RECIP(y), _RECIP(z), _RECIP(i), _RECIP(j), _RECIP(k)); }
FI XYZEval<T> copy() const { XYZEval<T> v = *this; return v; }
FI XYZEval<T> 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)), T(_ABS(u)), T(_ABS(v)), T(_ABS(w))); }
FI XYZEval<int16_t> 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), int16_t(u), int16_t(v), int16_t(w)); }
FI XYZEval<int16_t> 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), int16_t(u), int16_t(v), int16_t(w)); }
FI XYZEval<int32_t> 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), int32_t(u), int32_t(v), int32_t(w)); }
FI XYZEval<int32_t> 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), int32_t(u), int32_t(v), int32_t(w)); }
FI XYZEval<int32_t> 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)), int32_t(LROUND(u)), int32_t(LROUND(v)), int32_t(LROUND(w))); }
FI XYZEval<int32_t> 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)), int32_t(LROUND(u)), int32_t(LROUND(v)), int32_t(LROUND(w))); }
FI XYZEval<float> asFloat() { return LOGICAL_AXIS_ARRAY(static_cast<float>(e), static_cast<float>(x), static_cast<float>(y), static_cast<float>(z), static_cast<float>(i), static_cast<float>(j), static_cast<float>(k), static_cast<float>(u), static_cast<float>(v), static_cast<float>(w)); }
FI XYZEval<float> asFloat() const { return LOGICAL_AXIS_ARRAY(static_cast<float>(e), static_cast<float>(x), static_cast<float>(y), static_cast<float>(z), static_cast<float>(i), static_cast<float>(j), static_cast<float>(k), static_cast<float>(u), static_cast<float>(v), static_cast<float>(w)); }
FI XYZEval<float> reciprocal() const { return LOGICAL_AXIS_ARRAY(_RECIP(e), _RECIP(x), _RECIP(y), _RECIP(z), _RECIP(i), _RECIP(j), _RECIP(k), _RECIP(u), _RECIP(v), _RECIP(w)); }
// Marlin workspace shifting is done with G92 and M206
FI XYZEval<float> asLogical() const { XYZEval<float> o = asFloat(); toLogical(o); return o; }
@ -651,9 +688,9 @@ struct XYZEval {
FI const T& operator[](const int n) const { return pos[n]; }
// Assignment operator overrides do the expected thing
FI XYZEval<T>& operator= (const T v) { set(LIST_N_1(LINEAR_AXES, v)); return *this; }
FI XYZEval<T>& operator= (const T v) { set(LIST_N_1(NUM_AXES, v)); return *this; }
FI XYZEval<T>& operator= (const XYval<T> &rs) { set(rs.x, rs.y); return *this; }
FI XYZEval<T>& operator= (const XYZval<T> &rs) { set(LINEAR_AXIS_ELEM(rs)); return *this; }
FI XYZEval<T>& operator= (const XYZval<T> &rs) { set(NUM_AXIS_ELEM(rs)); return *this; }
// Override other operators to get intuitive behaviors
FI XYZEval<T> operator+ (const XYval<T> &rs) const { XYZEval<T> ls = *this; ls.x += rs.x; ls.y += rs.y; return ls; }
@ -664,57 +701,57 @@ struct XYZEval {
FI XYZEval<T> operator* (const XYval<T> &rs) { XYZEval<T> ls = *this; ls.x *= rs.x; ls.y *= rs.y; return ls; }
FI XYZEval<T> operator/ (const XYval<T> &rs) const { XYZEval<T> ls = *this; ls.x /= rs.x; ls.y /= rs.y; return ls; }
FI XYZEval<T> operator/ (const XYval<T> &rs) { XYZEval<T> ls = *this; ls.x /= rs.x; ls.y /= rs.y; return ls; }
FI XYZEval<T> operator+ (const XYZval<T> &rs) const { XYZval<T> 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<T> operator+ (const XYZval<T> &rs) { XYZval<T> 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<T> operator- (const XYZval<T> &rs) const { XYZval<T> 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<T> operator- (const XYZval<T> &rs) { XYZval<T> 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<T> operator* (const XYZval<T> &rs) const { XYZval<T> 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<T> operator* (const XYZval<T> &rs) { XYZval<T> 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<T> operator/ (const XYZval<T> &rs) const { XYZval<T> 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<T> operator/ (const XYZval<T> &rs) { XYZval<T> 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<T> operator+ (const XYZEval<T> &rs) const { XYZEval<T> 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<T> operator+ (const XYZEval<T> &rs) { XYZEval<T> 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<T> operator- (const XYZEval<T> &rs) const { XYZEval<T> 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<T> operator- (const XYZEval<T> &rs) { XYZEval<T> 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<T> operator* (const XYZEval<T> &rs) const { XYZEval<T> 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<T> operator* (const XYZEval<T> &rs) { XYZEval<T> 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<T> operator/ (const XYZEval<T> &rs) const { XYZEval<T> 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<T> operator/ (const XYZEval<T> &rs) { XYZEval<T> 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<T> operator* (const float &v) const { XYZEval<T> 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<T> operator* (const float &v) { XYZEval<T> 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<T> operator* (const int &v) const { XYZEval<T> 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<T> operator* (const int &v) { XYZEval<T> 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<T> operator/ (const float &v) const { XYZEval<T> 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<T> operator/ (const float &v) { XYZEval<T> 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<T> operator/ (const int &v) const { XYZEval<T> 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<T> operator/ (const int &v) { XYZEval<T> 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<T> operator>>(const int &v) const { XYZEval<T> 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<T> operator>>(const int &v) { XYZEval<T> 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<T> operator<<(const int &v) const { XYZEval<T> 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<T> operator<<(const int &v) { XYZEval<T> 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<T> operator-() const { return LOGICAL_AXIS_ARRAY(-e, -x, -y, -z, -i, -j, -k); }
FI XYZEval<T> operator-() { return LOGICAL_AXIS_ARRAY(-e, -x, -y, -z, -i, -j, -k); }
FI XYZEval<T> operator+ (const XYZval<T> &rs) const { XYZval<T> ls = *this; NUM_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, ls.u += rs.u, ls.v += rs.v, ls.w += rs.w); return ls; }
FI XYZEval<T> operator+ (const XYZval<T> &rs) { XYZval<T> ls = *this; NUM_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, ls.u += rs.u, ls.v += rs.v, ls.w += rs.w); return ls; }
FI XYZEval<T> operator- (const XYZval<T> &rs) const { XYZval<T> ls = *this; NUM_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, ls.u -= rs.u, ls.v -= rs.v, ls.w -= rs.w); return ls; }
FI XYZEval<T> operator- (const XYZval<T> &rs) { XYZval<T> ls = *this; NUM_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, ls.u -= rs.u, ls.v -= rs.v, ls.w -= rs.w); return ls; }
FI XYZEval<T> operator* (const XYZval<T> &rs) const { XYZval<T> ls = *this; NUM_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, ls.u *= rs.u, ls.v *= rs.v, ls.w *= rs.w); return ls; }
FI XYZEval<T> operator* (const XYZval<T> &rs) { XYZval<T> ls = *this; NUM_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, ls.u *= rs.u, ls.v *= rs.v, ls.w *= rs.w); return ls; }
FI XYZEval<T> operator/ (const XYZval<T> &rs) const { XYZval<T> ls = *this; NUM_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, ls.u /= rs.u, ls.v /= rs.v, ls.w /= rs.w); return ls; }
FI XYZEval<T> operator/ (const XYZval<T> &rs) { XYZval<T> ls = *this; NUM_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, ls.u /= rs.u, ls.v /= rs.v, ls.w /= rs.w); return ls; }
FI XYZEval<T> operator+ (const XYZEval<T> &rs) const { XYZEval<T> 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, ls.u += rs.u, ls.v += rs.v, ls.w += rs.w); return ls; }
FI XYZEval<T> operator+ (const XYZEval<T> &rs) { XYZEval<T> 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, ls.u += rs.u, ls.v += rs.v, ls.w += rs.w); return ls; }
FI XYZEval<T> operator- (const XYZEval<T> &rs) const { XYZEval<T> 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, ls.u -= rs.u, ls.v -= rs.v, ls.w -= rs.w); return ls; }
FI XYZEval<T> operator- (const XYZEval<T> &rs) { XYZEval<T> 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, ls.u -= rs.u, ls.v -= rs.v, ls.w -= rs.w); return ls; }
FI XYZEval<T> operator* (const XYZEval<T> &rs) const { XYZEval<T> 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, ls.u *= rs.u, ls.v *= rs.v, ls.w *= rs.w); return ls; }
FI XYZEval<T> operator* (const XYZEval<T> &rs) { XYZEval<T> 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, ls.u *= rs.u, ls.v *= rs.v, ls.w *= rs.w); return ls; }
FI XYZEval<T> operator/ (const XYZEval<T> &rs) const { XYZEval<T> 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, ls.u /= rs.u, ls.v /= rs.v, ls.w /= rs.w); return ls; }
FI XYZEval<T> operator/ (const XYZEval<T> &rs) { XYZEval<T> 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, ls.u /= rs.u, ls.v /= rs.v, ls.w /= rs.w); return ls; }
FI XYZEval<T> operator* (const float &v) const { XYZEval<T> 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, ls.u *= v, ls.v *= v, ls.w *= v ); return ls; }
FI XYZEval<T> operator* (const float &v) { XYZEval<T> 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, ls.u *= v, ls.v *= v, ls.w *= v ); return ls; }
FI XYZEval<T> operator* (const int &v) const { XYZEval<T> 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, ls.u *= v, ls.v *= v, ls.w *= v ); return ls; }
FI XYZEval<T> operator* (const int &v) { XYZEval<T> 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, ls.u *= v, ls.v *= v, ls.w *= v ); return ls; }
FI XYZEval<T> operator/ (const float &v) const { XYZEval<T> 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, ls.u /= v, ls.v /= v, ls.w /= v ); return ls; }
FI XYZEval<T> operator/ (const float &v) { XYZEval<T> 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, ls.u /= v, ls.v /= v, ls.w /= v ); return ls; }
FI XYZEval<T> operator/ (const int &v) const { XYZEval<T> 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, ls.u /= v, ls.v /= v, ls.w /= v ); return ls; }
FI XYZEval<T> operator/ (const int &v) { XYZEval<T> 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, ls.u /= v, ls.v /= v, ls.w /= v ); return ls; }
FI XYZEval<T> operator>>(const int &v) const { XYZEval<T> 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), _RS(ls.u), _RS(ls.v), _RS(ls.w) ); return ls; }
FI XYZEval<T> operator>>(const int &v) { XYZEval<T> 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), _RS(ls.u), _RS(ls.v), _RS(ls.w) ); return ls; }
FI XYZEval<T> operator<<(const int &v) const { XYZEval<T> 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), _LS(ls.u), _LS(ls.v), _LS(ls.w) ); return ls; }
FI XYZEval<T> operator<<(const int &v) { XYZEval<T> 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), _LS(ls.u), _LS(ls.v), _LS(ls.w) ); return ls; }
FI const XYZEval<T> operator-() const { return LOGICAL_AXIS_ARRAY(-e, -x, -y, -z, -i, -j, -k, -u, -v, -w); }
FI XYZEval<T> operator-() { return LOGICAL_AXIS_ARRAY(-e, -x, -y, -z, -i, -j, -k, -u, -v, -w); }
// Modifier operators
FI XYZEval<T>& operator+=(const XYval<T> &rs) { x += rs.x; y += rs.y; return *this; }
FI XYZEval<T>& operator-=(const XYval<T> &rs) { x -= rs.x; y -= rs.y; return *this; }
FI XYZEval<T>& operator*=(const XYval<T> &rs) { x *= rs.x; y *= rs.y; return *this; }
FI XYZEval<T>& operator/=(const XYval<T> &rs) { x /= rs.x; y /= rs.y; return *this; }
FI XYZEval<T>& operator+=(const XYZval<T> &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<T>& operator-=(const XYZval<T> &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<T>& operator*=(const XYZval<T> &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<T>& operator/=(const XYZval<T> &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<T>& operator+=(const XYZEval<T> &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<T>& operator-=(const XYZEval<T> &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<T>& operator*=(const XYZEval<T> &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<T>& operator/=(const XYZEval<T> &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<T>& 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<T>& 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<T>& operator<<=(const int &v) { LOGICAL_AXIS_CODE(_LS(e), _LS(x), _LS(y), _LS(z), _LS(i), _LS(j), _LS(k)); return *this; }
FI XYZEval<T>& operator+=(const XYZval<T> &rs) { NUM_AXIS_CODE(x += rs.x, y += rs.y, z += rs.z, i += rs.i, j += rs.j, k += rs.k, u += rs.u, v += rs.v, w += rs.w); return *this; }
FI XYZEval<T>& operator-=(const XYZval<T> &rs) { NUM_AXIS_CODE(x -= rs.x, y -= rs.y, z -= rs.z, i -= rs.i, j -= rs.j, k -= rs.k, u -= rs.u, v -= rs.v, w -= rs.w); return *this; }
FI XYZEval<T>& operator*=(const XYZval<T> &rs) { NUM_AXIS_CODE(x *= rs.x, y *= rs.y, z *= rs.z, i *= rs.i, j *= rs.j, k *= rs.k, u *= rs.u, v *= rs.v, w *= rs.w); return *this; }
FI XYZEval<T>& operator/=(const XYZval<T> &rs) { NUM_AXIS_CODE(x /= rs.x, y /= rs.y, z /= rs.z, i /= rs.i, j /= rs.j, k /= rs.k, u /= rs.u, v /= rs.v, w /= rs.w); return *this; }
FI XYZEval<T>& operator+=(const XYZEval<T> &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, u += rs.u, v += rs.v, w += rs.w); return *this; }
FI XYZEval<T>& operator-=(const XYZEval<T> &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, u -= rs.u, v -= rs.v, w -= rs.w); return *this; }
FI XYZEval<T>& operator*=(const XYZEval<T> &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, u *= rs.u, v *= rs.v, w *= rs.w); return *this; }
FI XYZEval<T>& operator/=(const XYZEval<T> &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, u /= rs.u, v /= rs.v, w /= rs.w); return *this; }
FI XYZEval<T>& operator*=(const T &v) { LOGICAL_AXIS_CODE(e *= v, x *= v, y *= v, z *= v, i *= v, j *= v, k *= v, u *= v, v *= v, w *= v); return *this; }
FI XYZEval<T>& operator>>=(const int &v) { LOGICAL_AXIS_CODE(_RS(e), _RS(x), _RS(y), _RS(z), _RS(i), _RS(j), _RS(k), _RS(u), _RS(v), _RS(w)); return *this; }
FI XYZEval<T>& operator<<=(const int &v) { LOGICAL_AXIS_CODE(_LS(e), _LS(x), _LS(y), _LS(z), _LS(i), _LS(j), _LS(k), _LS(u), _LS(v), _LS(w)); return *this; }
// Exact comparisons. For floats a "NEAR" operation may be better.
FI bool operator==(const XYZval<T> &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<T> &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<T> &rs) { return true NUM_AXIS_GANG(&& x == rs.x, && y == rs.y, && z == rs.z, && i == rs.i, && j == rs.j, && k == rs.k, && u == rs.u, && v == rs.v, && w == rs.w); }
FI bool operator==(const XYZval<T> &rs) const { return true NUM_AXIS_GANG(&& x == rs.x, && y == rs.y, && z == rs.z, && i == rs.i, && j == rs.j, && k == rs.k, && u == rs.u, && v == rs.v, && w == rs.w); }
FI bool operator!=(const XYZval<T> &rs) { return !operator==(rs); }
FI bool operator!=(const XYZval<T> &rs) const { return !operator==(rs); }
};

13
Marlin/src/core/utility.cpp

@ -125,18 +125,17 @@ void safe_delay(millis_t ms) {
#endif
#if ABL_PLANAR
SERIAL_ECHOPGM("ABL Adjustment");
LOOP_LINEAR_AXES(a) {
SERIAL_CHAR(' ', AXIS_CHAR(a));
LOOP_NUM_AXES(a) {
SERIAL_ECHOPGM_P((PGM_P)pgm_read_ptr(&SP_AXIS_STR[a]));
serial_offset(planner.get_axis_position_mm(AxisEnum(a)) - current_position[a]);
}
#else
#if ENABLED(AUTO_BED_LEVELING_UBL)
SERIAL_ECHOPGM("UBL Adjustment Z");
const float rz = ubl.get_z_correction(current_position);
#elif ENABLED(AUTO_BED_LEVELING_BILINEAR)
SERIAL_ECHOPGM("ABL Adjustment Z");
const float rz = bbl.get_z_correction(current_position);
#endif
const float rz = bedlevel.get_z_correction(current_position);
SERIAL_ECHO(ftostr43sign(rz, '+'));
#if ENABLED(ENABLE_LEVELING_FADE_HEIGHT)
if (planner.z_fade_height) {
@ -156,11 +155,13 @@ void safe_delay(millis_t ms) {
SERIAL_ECHOPGM("Mesh Bed Leveling");
if (planner.leveling_active) {
SERIAL_ECHOLNPGM(" (enabled)");
SERIAL_ECHOPGM("MBL Adjustment Z", ftostr43sign(mbl.get_z(current_position), '+'));
const float z_offset = bedlevel.get_z_offset(),
z_correction = bedlevel.get_z_correction(current_position);
SERIAL_ECHOPGM("MBL Adjustment Z", ftostr43sign(z_offset + z_correction, '+'));
#if ENABLED(ENABLE_LEVELING_FADE_HEIGHT)
if (planner.z_fade_height) {
SERIAL_ECHOPGM(" (", ftostr43sign(
mbl.get_z(current_position, planner.fade_scaling_factor_for_z(current_position.z)), '+'
z_offset + z_correction * planner.fade_scaling_factor_for_z(current_position.z), '+'
));
SERIAL_CHAR(')');
}

9
Marlin/src/core/utility.h

@ -77,10 +77,13 @@ public:
// 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
// Axis names for G-code parsing, reports, etc.
const xyze_char_t axis_codes LOGICAL_AXIS_ARRAY('E', 'X', 'Y', 'Z', AXIS4_NAME, AXIS5_NAME, AXIS6_NAME, AXIS7_NAME, AXIS8_NAME, AXIS9_NAME);
#if NUM_AXES <= XYZ && !HAS_EXTRUDERS
#define AXIS_CHAR(A) ((char)('X' + A))
#define IAXIS_CHAR AXIS_CHAR
#else
const xyze_char_t iaxis_codes LOGICAL_AXIS_ARRAY('E', 'X', 'Y', 'Z', 'I', 'J', 'K', 'U', 'V', 'W');
#define AXIS_CHAR(A) axis_codes[A]
#define IAXIS_CHAR(A) iaxis_codes[A]
#endif

49
Marlin/src/feature/backlash.cpp

@ -97,7 +97,7 @@ void Backlash::add_correction_steps(const int32_t &da, const int32_t &db, const
const float f_corr = float(correction) / all_on;
LOOP_LINEAR_AXES(axis) {
LOOP_NUM_AXES(axis) {
if (distance_mm[axis]) {
const bool reverse = TEST(dm, axis);
@ -145,7 +145,7 @@ void Backlash::add_correction_steps(const int32_t &da, const int32_t &db, const
}
int32_t Backlash::get_applied_steps(const AxisEnum axis) {
if (axis >= LINEAR_AXES) return 0;
if (axis >= NUM_AXES) return 0;
const bool reverse = TEST(last_direction_bits, axis);
@ -162,32 +162,37 @@ int32_t Backlash::get_applied_steps(const AxisEnum axis) {
}
class Backlash::StepAdjuster {
xyz_long_t applied_steps;
public:
StepAdjuster() {
LOOP_LINEAR_AXES(axis) applied_steps[axis] = backlash.get_applied_steps((AxisEnum)axis);
}
~StepAdjuster() {
// after backlash compensation parameter changes, ensure applied step count does not change
LOOP_LINEAR_AXES(axis) residual_error[axis] += backlash.get_applied_steps((AxisEnum)axis) - applied_steps[axis];
}
private:
xyz_long_t applied_steps;
public:
StepAdjuster() {
LOOP_NUM_AXES(axis) applied_steps[axis] = backlash.get_applied_steps((AxisEnum)axis);
}
~StepAdjuster() {
// after backlash compensation parameter changes, ensure applied step count does not change
LOOP_NUM_AXES(axis) residual_error[axis] += backlash.get_applied_steps((AxisEnum)axis) - applied_steps[axis];
}
};
void Backlash::set_correction_uint8(const uint8_t v) {
StepAdjuster adjuster;
correction = v;
}
#if ENABLED(BACKLASH_GCODE)
void Backlash::set_distance_mm(const AxisEnum axis, const float v) {
StepAdjuster adjuster;
distance_mm[axis] = v;
}
void Backlash::set_correction_uint8(const uint8_t v) {
StepAdjuster adjuster;
correction = v;
}
#ifdef BACKLASH_SMOOTHING_MM
void Backlash::set_smoothing_mm(const float v) {
void Backlash::set_distance_mm(const AxisEnum axis, const float v) {
StepAdjuster adjuster;
smoothing_mm = v;
distance_mm[axis] = v;
}
#ifdef BACKLASH_SMOOTHING_MM
void Backlash::set_smoothing_mm(const float v) {
StepAdjuster adjuster;
smoothing_mm = v;
}
#endif
#endif
#if ENABLED(MEASURE_BACKLASH_WHEN_PROBING)

Some files were not shown because too many files changed in this diff

Loading…
Cancel
Save