diff --git a/Marlin/HAL.h b/Marlin/HAL.h
new file mode 100644
index 000000000..e4c2f805a
--- /dev/null
+++ b/Marlin/HAL.h
@@ -0,0 +1,305 @@
+/* **************************************************************************
+
+ Marlin 3D Printer Firmware
+ Copyright (C) 2016 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+
+ Copyright (c) 2016 Bob Cousins bobcousins42@googlemail.com
+
+ This program is free software: you can redistribute it and/or modify
+ it under the terms of the GNU General Public License as published by
+ the Free Software Foundation, either version 3 of the License, or
+ (at your option) any later version.
+
+ This program is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ GNU General Public License for more details.
+
+ You should have received a copy of the GNU General Public License
+ along with this program. If not, see .
+****************************************************************************/
+
+/**
+ * Description: HAL for __AVR__
+ */
+
+#ifndef _HAL_AVR_H_
+#define _HAL_AVR_H_
+
+// --------------------------------------------------------------------------
+// Includes
+// --------------------------------------------------------------------------
+
+#include "fastio.h"
+
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+
+// --------------------------------------------------------------------------
+// Defines
+// --------------------------------------------------------------------------
+
+//#define analogInputToDigitalPin(IO) IO
+
+// Bracket code that shouldn't be interrupted
+#ifndef CRITICAL_SECTION_START
+ #define CRITICAL_SECTION_START unsigned char _sreg = SREG; cli();
+ #define CRITICAL_SECTION_END SREG = _sreg;
+#endif
+
+// --------------------------------------------------------------------------
+// Types
+// --------------------------------------------------------------------------
+
+typedef uint16_t hal_timer_t;
+#define HAL_TIMER_TYPE_MAX 0xFFFF
+
+typedef int8_t pin_t;
+
+#define HAL_SERVO_LIB Servo
+
+// --------------------------------------------------------------------------
+// Public Variables
+// --------------------------------------------------------------------------
+
+//extern uint8_t MCUSR;
+
+// --------------------------------------------------------------------------
+// Public functions
+// --------------------------------------------------------------------------
+
+//void cli(void);
+
+//void _delay_ms(const int delay);
+
+inline void HAL_clear_reset_source(void) { MCUSR = 0; }
+inline uint8_t HAL_get_reset_source(void) { return MCUSR; }
+
+// eeprom
+//void eeprom_write_byte(unsigned char *pos, unsigned char value);
+//unsigned char eeprom_read_byte(unsigned char *pos);
+
+// timers
+#define HAL_TIMER_RATE ((F_CPU) / 8) // i.e., 2MHz or 2.5MHz
+
+#define STEP_TIMER_NUM 1
+#define TEMP_TIMER_NUM 0
+#define PULSE_TIMER_NUM TEMP_TIMER_NUM
+
+#define HAL_STEPPER_TIMER_RATE HAL_TIMER_RATE
+#define HAL_TICKS_PER_US ((HAL_STEPPER_TIMER_RATE) / 1000000) // Cannot be of type double
+#define STEPPER_TIMER_PRESCALE 8
+#define STEP_TIMER_MIN_INTERVAL 8 // minimum time in µs between stepper interrupts
+
+#define TEMP_TIMER_FREQUENCY ((F_CPU) / 64.0 / 256.0)
+
+#define TIMER_OCR_1 OCR1A
+#define TIMER_COUNTER_1 TCNT1
+
+#define TIMER_OCR_0 OCR0A
+#define TIMER_COUNTER_0 TCNT0
+
+#define PULSE_TIMER_PRESCALE 8
+
+#define ENABLE_STEPPER_DRIVER_INTERRUPT() SBI(TIMSK1, OCIE1A)
+#define DISABLE_STEPPER_DRIVER_INTERRUPT() CBI(TIMSK1, OCIE1A)
+#define STEPPER_ISR_ENABLED() TEST(TIMSK1, OCIE1A)
+
+#define ENABLE_TEMPERATURE_INTERRUPT() SBI(TIMSK0, OCIE0B)
+#define DISABLE_TEMPERATURE_INTERRUPT() CBI(TIMSK0, OCIE0B)
+#define TEMPERATURE_ISR_ENABLED() TEST(TIMSK0, OCIE0B)
+
+#define HAL_timer_start(timer_num, frequency)
+
+#define _CAT(a, ...) a ## __VA_ARGS__
+#define HAL_timer_set_compare(timer, compare) (_CAT(TIMER_OCR_, timer) = compare)
+#define HAL_timer_restrain(timer, interval_ticks) NOLESS(_CAT(TIMER_OCR_, timer), _CAT(TIMER_COUNTER_, timer) + interval_ticks)
+
+#define HAL_timer_get_compare(timer) _CAT(TIMER_OCR_, timer)
+#define HAL_timer_get_count(timer) _CAT(TIMER_COUNTER_, timer)
+
+/**
+ * On AVR there is no hardware prioritization and preemption of
+ * interrupts, so this emulates it. The UART has first priority
+ * (otherwise, characters will be lost due to UART overflow).
+ * Then: Stepper, Endstops, Temperature, and -finally- all others.
+ */
+#define HAL_timer_isr_prologue(TIMER_NUM)
+#define HAL_timer_isr_epilogue(TIMER_NUM)
+
+/* 18 cycles maximum latency */
+#define HAL_STEP_TIMER_ISR \
+extern "C" void TIMER1_COMPA_vect (void) __attribute__ ((signal, naked, used, externally_visible)); \
+extern "C" void TIMER1_COMPA_vect_bottom (void) asm ("TIMER1_COMPA_vect_bottom") __attribute__ ((used, externally_visible, noinline)); \
+void TIMER1_COMPA_vect (void) { \
+ __asm__ __volatile__ ( \
+ A("push r16") /* 2 Save R16 */ \
+ A("in r16, __SREG__") /* 1 Get SREG */ \
+ A("push r16") /* 2 Save SREG into stack */ \
+ A("lds r16, %[timsk0]") /* 2 Load into R0 the Temperature timer Interrupt mask register */ \
+ A("push r16") /* 2 Save TIMSK0 into the stack */ \
+ A("andi r16,~%[msk0]") /* 1 Disable the temperature ISR */ \
+ A("sts %[timsk0], r16") /* 2 And set the new value */ \
+ A("lds r16, %[timsk1]") /* 2 Load into R0 the stepper timer Interrupt mask register [TIMSK1] */ \
+ A("andi r16,~%[msk1]") /* 1 Disable the stepper ISR */ \
+ A("sts %[timsk1], r16") /* 2 And set the new value */ \
+ A("sei") /* 1 Enable global interrupts - stepper and temperature ISRs are disabled, so no risk of reentry or being preempted by the temperature ISR */ \
+ A("push r16") /* 2 Save TIMSK1 into stack */ \
+ A("in r16, 0x3B") /* 1 Get RAMPZ register */ \
+ A("push r16") /* 2 Save RAMPZ into stack */ \
+ A("in r16, 0x3C") /* 1 Get EIND register */ \
+ A("push r0") /* C runtime can modify all the following registers without restoring them */ \
+ A("push r1") \
+ A("push r18") \
+ A("push r19") \
+ A("push r20") \
+ A("push r21") \
+ A("push r22") \
+ A("push r23") \
+ A("push r24") \
+ A("push r25") \
+ A("push r26") \
+ A("push r27") \
+ A("push r30") \
+ A("push r31") \
+ A("clr r1") /* C runtime expects this register to be 0 */ \
+ A("call TIMER1_COMPA_vect_bottom") /* Call the bottom handler - No inlining allowed, otherwise registers used are not saved */ \
+ A("pop r31") \
+ A("pop r30") \
+ A("pop r27") \
+ A("pop r26") \
+ A("pop r25") \
+ A("pop r24") \
+ A("pop r23") \
+ A("pop r22") \
+ A("pop r21") \
+ A("pop r20") \
+ A("pop r19") \
+ A("pop r18") \
+ A("pop r1") \
+ A("pop r0") \
+ A("out 0x3C, r16") /* 1 Restore EIND register */ \
+ A("pop r16") /* 2 Get the original RAMPZ register value */ \
+ A("out 0x3B, r16") /* 1 Restore RAMPZ register to its original value */ \
+ A("pop r16") /* 2 Get the original TIMSK1 value but with stepper ISR disabled */ \
+ A("ori r16,%[msk1]") /* 1 Reenable the stepper ISR */ \
+ A("cli") /* 1 Disable global interrupts - Reenabling Stepper ISR can reenter amd temperature can reenter, and we want that, if it happens, after this ISR has ended */ \
+ A("sts %[timsk1], r16") /* 2 And restore the old value - This reenables the stepper ISR */ \
+ A("pop r16") /* 2 Get the temperature timer Interrupt mask register [TIMSK0] */ \
+ A("sts %[timsk0], r16") /* 2 And restore the old value - This reenables the temperature ISR */ \
+ A("pop r16") /* 2 Get the old SREG value */ \
+ A("out __SREG__, r16") /* 1 And restore the SREG value */ \
+ A("pop r16") /* 2 Restore R16 value */ \
+ A("reti") /* 4 Return from interrupt */ \
+ : \
+ : [timsk0] "i" ((uint16_t)&TIMSK0), \
+ [timsk1] "i" ((uint16_t)&TIMSK1), \
+ [msk0] "M" ((uint8_t)(1< 7) ADCSRB = _BV(MUX5); else ADCSRB = 0; SET_ADMUX_ADCSRA(pin)
+#else
+ #define HAL_START_ADC(pin) ADCSRB = 0; SET_ADMUX_ADCSRA(pin)
+#endif
+
+#define HAL_READ_ADC ADC
+
+#define GET_PIN_MAP_PIN(index) index
+#define GET_PIN_MAP_INDEX(pin) pin
+#define PARSED_PIN_INDEX(code, dval) parser.intval(code, dval)
+
+#define HAL_SENSITIVE_PINS 0, 1
+
+#endif // _HAL_AVR_H_
diff --git a/Marlin/MarlinConfig.h b/Marlin/MarlinConfig.h
index 5f77dba0e..47e6020d9 100644
--- a/Marlin/MarlinConfig.h
+++ b/Marlin/MarlinConfig.h
@@ -23,21 +23,25 @@
#ifndef MARLIN_CONFIG_H
#define MARLIN_CONFIG_H
-#include "fastio.h"
-#include "macros.h"
#include "boards.h"
+#include "macros.h"
#include "Version.h"
#include "Configuration.h"
#include "Conditionals_LCD.h"
#include "Configuration_adv.h"
-#include "pins.h"
+
#if defined(__AVR__) && !defined(USBCON)
#define HardwareSerial_h // trick to disable the standard HWserial
#endif
-#include "Arduino.h"
+
+#include "types.h"
+#include "HAL.h"
+#include "pins.h"
#include "Conditionals_post.h"
#include "SanityCheck.h"
-
-#include
+#include "enum.h"
+#include "language.h"
+#include "utility.h"
+#include "serial.h"
#endif // MARLIN_CONFIG_H
diff --git a/Marlin/Marlin_main.cpp b/Marlin/Marlin_main.cpp
index cdaf65376..2fac62ab3 100644
--- a/Marlin/Marlin_main.cpp
+++ b/Marlin/Marlin_main.cpp
@@ -336,10 +336,6 @@
#include "I2CPositionEncoder.h"
#endif
-#if ENABLED(ENDSTOP_INTERRUPTS_FEATURE)
- #include "endstop_interrupts.h"
-#endif
-
#if ENABLED(M100_FREE_MEMORY_WATCHER)
void gcode_M100();
void M100_dump_routine(const char * const title, const char *start, const char *end);
@@ -8474,7 +8470,7 @@ inline void gcode_M111() {
*/
inline void gcode_M81() {
thermalManager.disable_all_heaters();
- stepper.finish_and_disable();
+ planner.finish_and_disable();
#if FAN_COUNT > 0
for (uint8_t i = 0; i < FAN_COUNT; i++) fanSpeeds[i] = 0;
@@ -8517,7 +8513,7 @@ inline void gcode_M18_M84() {
else {
bool all_axis = !(parser.seen('X') || parser.seen('Y') || parser.seen('Z') || parser.seen('E'));
if (all_axis) {
- stepper.finish_and_disable();
+ planner.finish_and_disable();
}
else {
planner.synchronize();
@@ -9963,7 +9959,7 @@ inline void gcode_M400() { planner.synchronize(); }
#endif // FILAMENT_WIDTH_SENSOR
void quickstop_stepper() {
- stepper.quick_stop();
+ planner.quick_stop();
planner.synchronize();
set_current_from_steppers_for_axis(ALL_AXES);
SYNC_PLAN_POSITION_KINEMATIC();
@@ -10342,7 +10338,7 @@ inline void gcode_M502() {
* M540: Set whether SD card print should abort on endstop hit (M540 S<0|1>)
*/
inline void gcode_M540() {
- if (parser.seen('S')) stepper.abort_on_endstop_hit = parser.value_bool();
+ if (parser.seen('S')) planner.abort_on_endstop_hit = parser.value_bool();
}
#endif // ABORT_ON_ENDSTOP_HIT_FEATURE_ENABLED
@@ -12995,7 +12991,8 @@ void set_current_from_steppers_for_axis(const AxisEnum axis) {
idle();
}
LOOP_XYZE(i) raw[i] += segment_distance[i];
- planner.buffer_line_kinematic(raw, fr_mm_s, active_extruder, cartesian_segment_mm);
+ if (!planner.buffer_line_kinematic(raw, fr_mm_s, active_extruder, cartesian_segment_mm))
+ break;
}
// Since segment_distance is only approximate,
@@ -13281,7 +13278,8 @@ void set_current_from_steppers_for_axis(const AxisEnum axis) {
#if ENABLED(SCARA_FEEDRATE_SCALING)
// For SCARA scale the feed rate from mm/s to degrees/s
// i.e., Complete the angular vector in the given time.
- planner.buffer_segment(delta[A_AXIS], delta[B_AXIS], raw[Z_AXIS], raw[E_AXIS], HYPOT(delta[A_AXIS] - oldA, delta[B_AXIS] - oldB) * inverse_secs, active_extruder);
+ if (!planner.buffer_segment(delta[A_AXIS], delta[B_AXIS], raw[Z_AXIS], raw[E_AXIS], HYPOT(delta[A_AXIS] - oldA, delta[B_AXIS] - oldB) * inverse_secs, active_extruder))
+ break;
/*
SERIAL_ECHO(segments);
SERIAL_ECHOPAIR(": X=", raw[X_AXIS]); SERIAL_ECHOPAIR(" Y=", raw[Y_AXIS]);
@@ -13291,7 +13289,8 @@ void set_current_from_steppers_for_axis(const AxisEnum axis) {
//*/
oldA = delta[A_AXIS]; oldB = delta[B_AXIS];
#else
- planner.buffer_line(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], raw[E_AXIS], _feedrate_mm_s, active_extruder, cartesian_segment_mm);
+ if (!planner.buffer_line(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], raw[E_AXIS], _feedrate_mm_s, active_extruder, cartesian_segment_mm))
+ break;
#endif
}
@@ -13385,14 +13384,14 @@ void set_current_from_steppers_for_axis(const AxisEnum axis) {
}
// unpark extruder: 1) raise, 2) move into starting XY position, 3) lower
for (uint8_t i = 0; i < 3; i++)
- planner.buffer_line(
+ if (!planner.buffer_line(
i == 0 ? raised_parked_position[X_AXIS] : current_position[X_AXIS],
i == 0 ? raised_parked_position[Y_AXIS] : current_position[Y_AXIS],
i == 2 ? current_position[Z_AXIS] : raised_parked_position[Z_AXIS],
current_position[E_AXIS],
i == 1 ? PLANNER_XY_FEEDRATE() : planner.max_feedrate_mm_s[Z_AXIS],
- active_extruder
- );
+ active_extruder)
+ ) break;
delayed_move_time = 0;
active_extruder_parked = false;
#if ENABLED(DEBUG_LEVELING_FEATURE)
@@ -13409,17 +13408,12 @@ void set_current_from_steppers_for_axis(const AxisEnum axis) {
}
#endif
// move duplicate extruder into correct duplication position.
- planner.set_position_mm(
- inactive_extruder_x_pos,
- current_position[Y_AXIS],
- current_position[Z_AXIS],
- current_position[E_AXIS]
- );
- planner.buffer_line(
+ planner.set_position_mm(inactive_extruder_x_pos, current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
+ if (!planner.buffer_line(
current_position[X_AXIS] + duplicate_extruder_x_offset,
current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS],
- planner.max_feedrate_mm_s[X_AXIS], 1
- );
+ planner.max_feedrate_mm_s[X_AXIS], 1)
+ ) break;
planner.synchronize();
SYNC_PLAN_POSITION_KINEMATIC();
extruder_duplication_enabled = true;
@@ -13652,14 +13646,17 @@ void prepare_move_to_destination() {
// i.e., Complete the angular vector in the given time.
inverse_kinematics(raw);
ADJUST_DELTA(raw);
- planner.buffer_segment(delta[A_AXIS], delta[B_AXIS], raw[Z_AXIS], raw[E_AXIS], HYPOT(delta[A_AXIS] - oldA, delta[B_AXIS] - oldB) * inverse_secs, active_extruder);
+ if (!planner.buffer_segment(delta[A_AXIS], delta[B_AXIS], raw[Z_AXIS], raw[E_AXIS], HYPOT(delta[A_AXIS] - oldA, delta[B_AXIS] - oldB) * inverse_secs, active_extruder))
+ break;
oldA = delta[A_AXIS]; oldB = delta[B_AXIS];
#elif HAS_UBL_AND_CURVES
float pos[XYZ] = { raw[X_AXIS], raw[Y_AXIS], raw[Z_AXIS] };
planner.apply_leveling(pos);
- planner.buffer_segment(pos[X_AXIS], pos[Y_AXIS], pos[Z_AXIS], raw[E_AXIS], fr_mm_s, active_extruder);
+ if (!planner.buffer_segment(pos[X_AXIS], pos[Y_AXIS], pos[Z_AXIS], raw[E_AXIS], fr_mm_s, active_extruder))
+ break;
#else
- planner.buffer_line_kinematic(raw, fr_mm_s, active_extruder);
+ if (!planner.buffer_line_kinematic(raw, fr_mm_s, active_extruder))
+ break;
#endif
}
@@ -14291,7 +14288,9 @@ void setup() {
print_job_timer.init(); // Initial setup of print job timer
- stepper.init(); // Initialize stepper, this enables interrupts!
+ endstops.init(); // Init endstops and pullups
+
+ stepper.init(); // Init stepper. This enables interrupts!
servo_init(); // Initialize all servos, stow servo probe
@@ -14416,10 +14415,6 @@ void setup() {
i2c.onRequest(i2c_on_request);
#endif
- #if ENABLED(ENDSTOP_INTERRUPTS_FEATURE)
- setup_endstop_interrupts();
- #endif
-
#if DO_SWITCH_EXTRUDER
move_extruder_servo(0); // Initialize extruder servo
#endif
diff --git a/Marlin/cardreader.cpp b/Marlin/cardreader.cpp
index 109ab428b..28fdf1152 100644
--- a/Marlin/cardreader.cpp
+++ b/Marlin/cardreader.cpp
@@ -941,7 +941,7 @@ void CardReader::printingHasFinished() {
#endif
#if ENABLED(SD_FINISHED_STEPPERRELEASE) && defined(SD_FINISHED_RELEASECOMMAND)
- stepper.cleaning_buffer_counter = 1; // The command will fire from the Stepper ISR
+ planner.finish_and_disable();
#endif
print_job_timer.stop();
if (print_job_timer.duration() > 60)
diff --git a/Marlin/endstop_interrupts.h b/Marlin/endstop_interrupts.h
index c83bac23a..65f0d1a5b 100644
--- a/Marlin/endstop_interrupts.h
+++ b/Marlin/endstop_interrupts.h
@@ -24,7 +24,7 @@
* Endstop Interrupts
*
* Without endstop interrupts the endstop pins must be polled continually in
- * the stepper-ISR via endstops.update(), most of the time finding no change.
+ * the temperature-ISR via endstops.update(), most of the time finding no change.
* With this feature endstops.update() is called only when we know that at
* least one endstop has changed state, saving valuable CPU cycles.
*
@@ -40,6 +40,9 @@
#include "macros.h"
+// One ISR for all EXT-Interrupts
+void endstop_ISR(void) { endstops.check_possible_change(); }
+
/**
* Patch for pins_arduino.h (...\Arduino\hardware\arduino\avr\variants\mega\pins_arduino.h)
*
@@ -72,8 +75,6 @@
0 )
#endif
-volatile uint8_t e_hit = 0; // Different from 0 when the endstops should be tested in detail.
- // Must be reset to 0 by the test function when finished.
// Install Pin change interrupt for a pin. Can be called multiple times.
void pciSetup(const int8_t pin) {
@@ -82,30 +83,22 @@ void pciSetup(const int8_t pin) {
SBI(PCICR, digitalPinToPCICRbit(pin)); // enable interrupt for the group
}
-// This is what is really done inside the interrupts.
-FORCE_INLINE void endstop_ISR_worker( void ) {
- e_hit = 2; // Because the detection of a e-stop hit has a 1 step debouncer it has to be called at least twice.
-}
-
-// Use one Routine to handle each group
-// One ISR for all EXT-Interrupts
-void endstop_ISR(void) { endstop_ISR_worker(); }
// Handlers for pin change interrupts
#ifdef PCINT0_vect
- ISR(PCINT0_vect) { endstop_ISR_worker(); }
+ ISR(PCINT0_vect) { endstop_ISR(); }
#endif
#ifdef PCINT1_vect
- ISR(PCINT1_vect) { endstop_ISR_worker(); }
+ ISR(PCINT1_vect) { endstop_ISR(); }
#endif
#ifdef PCINT2_vect
- ISR(PCINT2_vect) { endstop_ISR_worker(); }
+ ISR(PCINT2_vect) { endstop_ISR(); }
#endif
#ifdef PCINT3_vect
- ISR(PCINT3_vect) { endstop_ISR_worker(); }
+ ISR(PCINT3_vect) { endstop_ISR(); }
#endif
void setup_endstop_interrupts( void ) {
diff --git a/Marlin/endstops.cpp b/Marlin/endstops.cpp
index 93fbd9a5a..734dcecfa 100644
--- a/Marlin/endstops.cpp
+++ b/Marlin/endstops.cpp
@@ -31,18 +31,27 @@
#include "stepper.h"
#include "ultralcd.h"
-// TEST_ENDSTOP: test the old and the current status of an endstop
-#define TEST_ENDSTOP(ENDSTOP) (TEST(current_endstop_bits & old_endstop_bits, ENDSTOP))
+#if ENABLED(ENDSTOP_INTERRUPTS_FEATURE)
+ #include "endstop_interrupts.h"
+#endif
+
+// TEST_ENDSTOP: test the current status of an endstop
+#define TEST_ENDSTOP(ENDSTOP) (TEST(current_endstop_bits, ENDSTOP))
+
+#if HAS_BED_PROBE
+ #define ENDSTOPS_ENABLED (endstops.enabled || endstops.z_probe_enabled)
+#else
+ #define ENDSTOPS_ENABLED endstops.enabled
+#endif
Endstops endstops;
// public:
bool Endstops::enabled, Endstops::enabled_globally; // Initialized by settings.load()
-volatile char Endstops::endstop_hit_bits; // use X_MIN, Y_MIN, Z_MIN and Z_MIN_PROBE as BIT value
+volatile uint8_t Endstops::endstop_hit_bits; // use X_MIN, Y_MIN, Z_MIN and Z_MIN_PROBE as BIT value
-Endstops::esbits_t Endstops::current_endstop_bits = 0,
- Endstops::old_endstop_bits = 0;
+Endstops::esbits_t Endstops::current_endstop_bits = 0;
#if HAS_BED_PROBE
volatile bool Endstops::z_probe_enabled = false;
@@ -169,8 +178,93 @@ void Endstops::init() {
#endif
#endif
+ #if ENABLED(ENDSTOP_INTERRUPTS_FEATURE)
+ setup_endstop_interrupts();
+ #endif
+
+ // Enable endstops
+ enable_globally(
+ #if ENABLED(ENDSTOPS_ALWAYS_ON_DEFAULT)
+ true
+ #else
+ false
+ #endif
+ );
+
} // Endstops::init
+// Called from ISR. A change was detected. Find out what happened!
+void Endstops::check_possible_change() { if (ENDSTOPS_ENABLED) endstops.update(); }
+
+// Called from ISR: Poll endstop state if required
+void Endstops::poll() {
+
+ #if ENABLED(PINS_DEBUGGING)
+ endstops.run_monitor(); // report changes in endstop status
+ #endif
+
+ #if DISABLED(ENDSTOP_INTERRUPTS_FEATURE)
+ if (ENDSTOPS_ENABLED) endstops.update();
+ #endif
+}
+
+void Endstops::enable_globally(const bool onoff) {
+ enabled_globally = enabled = onoff;
+
+ #if ENABLED(ENDSTOP_INTERRUPTS_FEATURE)
+ if (onoff) endstops.update(); // If enabling, update state now
+ #endif
+}
+
+// Enable / disable endstop checking
+void Endstops::enable(const bool onoff) {
+ enabled = onoff;
+
+ #if ENABLED(ENDSTOP_INTERRUPTS_FEATURE)
+ if (onoff) endstops.update(); // If enabling, update state now
+ #endif
+}
+
+
+// Disable / Enable endstops based on ENSTOPS_ONLY_FOR_HOMING and global enable
+void Endstops::not_homing() {
+ enabled = enabled_globally;
+
+ #if ENABLED(ENDSTOP_INTERRUPTS_FEATURE)
+ if (enabled) endstops.update(); // If enabling, update state now
+ #endif
+}
+
+// Clear endstops (i.e., they were hit intentionally) to suppress the report
+void Endstops::hit_on_purpose() {
+ endstop_hit_bits = 0;
+
+ #if ENABLED(ENDSTOP_INTERRUPTS_FEATURE)
+ if (enabled) endstops.update(); // If enabling, update state now
+ #endif
+}
+
+// Enable / disable endstop z-probe checking
+#if HAS_BED_PROBE
+ void Endstops::enable_z_probe(bool onoff) {
+ z_probe_enabled = onoff;
+
+ #if ENABLED(ENDSTOP_INTERRUPTS_FEATURE)
+ if (enabled) endstops.update(); // If enabling, update state now
+ #endif
+ }
+#endif
+
+#if ENABLED(PINS_DEBUGGING)
+ void Endstops::run_monitor() {
+ if (!monitor_flag) return;
+ static uint8_t monitor_count = 16; // offset this check from the others
+ monitor_count += _BV(1); // 15 Hz
+ monitor_count &= 0x7F;
+ if (!monitor_count) monitor(); // report changes in endstop status
+ }
+#endif
+
void Endstops::report_state() {
if (endstop_hit_bits) {
#if ENABLED(ULTRA_LCD)
@@ -181,7 +275,7 @@ void Endstops::report_state() {
#endif
#define _ENDSTOP_HIT_ECHO(A,C) do{ \
- SERIAL_ECHOPAIR(" " STRINGIFY(A) ":", stepper.triggered_position_mm(_AXIS(A))); \
+ SERIAL_ECHOPAIR(" " STRINGIFY(A) ":", planner.triggered_position_mm(_AXIS(A))); \
_SET_STOP_CHAR(A,C); }while(0)
#define _ENDSTOP_HIT_TEST(A,C) \
@@ -211,7 +305,7 @@ void Endstops::report_state() {
hit_on_purpose();
#if ENABLED(ABORT_ON_ENDSTOP_HIT_FEATURE_ENABLED) && ENABLED(SDSUPPORT)
- if (stepper.abort_on_endstop_hit) {
+ if (planner.abort_on_endstop_hit) {
card.sdprinting = false;
card.closefile();
quickstop_stepper();
@@ -273,38 +367,41 @@ void Endstops::M119() {
#endif
} // Endstops::M119
+// The following routines are called from an ISR context. It could be the temperature ISR, the
+// endstop ISR or the Stepper ISR.
+
#if ENABLED(X_DUAL_ENDSTOPS)
void Endstops::test_dual_x_endstops(const EndstopEnum es1, const EndstopEnum es2) {
const byte x_test = TEST_ENDSTOP(es1) | (TEST_ENDSTOP(es2) << 1); // bit 0 for X, bit 1 for X2
- if (x_test && stepper.current_block->steps[X_AXIS] > 0) {
+ if (x_test && stepper.movement_non_null(X_AXIS)) {
SBI(endstop_hit_bits, X_MIN);
if (!stepper.performing_homing || (x_test == 0x3)) //if not performing home or if both endstops were trigged during homing...
- stepper.kill_current_block();
+ stepper.quick_stop();
}
}
#endif
#if ENABLED(Y_DUAL_ENDSTOPS)
void Endstops::test_dual_y_endstops(const EndstopEnum es1, const EndstopEnum es2) {
const byte y_test = TEST_ENDSTOP(es1) | (TEST_ENDSTOP(es2) << 1); // bit 0 for Y, bit 1 for Y2
- if (y_test && stepper.current_block->steps[Y_AXIS] > 0) {
+ if (y_test && stepper.movement_non_null(Y_AXIS)) {
SBI(endstop_hit_bits, Y_MIN);
if (!stepper.performing_homing || (y_test == 0x3)) //if not performing home or if both endstops were trigged during homing...
- stepper.kill_current_block();
+ stepper.quick_stop();
}
}
#endif
#if ENABLED(Z_DUAL_ENDSTOPS)
void Endstops::test_dual_z_endstops(const EndstopEnum es1, const EndstopEnum es2) {
const byte z_test = TEST_ENDSTOP(es1) | (TEST_ENDSTOP(es2) << 1); // bit 0 for Z, bit 1 for Z2
- if (z_test && stepper.current_block->steps[Z_AXIS] > 0) {
+ if (z_test && stepper.movement_non_null(Z_AXIS)) {
SBI(endstop_hit_bits, Z_MIN);
if (!stepper.performing_homing || (z_test == 0x3)) //if not performing home or if both endstops were trigged during homing...
- stepper.kill_current_block();
+ stepper.quick_stop();
}
}
#endif
-// Check endstops - Called from ISR!
+// Check endstops - Could be called from ISR!
void Endstops::update() {
#define _ENDSTOP(AXIS, MINMAX) AXIS ##_## MINMAX
@@ -322,7 +419,7 @@ void Endstops::update() {
UPDATE_ENDSTOP_BIT(AXIS, MINMAX); \
if (TEST_ENDSTOP(_ENDSTOP(AXIS, MINMAX))) { \
_ENDSTOP_HIT(AXIS, MINMAX); \
- stepper.endstop_triggered(_AXIS(AXIS)); \
+ planner.endstop_triggered(_AXIS(AXIS)); \
} \
}while(0)
@@ -331,9 +428,9 @@ void Endstops::update() {
if (G38_move) {
UPDATE_ENDSTOP_BIT(Z, MIN_PROBE);
if (TEST_ENDSTOP(_ENDSTOP(Z, MIN_PROBE))) {
- if (stepper.current_block->steps[_AXIS(X)] > 0) { _ENDSTOP_HIT(X, MIN); stepper.endstop_triggered(_AXIS(X)); }
- else if (stepper.current_block->steps[_AXIS(Y)] > 0) { _ENDSTOP_HIT(Y, MIN); stepper.endstop_triggered(_AXIS(Y)); }
- else if (stepper.current_block->steps[_AXIS(Z)] > 0) { _ENDSTOP_HIT(Z, MIN); stepper.endstop_triggered(_AXIS(Z)); }
+ if (stepper.movement_non_null(_AXIS(X))) { _ENDSTOP_HIT(X, MIN); planner.endstop_triggered(_AXIS(X)); }
+ else if (stepper.movement_non_null(_AXIS(Y))) { _ENDSTOP_HIT(Y, MIN); planner.endstop_triggered(_AXIS(Y)); }
+ else if (stepper.movement_non_null(_AXIS(Z))) { _ENDSTOP_HIT(Z, MIN); planner.endstop_triggered(_AXIS(Z)); }
G38_endstop_hit = true;
}
}
@@ -344,7 +441,7 @@ void Endstops::update() {
*/
#if IS_CORE
- #define S_(N) stepper.current_block->steps[CORE_AXIS_##N]
+ #define S_(N) stepper.movement_non_null(CORE_AXIS_##N)
#define D_(N) stepper.motor_direction(CORE_AXIS_##N)
#endif
@@ -364,7 +461,7 @@ void Endstops::update() {
#define X_MOVE_TEST ( S_(1) != S_(2) || (S_(1) > 0 && D_(1) X_CMP D_(2)) )
#define X_AXIS_HEAD X_HEAD
#else
- #define X_MOVE_TEST stepper.current_block->steps[X_AXIS] > 0
+ #define X_MOVE_TEST stepper.movement_non_null(X_AXIS)
#define X_AXIS_HEAD X_AXIS
#endif
@@ -384,7 +481,7 @@ void Endstops::update() {
#define Y_MOVE_TEST ( S_(1) != S_(2) || (S_(1) > 0 && D_(1) Y_CMP D_(2)) )
#define Y_AXIS_HEAD Y_HEAD
#else
- #define Y_MOVE_TEST stepper.current_block->steps[Y_AXIS] > 0
+ #define Y_MOVE_TEST stepper.movement_non_null(Y_AXIS)
#define Y_AXIS_HEAD Y_AXIS
#endif
@@ -404,13 +501,13 @@ void Endstops::update() {
#define Z_MOVE_TEST ( S_(1) != S_(2) || (S_(1) > 0 && D_(1) Z_CMP D_(2)) )
#define Z_AXIS_HEAD Z_HEAD
#else
- #define Z_MOVE_TEST stepper.current_block->steps[Z_AXIS] > 0
+ #define Z_MOVE_TEST stepper.movement_non_null(Z_AXIS)
#define Z_AXIS_HEAD Z_AXIS
#endif
// With Dual X, endstops are only checked in the homing direction for the active extruder
#if ENABLED(DUAL_X_CARRIAGE)
- #define E0_ACTIVE stepper.current_block->active_extruder == 0
+ #define E0_ACTIVE stepper.movement_extruder() == 0
#define X_MIN_TEST ((X_HOME_DIR < 0 && E0_ACTIVE) || (X2_HOME_DIR < 0 && !E0_ACTIVE))
#define X_MAX_TEST ((X_HOME_DIR > 0 && E0_ACTIVE) || (X2_HOME_DIR > 0 && !E0_ACTIVE))
#else
@@ -421,124 +518,117 @@ void Endstops::update() {
/**
* Check and update endstops according to conditions
*/
- if (stepper.current_block) {
-
- if (X_MOVE_TEST) {
- if (stepper.motor_direction(X_AXIS_HEAD)) { // -direction
- #if HAS_X_MIN
- #if ENABLED(X_DUAL_ENDSTOPS)
- UPDATE_ENDSTOP_BIT(X, MIN);
- #if HAS_X2_MIN
- UPDATE_ENDSTOP_BIT(X2, MIN);
- #else
- COPY_BIT(current_endstop_bits, X_MIN, X2_MIN);
- #endif
- test_dual_x_endstops(X_MIN, X2_MIN);
+ if (X_MOVE_TEST) {
+ if (stepper.motor_direction(X_AXIS_HEAD)) { // -direction
+ #if HAS_X_MIN
+ #if ENABLED(X_DUAL_ENDSTOPS)
+ UPDATE_ENDSTOP_BIT(X, MIN);
+ #if HAS_X2_MIN
+ UPDATE_ENDSTOP_BIT(X2, MIN);
#else
- if (X_MIN_TEST) UPDATE_ENDSTOP(X, MIN);
+ COPY_BIT(current_endstop_bits, X_MIN, X2_MIN);
#endif
+ test_dual_x_endstops(X_MIN, X2_MIN);
+ #else
+ if (X_MIN_TEST) UPDATE_ENDSTOP(X, MIN);
#endif
- }
- else { // +direction
- #if HAS_X_MAX
- #if ENABLED(X_DUAL_ENDSTOPS)
- UPDATE_ENDSTOP_BIT(X, MAX);
- #if HAS_X2_MAX
- UPDATE_ENDSTOP_BIT(X2, MAX);
- #else
- COPY_BIT(current_endstop_bits, X_MAX, X2_MAX);
- #endif
- test_dual_x_endstops(X_MAX, X2_MAX);
- #else
- if (X_MAX_TEST) UPDATE_ENDSTOP(X, MAX);
- #endif
- #endif
- }
+ #endif
}
-
- if (Y_MOVE_TEST) {
- if (stepper.motor_direction(Y_AXIS_HEAD)) { // -direction
- #if HAS_Y_MIN
- #if ENABLED(Y_DUAL_ENDSTOPS)
- UPDATE_ENDSTOP_BIT(Y, MIN);
- #if HAS_Y2_MIN
- UPDATE_ENDSTOP_BIT(Y2, MIN);
- #else
- COPY_BIT(current_endstop_bits, Y_MIN, Y2_MIN);
- #endif
- test_dual_y_endstops(Y_MIN, Y2_MIN);
+ else { // +direction
+ #if HAS_X_MAX
+ #if ENABLED(X_DUAL_ENDSTOPS)
+ UPDATE_ENDSTOP_BIT(X, MAX);
+ #if HAS_X2_MAX
+ UPDATE_ENDSTOP_BIT(X2, MAX);
#else
- UPDATE_ENDSTOP(Y, MIN);
+ COPY_BIT(current_endstop_bits, X_MAX, X2_MAX);
#endif
+ test_dual_x_endstops(X_MAX, X2_MAX);
+ #else
+ if (X_MAX_TEST) UPDATE_ENDSTOP(X, MAX);
#endif
- }
- else { // +direction
- #if HAS_Y_MAX
- #if ENABLED(Y_DUAL_ENDSTOPS)
- UPDATE_ENDSTOP_BIT(Y, MAX);
- #if HAS_Y2_MAX
- UPDATE_ENDSTOP_BIT(Y2, MAX);
- #else
- COPY_BIT(current_endstop_bits, Y_MAX, Y2_MAX);
- #endif
- test_dual_y_endstops(Y_MAX, Y2_MAX);
- #else
- UPDATE_ENDSTOP(Y, MAX);
- #endif
- #endif
- }
+ #endif
}
+ }
- if (Z_MOVE_TEST) {
- if (stepper.motor_direction(Z_AXIS_HEAD)) { // Z -direction. Gantry down, bed up.
- #if HAS_Z_MIN
- #if ENABLED(Z_DUAL_ENDSTOPS)
- UPDATE_ENDSTOP_BIT(Z, MIN);
- #if HAS_Z2_MIN
- UPDATE_ENDSTOP_BIT(Z2, MIN);
- #else
- COPY_BIT(current_endstop_bits, Z_MIN, Z2_MIN);
- #endif
- test_dual_z_endstops(Z_MIN, Z2_MIN);
+ if (Y_MOVE_TEST) {
+ if (stepper.motor_direction(Y_AXIS_HEAD)) { // -direction
+ #if HAS_Y_MIN
+ #if ENABLED(Y_DUAL_ENDSTOPS)
+ UPDATE_ENDSTOP_BIT(Y, MIN);
+ #if HAS_Y2_MIN
+ UPDATE_ENDSTOP_BIT(Y2, MIN);
#else
- #if ENABLED(Z_MIN_PROBE_USES_Z_MIN_ENDSTOP_PIN)
- if (z_probe_enabled) UPDATE_ENDSTOP(Z, MIN);
- #else
- UPDATE_ENDSTOP(Z, MIN);
- #endif
+ COPY_BIT(current_endstop_bits, Y_MIN, Y2_MIN);
#endif
+ test_dual_y_endstops(Y_MIN, Y2_MIN);
+ #else
+ UPDATE_ENDSTOP(Y, MIN);
#endif
-
- // When closing the gap check the enabled probe
- #if ENABLED(Z_MIN_PROBE_ENDSTOP)
- if (z_probe_enabled) {
- UPDATE_ENDSTOP(Z, MIN_PROBE);
- if (TEST_ENDSTOP(Z_MIN_PROBE)) SBI(endstop_hit_bits, Z_MIN_PROBE);
- }
- #endif
- }
- else { // Z +direction. Gantry up, bed down.
- #if HAS_Z_MAX
- // Check both Z dual endstops
- #if ENABLED(Z_DUAL_ENDSTOPS)
- UPDATE_ENDSTOP_BIT(Z, MAX);
- #if HAS_Z2_MAX
- UPDATE_ENDSTOP_BIT(Z2, MAX);
- #else
- COPY_BIT(current_endstop_bits, Z_MAX, Z2_MAX);
- #endif
- test_dual_z_endstops(Z_MAX, Z2_MAX);
- // If this pin is not hijacked for the bed probe
- // then it belongs to the Z endstop
- #elif DISABLED(Z_MIN_PROBE_ENDSTOP) || Z_MAX_PIN != Z_MIN_PROBE_PIN
- UPDATE_ENDSTOP(Z, MAX);
- #endif
- #endif
- }
+ #endif
}
+ else { // +direction
+ #if HAS_Y_MAX
+ #if ENABLED(Y_DUAL_ENDSTOPS)
+ UPDATE_ENDSTOP_BIT(Y, MAX);
+ #if HAS_Y2_MAX
+ UPDATE_ENDSTOP_BIT(Y2, MAX);
+ #else
+ COPY_BIT(current_endstop_bits, Y_MAX, Y2_MAX);
+ #endif
+ test_dual_y_endstops(Y_MAX, Y2_MAX);
+ #else
+ UPDATE_ENDSTOP(Y, MAX);
+ #endif
+ #endif
+ }
+ }
- } // stepper.current_block
-
- old_endstop_bits = current_endstop_bits;
+ if (Z_MOVE_TEST) {
+ if (stepper.motor_direction(Z_AXIS_HEAD)) { // Z -direction. Gantry down, bed up.
+ #if HAS_Z_MIN
+ #if ENABLED(Z_DUAL_ENDSTOPS)
+ UPDATE_ENDSTOP_BIT(Z, MIN);
+ #if HAS_Z2_MIN
+ UPDATE_ENDSTOP_BIT(Z2, MIN);
+ #else
+ COPY_BIT(current_endstop_bits, Z_MIN, Z2_MIN);
+ #endif
+ test_dual_z_endstops(Z_MIN, Z2_MIN);
+ #else
+ #if ENABLED(Z_MIN_PROBE_USES_Z_MIN_ENDSTOP_PIN)
+ if (z_probe_enabled) UPDATE_ENDSTOP(Z, MIN);
+ #else
+ UPDATE_ENDSTOP(Z, MIN);
+ #endif
+ #endif
+ #endif
+ // When closing the gap check the enabled probe
+ #if ENABLED(Z_MIN_PROBE_ENDSTOP)
+ if (z_probe_enabled) {
+ UPDATE_ENDSTOP(Z, MIN_PROBE);
+ if (TEST_ENDSTOP(Z_MIN_PROBE)) SBI(endstop_hit_bits, Z_MIN_PROBE);
+ }
+ #endif
+ }
+ else { // Z +direction. Gantry up, bed down.
+ #if HAS_Z_MAX
+ // Check both Z dual endstops
+ #if ENABLED(Z_DUAL_ENDSTOPS)
+ UPDATE_ENDSTOP_BIT(Z, MAX);
+ #if HAS_Z2_MAX
+ UPDATE_ENDSTOP_BIT(Z2, MAX);
+ #else
+ COPY_BIT(current_endstop_bits, Z_MAX, Z2_MAX);
+ #endif
+ test_dual_z_endstops(Z_MAX, Z2_MAX);
+ // If this pin is not hijacked for the bed probe
+ // then it belongs to the Z endstop
+ #elif DISABLED(Z_MIN_PROBE_ENDSTOP) || Z_MAX_PIN != Z_MIN_PROBE_PIN
+ UPDATE_ENDSTOP(Z, MAX);
+ #endif
+ #endif
+ }
+ }
} // Endstops::update()
diff --git a/Marlin/endstops.h b/Marlin/endstops.h
index 96cb3d089..903a407cd 100644
--- a/Marlin/endstops.h
+++ b/Marlin/endstops.h
@@ -27,15 +27,30 @@
#ifndef __ENDSTOPS_H__
#define __ENDSTOPS_H__
-#include "enum.h"
#include "MarlinConfig.h"
+enum EndstopEnum : char {
+ X_MIN,
+ Y_MIN,
+ Z_MIN,
+ Z_MIN_PROBE,
+ X_MAX,
+ Y_MAX,
+ Z_MAX,
+ X2_MIN,
+ X2_MAX,
+ Y2_MIN,
+ Y2_MAX,
+ Z2_MIN,
+ Z2_MAX
+};
+
class Endstops {
public:
static bool enabled, enabled_globally;
- static volatile char endstop_hit_bits; // use X_MIN, Y_MIN, Z_MIN and Z_MIN_PROBE as BIT value
+ static volatile uint8_t endstop_hit_bits; // use X_MIN, Y_MIN, Z_MIN and Z_MIN_PROBE as BIT value
#if ENABLED(X_DUAL_ENDSTOPS) || ENABLED(Y_DUAL_ENDSTOPS) || ENABLED(Z_DUAL_ENDSTOPS)
typedef uint16_t esbits_t;
@@ -52,23 +67,26 @@ class Endstops {
typedef byte esbits_t;
#endif
- static esbits_t current_endstop_bits, old_endstop_bits;
+ static esbits_t current_endstop_bits;
- Endstops() {
- enable_globally(
- #if ENABLED(ENDSTOPS_ALWAYS_ON_DEFAULT)
- true
- #else
- false
- #endif
- );
- };
+ Endstops() {};
/**
* Initialize the endstop pins
*/
static void init();
+ /**
+ * A change was detected or presumed to be in endstops pins. Find out what
+ * changed, if anything. Called from ISR contexts
+ */
+ static void check_possible_change();
+
+ /**
+ * Periodic call to poll endstops if required. Called from temperature ISR
+ */
+ static void poll();
+
/**
* Update the endstops bits from the pins
*/
@@ -85,21 +103,28 @@ class Endstops {
static void M119();
// Enable / disable endstop checking globally
- static void enable_globally(bool onoff=true) { enabled_globally = enabled = onoff; }
+ static void enable_globally(const bool onoff=true);
// Enable / disable endstop checking
- static void enable(bool onoff=true) { enabled = onoff; }
+ static void enable(const bool onoff=true);
// Disable / Enable endstops based on ENSTOPS_ONLY_FOR_HOMING and global enable
- static void not_homing() { enabled = enabled_globally; }
+ static void not_homing();
// Clear endstops (i.e., they were hit intentionally) to suppress the report
- static void hit_on_purpose() { endstop_hit_bits = 0; }
+ static void hit_on_purpose();
// Enable / disable endstop z-probe checking
#if HAS_BED_PROBE
static volatile bool z_probe_enabled;
- static void enable_z_probe(bool onoff=true) { z_probe_enabled = onoff; }
+ static void enable_z_probe(bool onoff=true);
+ #endif
+
+ // Debugging of endstops
+ #if ENABLED(PINS_DEBUGGING)
+ static bool monitor_flag;
+ static void monitor();
+ static void run_monitor();
#endif
private:
@@ -117,10 +142,4 @@ class Endstops {
extern Endstops endstops;
-#if HAS_BED_PROBE
- #define ENDSTOPS_ENABLED (endstops.enabled || endstops.z_probe_enabled)
-#else
- #define ENDSTOPS_ENABLED endstops.enabled
-#endif
-
#endif // __ENDSTOPS_H__
diff --git a/Marlin/enum.h b/Marlin/enum.h
index 378e47f32..1d5f0fcfb 100644
--- a/Marlin/enum.h
+++ b/Marlin/enum.h
@@ -88,22 +88,6 @@ enum DebugFlags : unsigned char {
DEBUG_ALL = 0xFF
};
-enum EndstopEnum : char {
- X_MIN,
- Y_MIN,
- Z_MIN,
- Z_MIN_PROBE,
- X_MAX,
- Y_MAX,
- Z_MAX,
- X2_MIN,
- X2_MAX,
- Y2_MIN,
- Y2_MAX,
- Z2_MIN,
- Z2_MAX
-};
-
#if ENABLED(ADVANCED_PAUSE_FEATURE)
enum AdvancedPauseMenuResponse : char {
ADVANCED_PAUSE_RESPONSE_WAIT_FOR,
diff --git a/Marlin/fastio.h b/Marlin/fastio.h
index 03a3a9036..839db9929 100644
--- a/Marlin/fastio.h
+++ b/Marlin/fastio.h
@@ -28,7 +28,6 @@
#include
-typedef int8_t pin_t;
#ifndef _FASTIO_ARDUINO_H_
#define _FASTIO_ARDUINO_H_
diff --git a/Marlin/macros.h b/Marlin/macros.h
index 6b8e87812..1f72ba38d 100644
--- a/Marlin/macros.h
+++ b/Marlin/macros.h
@@ -47,12 +47,6 @@
#define _O2 __attribute__((optimize("O2")))
#define _O3 __attribute__((optimize("O3")))
-// Bracket code that shouldn't be interrupted
-#ifndef CRITICAL_SECTION_START
- #define CRITICAL_SECTION_START unsigned char _sreg = SREG; cli();
- #define CRITICAL_SECTION_END SREG = _sreg;
-#endif
-
// Clock speed factors
#define CYCLES_PER_MICROSECOND (F_CPU / 1000000L) // 16 or 20
#define INT0_PRESCALER 8
diff --git a/Marlin/planner.cpp b/Marlin/planner.cpp
index 4d4b92e9d..ffbee93ad 100644
--- a/Marlin/planner.cpp
+++ b/Marlin/planner.cpp
@@ -56,6 +56,10 @@
*
* IntersectionDistance[s1_, s2_, a_, d_] := (2 a d - s1^2 + s2^2)/(4 a)
*
+ * --
+ *
+ * The fast inverse function needed for Bézier interpolation for AVR
+ * was designed, written and tested by Eduardo José Tagle on April/2018
*/
#include "planner.h"
@@ -77,6 +81,10 @@
#include "power.h"
#endif
+// Delay for delivery of first block to the stepper ISR, if the queue contains 2 or
+// fewer movements. The delay is measured in milliseconds, and must be less than 250ms
+#define BLOCK_DELAY_FOR_1ST_MOVE 50
+
Planner planner;
// public:
@@ -85,13 +93,20 @@ Planner planner;
* A ring buffer of moves described in steps
*/
block_t Planner::block_buffer[BLOCK_BUFFER_SIZE];
-volatile uint8_t Planner::block_buffer_head, // Index of the next block to be pushed
- Planner::block_buffer_tail;
+volatile uint8_t Planner::block_buffer_head, // Index of the next block to be pushed
+ Planner::block_buffer_tail; // Index of the busy block, if any
+uint16_t Planner::cleaning_buffer_counter; // A counter to disable queuing of blocks
+uint8_t Planner::delay_before_delivering, // This counter delays delivery of blocks when queue becomes empty to allow the opportunity of merging blocks
+ Planner::block_buffer_planned; // Index of the optimally planned block
-float Planner::max_feedrate_mm_s[XYZE_N], // Max speeds in mm per second
+float Planner::max_feedrate_mm_s[XYZE_N], // Max speeds in mm per second
Planner::axis_steps_per_mm[XYZE_N],
Planner::steps_to_mm[XYZE_N];
+#if ENABLED(ABORT_ON_ENDSTOP_HIT_FEATURE_ENABLED)
+ bool Planner::abort_on_endstop_hit = false;
+#endif
+
#if ENABLED(DISTINCT_E_FACTORS)
uint8_t Planner::last_extruder = 0; // Respond to extruder change
#endif
@@ -160,7 +175,7 @@ int32_t Planner::position[NUM_AXIS] = { 0 };
uint32_t Planner::cutoff_long;
float Planner::previous_speed[NUM_AXIS],
- Planner::previous_nominal_speed;
+ Planner::previous_nominal_speed_sqr;
#if ENABLED(DISABLE_INACTIVE_EXTRUDER)
uint8_t Planner::g_uc_extruder_last_move[EXTRUDERS] = { 0 };
@@ -197,11 +212,13 @@ void Planner::init() {
ZERO(position_float);
#endif
ZERO(previous_speed);
- previous_nominal_speed = 0.0;
+ previous_nominal_speed_sqr = 0.0;
#if ABL_PLANAR
bed_level_matrix.set_to_identity();
#endif
clear_block_buffer();
+ block_buffer_planned = 0;
+ delay_before_delivering = 0;
}
#if ENABLED(BEZIER_JERK_CONTROL)
@@ -347,7 +364,7 @@ void Planner::init() {
//
static uint32_t get_period_inverse(uint32_t d) {
- static const uint8_t inv_tab[256] PROGMEM = {
+ static const uint8_t inv_tab[256] PROGMEM = {
255,253,252,250,248,246,244,242,240,238,236,234,233,231,229,227,
225,224,222,220,218,217,215,213,212,210,208,207,205,203,202,200,
199,197,195,194,192,191,189,188,186,185,183,182,180,179,178,176,
@@ -520,7 +537,7 @@ void Planner::init() {
A("rjmp 6f") // No, skip it
A("mov %14,%15")
A("clr %15")
- L("6") // %16:%15:%14 = initial estimation of 0x1000000 / d)
+ L("6") // %16:%15:%14 = initial estimation of 0x1000000 / d
// Now, we must refine the estimation present on %16:%15:%14 using 1 iteration
// of Newton-Raphson. As it has a quadratic convergence, 1 iteration is enough
@@ -709,7 +726,6 @@ void Planner::init() {
// Return the result
return r11 | (uint16_t(r12) << 8) | (uint32_t(r13) << 16);
}
-
#endif // BEZIER_JERK_CONTROL
#define MINIMAL_STEP_RATE 120
@@ -719,12 +735,13 @@ void Planner::init() {
* by the provided factors.
*/
void Planner::calculate_trapezoid_for_block(block_t* const block, const float &entry_factor, const float &exit_factor) {
+
uint32_t initial_rate = CEIL(block->nominal_rate * entry_factor),
final_rate = CEIL(block->nominal_rate * exit_factor); // (steps per second)
// Limit minimal step rate (Otherwise the timer will overflow.)
- NOLESS(initial_rate, MINIMAL_STEP_RATE);
- NOLESS(final_rate, MINIMAL_STEP_RATE);
+ NOLESS(initial_rate, uint32_t(MINIMAL_STEP_RATE));
+ NOLESS(final_rate, uint32_t(MINIMAL_STEP_RATE));
#if ENABLED(BEZIER_JERK_CONTROL)
uint32_t cruise_rate = initial_rate;
@@ -733,19 +750,18 @@ void Planner::calculate_trapezoid_for_block(block_t* const block, const float &e
const int32_t accel = block->acceleration_steps_per_s2;
// Steps required for acceleration, deceleration to/from nominal rate
- int32_t accelerate_steps = CEIL(estimate_acceleration_distance(initial_rate, block->nominal_rate, accel)),
- decelerate_steps = FLOOR(estimate_acceleration_distance(block->nominal_rate, final_rate, -accel)),
+ uint32_t accelerate_steps = CEIL(estimate_acceleration_distance(initial_rate, block->nominal_rate, accel)),
+ decelerate_steps = FLOOR(estimate_acceleration_distance(block->nominal_rate, final_rate, -accel));
// Steps between acceleration and deceleration, if any
- plateau_steps = block->step_event_count - accelerate_steps - decelerate_steps;
+ int32_t plateau_steps = block->step_event_count - accelerate_steps - decelerate_steps;
// Does accelerate_steps + decelerate_steps exceed step_event_count?
// Then we can't possibly reach the nominal rate, there will be no cruising.
// Use intersection_distance() to calculate accel / braking time in order to
// reach the final_rate exactly at the end of this block.
if (plateau_steps < 0) {
- accelerate_steps = CEIL(intersection_distance(initial_rate, final_rate, accel, block->step_event_count));
- NOLESS(accelerate_steps, 0); // Check limits due to numerical round-off
- accelerate_steps = MIN((uint32_t)accelerate_steps, block->step_event_count);//(We can cast here to unsigned, because the above line ensures that we are above zero)
+ const float accelerate_steps_float = CEIL(intersection_distance(initial_rate, final_rate, accel, block->step_event_count));
+ accelerate_steps = MIN(uint32_t(MAX(accelerate_steps_float, 0)), block->step_event_count);
plateau_steps = 0;
#if ENABLED(BEZIER_JERK_CONTROL)
@@ -772,8 +788,12 @@ void Planner::calculate_trapezoid_for_block(block_t* const block, const float &e
#endif
- CRITICAL_SECTION_START; // Fill variables used by the stepper in a critical section
- if (!TEST(block->flag, BLOCK_BIT_BUSY)) { // Don't update variables if block is busy.
+ // Fill variables used by the stepper in a critical section
+ const bool was_enabled = STEPPER_ISR_ENABLED();
+ if (was_enabled) DISABLE_STEPPER_DRIVER_INTERRUPT();
+
+ // Don't update variables if block is busy: It is being interpreted by the planner
+ if (!TEST(block->flag, BLOCK_BIT_BUSY)) {
block->accelerate_until = accelerate_steps;
block->decelerate_after = accelerate_steps + plateau_steps;
block->initial_rate = initial_rate;
@@ -786,32 +806,99 @@ void Planner::calculate_trapezoid_for_block(block_t* const block, const float &e
#endif
block->final_rate = final_rate;
}
- CRITICAL_SECTION_END;
+ if (was_enabled) ENABLE_STEPPER_DRIVER_INTERRUPT();
}
-// "Junction jerk" in this context is the immediate change in speed at the junction of two blocks.
-// This method will calculate the junction jerk as the euclidean distance between the nominal
-// velocities of the respective blocks.
-//inline float junction_jerk(block_t *before, block_t *after) {
-// return SQRT(
-// POW((before->speed_x-after->speed_x), 2)+POW((before->speed_y-after->speed_y), 2));
-//}
+/* PLANNER SPEED DEFINITION
+ +--------+ <- current->nominal_speed
+ / \
+ current->entry_speed -> + \
+ | + <- next->entry_speed (aka exit speed)
+ +-------------+
+ time -->
+
+ Recalculates the motion plan according to the following basic guidelines:
+
+ 1. Go over every feasible block sequentially in reverse order and calculate the junction speeds
+ (i.e. current->entry_speed) such that:
+ a. No junction speed exceeds the pre-computed maximum junction speed limit or nominal speeds of
+ neighboring blocks.
+ b. A block entry speed cannot exceed one reverse-computed from its exit speed (next->entry_speed)
+ with a maximum allowable deceleration over the block travel distance.
+ c. The last (or newest appended) block is planned from a complete stop (an exit speed of zero).
+ 2. Go over every block in chronological (forward) order and dial down junction speed values if
+ a. The exit speed exceeds the one forward-computed from its entry speed with the maximum allowable
+ acceleration over the block travel distance.
+
+ When these stages are complete, the planner will have maximized the velocity profiles throughout the all
+ of the planner blocks, where every block is operating at its maximum allowable acceleration limits. In
+ other words, for all of the blocks in the planner, the plan is optimal and no further speed improvements
+ are possible. If a new block is added to the buffer, the plan is recomputed according to the said
+ guidelines for a new optimal plan.
+
+ To increase computational efficiency of these guidelines, a set of planner block pointers have been
+ created to indicate stop-compute points for when the planner guidelines cannot logically make any further
+ changes or improvements to the plan when in normal operation and new blocks are streamed and added to the
+ planner buffer. For example, if a subset of sequential blocks in the planner have been planned and are
+ bracketed by junction velocities at their maximums (or by the first planner block as well), no new block
+ added to the planner buffer will alter the velocity profiles within them. So we no longer have to compute
+ them. Or, if a set of sequential blocks from the first block in the planner (or a optimal stop-compute
+ point) are all accelerating, they are all optimal and can not be altered by a new block added to the
+ planner buffer, as this will only further increase the plan speed to chronological blocks until a maximum
+ junction velocity is reached. However, if the operational conditions of the plan changes from infrequently
+ used feed holds or feedrate overrides, the stop-compute pointers will be reset and the entire plan is
+ recomputed as stated in the general guidelines.
+
+ Planner buffer index mapping:
+ - block_buffer_tail: Points to the beginning of the planner buffer. First to be executed or being executed.
+ - block_buffer_head: Points to the buffer block after the last block in the buffer. Used to indicate whether
+ the buffer is full or empty. As described for standard ring buffers, this block is always empty.
+ - block_buffer_planned: Points to the first buffer block after the last optimally planned block for normal
+ streaming operating conditions. Use for planning optimizations by avoiding recomputing parts of the
+ planner buffer that don't change with the addition of a new block, as describe above. In addition,
+ this block can never be less than block_buffer_tail and will always be pushed forward and maintain
+ this requirement when encountered by the plan_discard_current_block() routine during a cycle.
+
+ NOTE: Since the planner only computes on what's in the planner buffer, some motions with lots of short
+ line segments, like G2/3 arcs or complex curves, may seem to move slow. This is because there simply isn't
+ enough combined distance traveled in the entire buffer to accelerate up to the nominal speed and then
+ decelerate to a complete stop at the end of the buffer, as stated by the guidelines. If this happens and
+ becomes an annoyance, there are a few simple solutions: (1) Maximize the machine acceleration. The planner
+ will be able to compute higher velocity profiles within the same combined distance. (2) Maximize line
+ motion(s) distance per block to a desired tolerance. The more combined distance the planner has to use,
+ the faster it can go. (3) Maximize the planner buffer size. This also will increase the combined distance
+ for the planner to compute over. It also increases the number of computations the planner has to perform
+ to compute an optimal plan, so select carefully.
+*/
// The kernel called by recalculate() when scanning the plan from last to first entry.
-void Planner::reverse_pass_kernel(block_t* const current, const block_t* const next) {
- if (current && next) {
- // If entry speed is already at the maximum entry speed, no need to recheck. Block is cruising.
- // If not, block in state of acceleration or deceleration. Reset entry speed to maximum and
- // check for maximum allowable speed reductions to ensure maximum possible planned speed.
- const float max_entry_speed = current->max_entry_speed;
- if (current->entry_speed != max_entry_speed || TEST(next->flag, BLOCK_BIT_RECALCULATE)) {
- // If nominal length true, max junction speed is guaranteed to be reached. Only compute
- // for max allowable speed if block is decelerating and nominal length is false.
- const float new_entry_speed = (TEST(current->flag, BLOCK_BIT_NOMINAL_LENGTH) || max_entry_speed <= next->entry_speed)
- ? max_entry_speed
- : MIN(max_entry_speed, max_allowable_speed(-current->acceleration, next->entry_speed, current->millimeters));
- if (new_entry_speed != current->entry_speed) {
- current->entry_speed = new_entry_speed;
+void Planner::reverse_pass_kernel(block_t* const current, const block_t * const next) {
+ if (current) {
+ // If entry speed is already at the maximum entry speed, and there was no change of speed
+ // in the next block, there is no need to recheck. Block is cruising and there is no need to
+ // compute anything for this block,
+ // If not, block entry speed needs to be recalculated to ensure maximum possible planned speed.
+ const float max_entry_speed_sqr = current->max_entry_speed_sqr;
+
+ // Compute maximum entry speed decelerating over the current block from its exit speed.
+ // If not at the maximum entry speed, or the previous block entry speed changed
+ if (current->entry_speed_sqr != max_entry_speed_sqr || (next && TEST(next->flag, BLOCK_BIT_RECALCULATE))) {
+
+ // If nominal length true, max junction speed is guaranteed to be reached.
+ // If a block can de/ac-celerate from nominal speed to zero within the length of the block, then
+ // the current block and next block junction speeds are guaranteed to always be at their maximum
+ // junction speeds in deceleration and acceleration, respectively. This is due to how the current
+ // block nominal speed limits both the current and next maximum junction speeds. Hence, in both
+ // the reverse and forward planners, the corresponding block junction speed will always be at the
+ // the maximum junction speed and may always be ignored for any speed reduction checks.
+
+ const float new_entry_speed_sqr = TEST(current->flag, BLOCK_BIT_NOMINAL_LENGTH)
+ ? max_entry_speed_sqr
+ : MIN(max_entry_speed_sqr, max_allowable_speed_sqr(-current->acceleration, next ? next->entry_speed_sqr : sq(MINIMUM_PLANNER_SPEED), current->millimeters));
+ if (current->entry_speed_sqr != new_entry_speed_sqr) {
+ current->entry_speed_sqr = new_entry_speed_sqr;
+
+ // Need to recalculate the block speed
SBI(current->flag, BLOCK_BIT_RECALCULATE);
}
}
@@ -823,51 +910,72 @@ void Planner::reverse_pass_kernel(block_t* const current, const block_t* const n
* Once in reverse and once forward. This implements the reverse pass.
*/
void Planner::reverse_pass() {
- if (movesplanned() > 2) {
- const uint8_t endnr = next_block_index(block_buffer_tail); // tail is running. tail+1 shouldn't be altered because it's connected to the running block.
- uint8_t blocknr = prev_block_index(block_buffer_head);
- block_t* current = &block_buffer[blocknr];
+ // Initialize block index to the last block in the planner buffer.
+ uint8_t block_index = prev_block_index(block_buffer_head);
- // Last/newest block in buffer:
- const float max_entry_speed = current->max_entry_speed;
- if (current->entry_speed != max_entry_speed) {
- // If nominal length true, max junction speed is guaranteed to be reached. Only compute
- // for max allowable speed if block is decelerating and nominal length is false.
- const float new_entry_speed = TEST(current->flag, BLOCK_BIT_NOMINAL_LENGTH)
- ? max_entry_speed
- : MIN(max_entry_speed, max_allowable_speed(-current->acceleration, MINIMUM_PLANNER_SPEED, current->millimeters));
- if (current->entry_speed != new_entry_speed) {
- current->entry_speed = new_entry_speed;
- SBI(current->flag, BLOCK_BIT_RECALCULATE);
- }
+ // Read the index of the last buffer planned block.
+ // The ISR may change it so get a stable local copy.
+ uint8_t planned_block_index = block_buffer_planned;
+
+ // If there was a race condition and block_buffer_planned was incremented
+ // or was pointing at the head (queue empty) break loop now and avoid
+ // planning already consumed blocks
+ if (planned_block_index == block_buffer_head) return;
+
+ // Reverse Pass: Coarsely maximize all possible deceleration curves back-planning from the last
+ // block in buffer. Cease planning when the last optimal planned or tail pointer is reached.
+ // NOTE: Forward pass will later refine and correct the reverse pass to create an optimal plan.
+ block_t *current;
+ const block_t *next = NULL;
+ while (block_index != planned_block_index) {
+
+ // Perform the reverse pass
+ current = &block_buffer[block_index];
+
+ // Only consider non sync blocks
+ if (!TEST(current->flag, BLOCK_BIT_SYNC_POSITION)) {
+ reverse_pass_kernel(current, next);
+ next = current;
}
- do {
- const block_t * const next = current;
- blocknr = prev_block_index(blocknr);
- current = &block_buffer[blocknr];
- reverse_pass_kernel(current, next);
- } while (blocknr != endnr);
+ // Advance to the next
+ block_index = prev_block_index(block_index);
}
}
// The kernel called by recalculate() when scanning the plan from first to last entry.
-void Planner::forward_pass_kernel(const block_t* const previous, block_t* const current) {
+void Planner::forward_pass_kernel(const block_t* const previous, block_t* const current, const uint8_t block_index) {
if (previous) {
// If the previous block is an acceleration block, too short to complete the full speed
// change, adjust the entry speed accordingly. Entry speeds have already been reset,
// maximized, and reverse-planned. If nominal length is set, max junction speed is
// guaranteed to be reached. No need to recheck.
- if (!TEST(previous->flag, BLOCK_BIT_NOMINAL_LENGTH)) {
- if (previous->entry_speed < current->entry_speed) {
- const float new_entry_speed = MIN(current->entry_speed, max_allowable_speed(-previous->acceleration, previous->entry_speed, previous->millimeters));
- // Check for junction speed change
- if (current->entry_speed != new_entry_speed) {
- current->entry_speed = new_entry_speed;
- SBI(current->flag, BLOCK_BIT_RECALCULATE);
- }
+ if (!TEST(previous->flag, BLOCK_BIT_NOMINAL_LENGTH) &&
+ previous->entry_speed_sqr < current->entry_speed_sqr) {
+
+ // Compute the maximum allowable speed
+ const float new_entry_speed_sqr = max_allowable_speed_sqr(-previous->acceleration, previous->entry_speed_sqr, previous->millimeters);
+
+ // If true, current block is full-acceleration and we can move the planned pointer forward.
+ if (new_entry_speed_sqr < current->entry_speed_sqr) {
+
+ // Always <= max_entry_speed_sqr. Backward pass sets this.
+ current->entry_speed_sqr = new_entry_speed_sqr; // Always <= max_entry_speed_sqr. Backward pass sets this.
+
+ // Set optimal plan pointer.
+ block_buffer_planned = block_index;
+
+ // And mark we need to recompute the trapezoidal shape
+ SBI(current->flag, BLOCK_BIT_RECALCULATE);
}
}
+
+ // Any block set at its maximum entry speed also creates an optimal plan up to this
+ // point in the buffer. When the plan is bracketed by either the beginning of the
+ // buffer and a maximum entry speed or two maximum entry speeds, every block in between
+ // cannot logically be further improved. Hence, we don't have to recompute them anymore.
+ if (current->entry_speed_sqr == current->max_entry_speed_sqr)
+ block_buffer_planned = block_index;
}
}
@@ -876,15 +984,31 @@ void Planner::forward_pass_kernel(const block_t* const previous, block_t* const
* Once in reverse and once forward. This implements the forward pass.
*/
void Planner::forward_pass() {
- block_t* block[3] = { NULL, NULL, NULL };
- for (uint8_t b = block_buffer_tail; b != block_buffer_head; b = next_block_index(b)) {
- block[0] = block[1];
- block[1] = block[2];
- block[2] = &block_buffer[b];
- forward_pass_kernel(block[0], block[1]);
+ // Forward Pass: Forward plan the acceleration curve from the planned pointer onward.
+ // Also scans for optimal plan breakpoints and appropriately updates the planned pointer.
+
+ // Begin at buffer planned pointer. Note that block_buffer_planned can be modified
+ // by the stepper ISR, so read it ONCE. It it guaranteed that block_buffer_planned
+ // will never lead head, so the loop is safe to execute. Also note that the forward
+ // pass will never modify the values at the tail.
+ uint8_t block_index = block_buffer_planned;
+
+ block_t *current;
+ const block_t * previous = NULL;
+ while (block_index != block_buffer_head) {
+
+ // Perform the forward pass
+ current = &block_buffer[block_index];
+
+ // Skip SYNC blocks
+ if (!TEST(current->flag, BLOCK_BIT_SYNC_POSITION)) {
+ forward_pass_kernel(previous, current, block_index);
+ previous = current;
+ }
+ // Advance to the previous
+ block_index = next_block_index(block_index);
}
- forward_pass_kernel(block[1], block[2]);
}
/**
@@ -893,38 +1017,73 @@ void Planner::forward_pass() {
* recalculate() after updating the blocks.
*/
void Planner::recalculate_trapezoids() {
- int8_t block_index = block_buffer_tail;
- block_t *current, *next = NULL;
+ // The tail may be changed by the ISR so get a local copy.
+ uint8_t block_index = block_buffer_tail;
+
+ // As there could be a sync block in the head of the queue, and the next loop must not
+ // recalculate the head block (as it needs to be specially handled), scan backwards until
+ // we find the first non SYNC block
+ uint8_t head_block_index = block_buffer_head;
+ while (head_block_index != block_index) {
+
+ // Go back (head always point to the first free block)
+ uint8_t prev_index = prev_block_index(head_block_index);
+
+ // Get the pointer to the block
+ block_t *prev = &block_buffer[prev_index];
+
+ // If not dealing with a sync block, we are done. The last block is not a SYNC block
+ if (!TEST(prev->flag, BLOCK_BIT_SYNC_POSITION)) break;
+
+ // Examine the previous block. This and all following are SYNC blocks
+ head_block_index = prev_index;
+ };
+
+ // Go from the tail (currently executed block) to the first block, without including it)
+ block_t *current = NULL, *next = NULL;
+ float current_entry_speed = 0.0, next_entry_speed = 0.0;
+ while (block_index != head_block_index) {
- while (block_index != block_buffer_head) {
- current = next;
next = &block_buffer[block_index];
- if (current) {
- // Recalculate if current block entry or exit junction speed has changed.
- if (TEST(current->flag, BLOCK_BIT_RECALCULATE) || TEST(next->flag, BLOCK_BIT_RECALCULATE)) {
- // NOTE: Entry and exit factors always > 0 by all previous logic operations.
- const float nomr = 1.0 / current->nominal_speed;
- calculate_trapezoid_for_block(current, current->entry_speed * nomr, next->entry_speed * nomr);
- #if ENABLED(LIN_ADVANCE)
- if (current->use_advance_lead) {
- const float comp = current->e_D_ratio * extruder_advance_K * axis_steps_per_mm[E_AXIS];
- current->max_adv_steps = current->nominal_speed * comp;
- current->final_adv_steps = next->entry_speed * comp;
- }
- #endif
- CBI(current->flag, BLOCK_BIT_RECALCULATE); // Reset current only to ensure next trapezoid is computed
+
+ // Skip sync blocks
+ if (!TEST(next->flag, BLOCK_BIT_SYNC_POSITION)) {
+ next_entry_speed = SQRT(next->entry_speed_sqr);
+
+ if (current) {
+ // Recalculate if current block entry or exit junction speed has changed.
+ if (TEST(current->flag, BLOCK_BIT_RECALCULATE) || TEST(next->flag, BLOCK_BIT_RECALCULATE)) {
+ // NOTE: Entry and exit factors always > 0 by all previous logic operations.
+ const float current_nominal_speed = SQRT(current->nominal_speed_sqr),
+ nomr = 1.0 / current_nominal_speed;
+ calculate_trapezoid_for_block(current, current_entry_speed * nomr, next_entry_speed * nomr);
+ #if ENABLED(LIN_ADVANCE)
+ if (current->use_advance_lead) {
+ const float comp = current->e_D_ratio * extruder_advance_K * axis_steps_per_mm[E_AXIS];
+ current->max_adv_steps = current_nominal_speed * comp;
+ current->final_adv_steps = next_entry_speed * comp;
+ }
+ #endif
+ CBI(current->flag, BLOCK_BIT_RECALCULATE); // Reset current only to ensure next trapezoid is computed
+ }
}
+
+ current = next;
+ current_entry_speed = next_entry_speed;
}
+
block_index = next_block_index(block_index);
}
+
// Last/newest block in buffer. Exit speed is set with MINIMUM_PLANNER_SPEED. Always recalculated.
if (next) {
- const float nomr = 1.0 / next->nominal_speed;
- calculate_trapezoid_for_block(next, next->entry_speed * nomr, (MINIMUM_PLANNER_SPEED) * nomr);
+ const float next_nominal_speed = SQRT(next->nominal_speed_sqr),
+ nomr = 1.0 / next_nominal_speed;
+ calculate_trapezoid_for_block(next, next_entry_speed * nomr, (MINIMUM_PLANNER_SPEED) * nomr);
#if ENABLED(LIN_ADVANCE)
if (next->use_advance_lead) {
const float comp = next->e_D_ratio * extruder_advance_K * axis_steps_per_mm[E_AXIS];
- next->max_adv_steps = next->nominal_speed * comp;
+ next->max_adv_steps = next_nominal_speed * comp;
next->final_adv_steps = (MINIMUM_PLANNER_SPEED) * comp;
}
#endif
@@ -932,33 +1091,14 @@ void Planner::recalculate_trapezoids() {
}
}
-/**
- * Recalculate the motion plan according to the following algorithm:
- *
- * 1. Go over every block in reverse order...
- *
- * Calculate a junction speed reduction (block_t.entry_factor) so:
- *
- * a. The junction jerk is within the set limit, and
- *
- * b. No speed reduction within one block requires faster
- * deceleration than the one, true constant acceleration.
- *
- * 2. Go over every block in chronological order...
- *
- * Dial down junction speed reduction values if:
- * a. The speed increase within one block would require faster
- * acceleration than the one, true constant acceleration.
- *
- * After that, all blocks will have an entry_factor allowing all speed changes to
- * be performed using only the one, true constant acceleration, and where no junction
- * jerk is jerkier than the set limit, Jerky. Finally it will:
- *
- * 3. Recalculate "trapezoids" for all blocks.
- */
void Planner::recalculate() {
- reverse_pass();
- forward_pass();
+ // Initialize block index to the last block in the planner buffer.
+ const uint8_t block_index = prev_block_index(block_buffer_head);
+ // If there is just one block, no planning can be done. Avoid it!
+ if (block_index != block_buffer_planned) {
+ reverse_pass();
+ forward_pass();
+ }
recalculate_trapezoids();
}
@@ -974,7 +1114,7 @@ void Planner::recalculate() {
for (uint8_t b = block_buffer_tail; b != block_buffer_head; b = next_block_index(b)) {
block_t* block = &block_buffer[b];
if (block->steps[X_AXIS] || block->steps[Y_AXIS] || block->steps[Z_AXIS]) {
- float se = (float)block->steps[E_AXIS] / block->step_event_count * block->nominal_speed; // mm/sec;
+ const float se = (float)block->steps[E_AXIS] / block->step_event_count * SQRT(block->nominal_speed_sqr); // mm/sec;
NOLESS(high, se);
}
}
@@ -1275,6 +1415,53 @@ void Planner::check_axes_activity() {
#endif // PLANNER_LEVELING
+void Planner::quick_stop() {
+
+ // Remove all the queued blocks. Note that this function is NOT
+ // called from the Stepper ISR, so we must consider tail as readonly!
+ // that is why we set head to tail - But there is a race condition that
+ // must be handled: The tail could change between the read and the assignment
+ // so this must be enclosed in a critical section
+
+ const bool was_enabled = STEPPER_ISR_ENABLED();
+ if (was_enabled) DISABLE_STEPPER_DRIVER_INTERRUPT();
+
+ // Drop all queue entries
+ block_buffer_planned = block_buffer_head = block_buffer_tail;
+
+ // Restart the block delay for the first movement - As the queue was
+ // forced to empty, there's no risk the ISR will touch this.
+ delay_before_delivering = BLOCK_DELAY_FOR_1ST_MOVE;
+
+ #if ENABLED(ULTRA_LCD)
+ // Clear the accumulated runtime
+ clear_block_buffer_runtime();
+ #endif
+
+ // Make sure to drop any attempt of queuing moves for at least 1 second
+ cleaning_buffer_counter = 1000;
+
+ // Reenable Stepper ISR
+ if (was_enabled) ENABLE_STEPPER_DRIVER_INTERRUPT();
+
+ // And stop the stepper ISR
+ stepper.quick_stop();
+}
+
+void Planner::endstop_triggered(const AxisEnum axis) {
+ // Record stepper position and discard the current block
+ stepper.endstop_triggered(axis);
+}
+
+float Planner::triggered_position_mm(const AxisEnum axis) {
+ return stepper.triggered_position(axis) * steps_to_mm[axis];
+}
+
+void Planner::finish_and_disable() {
+ while (has_blocks_queued() || cleaning_buffer_counter) idle();
+ disable_all_steppers();
+}
+
/**
* Get an axis position according to stepper position(s)
* For CORE machines apply translation from ABC to XYZ.
@@ -1287,7 +1474,7 @@ float Planner::get_axis_position_mm(const AxisEnum axis) {
// Protect the access to the position.
const bool was_enabled = STEPPER_ISR_ENABLED();
- DISABLE_STEPPER_DRIVER_INTERRUPT();
+ if (was_enabled) DISABLE_STEPPER_DRIVER_INTERRUPT();
// ((a1+a2)+(a1-a2))/2 -> (a1+a2+a1-a2)/2 -> (a1+a1)/2 -> a1
// ((a1+a2)-(a1-a2))/2 -> (a1+a2-a1+a2)/2 -> (a2+a2)/2 -> a2
@@ -1309,18 +1496,79 @@ float Planner::get_axis_position_mm(const AxisEnum axis) {
/**
* Block until all buffered steps are executed / cleaned
*/
-void Planner::synchronize() { while (has_blocks_queued() || stepper.cleaning_buffer_counter) idle(); }
+void Planner::synchronize() { while (has_blocks_queued() || cleaning_buffer_counter) idle(); }
/**
* Planner::_buffer_steps
*
- * Add a new linear movement to the buffer (in terms of steps).
+ * Add a new linear movement to the planner queue (in terms of steps).
*
* target - target position in steps units
* fr_mm_s - (target) speed of the move
* extruder - target extruder
+ * millimeters - the length of the movement, if known
+ *
+ * Returns true if movement was properly queued, false otherwise
*/
-void Planner::_buffer_steps(const int32_t (&target)[XYZE]
+bool Planner::_buffer_steps(const int32_t (&target)[XYZE]
+ #if HAS_POSITION_FLOAT
+ , const float (&target_float)[XYZE]
+ #endif
+ , float fr_mm_s, const uint8_t extruder, const float &millimeters
+) {
+
+ // If we are cleaning, do not accept queuing of movements
+ if (cleaning_buffer_counter) return false;
+
+ // Wait for the next available block
+ uint8_t next_buffer_head;
+ block_t * const block = get_next_free_block(next_buffer_head);
+
+ // Fill the block with the specified movement
+ if (!_populate_block(block, false, target
+ #if HAS_POSITION_FLOAT
+ , target_float
+ #endif
+ , fr_mm_s, extruder, millimeters
+ )) {
+ // Movement was not queued, probably because it was too short.
+ // Simply accept that as movement queued and done
+ return true;
+ }
+
+ // If this is the first added movement, reload the delay, otherwise, cancel it.
+ if (block_buffer_head == block_buffer_tail) {
+ // If it was the first queued block, restart the 1st block delivery delay, to
+ // give the planner an opportunity to queue more movements and plan them
+ // As there are no queued movements, the Stepper ISR will not touch this
+ // variable, so there is no risk setting this here (but it MUST be done
+ // before the following line!!)
+ delay_before_delivering = BLOCK_DELAY_FOR_1ST_MOVE;
+ }
+
+ // Move buffer head
+ block_buffer_head = next_buffer_head;
+
+ // Recalculate and optimize trapezoidal speed profiles
+ recalculate();
+
+ // Movement successfully queued!
+ return true;
+}
+
+/**
+ * Planner::_populate_block
+ *
+ * Fills a new linear movement in the block (in terms of steps).
+ *
+ * target - target position in steps units
+ * fr_mm_s - (target) speed of the move
+ * extruder - target extruder
+ *
+ * Returns true is movement is acceptable, false otherwise
+ */
+bool Planner::_populate_block(block_t * const block, bool split_move,
+ const int32_t (&target)[XYZE]
#if HAS_POSITION_FLOAT
, const float (&target_float)[XYZE]
#endif
@@ -1334,7 +1582,7 @@ void Planner::_buffer_steps(const int32_t (&target)[XYZE]
int32_t de = target[E_AXIS] - position[E_AXIS];
/* <-- add a slash to enable
- SERIAL_ECHOPAIR(" _buffer_steps FR:", fr_mm_s);
+ SERIAL_ECHOPAIR(" _populate_block FR:", fr_mm_s);
SERIAL_ECHOPAIR(" A:", target[A_AXIS]);
SERIAL_ECHOPAIR(" (", da);
SERIAL_ECHOPAIR(" steps) B:", target[B_AXIS]);
@@ -1401,11 +1649,7 @@ void Planner::_buffer_steps(const int32_t (&target)[XYZE]
if (de < 0) SBI(dm, E_AXIS);
const float esteps_float = de * e_factor[extruder];
- const int32_t esteps = ABS(esteps_float) + 0.5;
-
- // Wait for the next available block
- uint8_t next_buffer_head;
- block_t * const block = get_next_free_block(next_buffer_head);
+ const uint32_t esteps = ABS(esteps_float) + 0.5;
// Clear all flags, including the "busy" bit
block->flag = 0x00;
@@ -1442,7 +1686,7 @@ void Planner::_buffer_steps(const int32_t (&target)[XYZE]
block->step_event_count = MAX4(block->steps[A_AXIS], block->steps[B_AXIS], block->steps[C_AXIS], esteps);
// Bail if this is a zero-length block
- if (block->step_event_count < MIN_STEPS_PER_SEGMENT) return;
+ if (block->step_event_count < MIN_STEPS_PER_SEGMENT) return false;
// For a mixing extruder, get a magnified step_event_count for each
#if ENABLED(MIXING_EXTRUDER)
@@ -1682,12 +1926,16 @@ void Planner::_buffer_steps(const int32_t (&target)[XYZE]
#endif
#if ENABLED(ULTRA_LCD)
- CRITICAL_SECTION_START
- block_buffer_runtime_us += segment_time_us;
- CRITICAL_SECTION_END
+ // Protect the access to the position.
+ const bool was_enabled = STEPPER_ISR_ENABLED();
+ if (was_enabled) DISABLE_STEPPER_DRIVER_INTERRUPT();
+
+ block_buffer_runtime_us += segment_time_us;
+
+ if (was_enabled) ENABLE_STEPPER_DRIVER_INTERRUPT();
#endif
- block->nominal_speed = block->millimeters * inverse_secs; // (mm/sec) Always > 0
+ block->nominal_speed_sqr = sq(block->millimeters * inverse_secs); // (mm/sec)^2 Always > 0
block->nominal_rate = CEIL(block->step_event_count * inverse_secs); // (step/sec) Always > 0
#if ENABLED(FILAMENT_WIDTH_SENSOR)
@@ -1775,8 +2023,8 @@ void Planner::_buffer_steps(const int32_t (&target)[XYZE]
// Correct the speed
if (speed_factor < 1.0) {
LOOP_XYZE(i) current_speed[i] *= speed_factor;
- block->nominal_speed *= speed_factor;
block->nominal_rate *= speed_factor;
+ block->nominal_speed_sqr = block->nominal_speed_sqr * sq(speed_factor);
}
// Compute and limit the acceleration rate for the trapezoid generator.
@@ -1871,13 +2119,13 @@ void Planner::_buffer_steps(const int32_t (&target)[XYZE]
block->acceleration_steps_per_s2 = accel;
block->acceleration = accel / steps_per_mm;
#if DISABLED(BEZIER_JERK_CONTROL)
- block->acceleration_rate = (long)(accel * (4096.0 * 4096.0 / (HAL_STEPPER_TIMER_RATE))); // * 8.388608
+ block->acceleration_rate = (uint32_t)(accel * (4096.0 * 4096.0 / (HAL_STEPPER_TIMER_RATE)));
#endif
#if ENABLED(LIN_ADVANCE)
if (block->use_advance_lead) {
block->advance_speed = (HAL_STEPPER_TIMER_RATE) / (extruder_advance_K * block->e_D_ratio * block->acceleration * axis_steps_per_mm[E_AXIS_N]);
#if ENABLED(LA_DEBUG)
- if (extruder_advance_K * block->e_D_ratio * block->acceleration * 2 < block->nominal_speed * block->e_D_ratio)
+ if (extruder_advance_K * block->e_D_ratio * block->acceleration * 2 < SQRT(block->nominal_speed_sqr) * block->e_D_ratio)
SERIAL_ECHOLNPGM("More than 2 steps per eISR loop executed.");
if (block->advance_speed < 200)
SERIAL_ECHOLNPGM("eISR running at > 10kHz.");
@@ -1885,7 +2133,7 @@ void Planner::_buffer_steps(const int32_t (&target)[XYZE]
}
#endif
- float vmax_junction; // Initial limit on the segment entry velocity
+ float vmax_junction_sqr; // Initial limit on the segment entry velocity (mm/s)^2
#if ENABLED(JUNCTION_DEVIATION)
@@ -1911,7 +2159,17 @@ void Planner::_buffer_steps(const int32_t (&target)[XYZE]
* changed dynamically during operation nor can the line move geometry. This must be kept in
* memory in the event of a feedrate override changing the nominal speeds of blocks, which can
* change the overall maximum entry speed conditions of all blocks.
- */
+ *
+ * #######
+ * https://github.com/MarlinFirmware/Marlin/issues/10341#issuecomment-388191754
+ *
+ * hoffbaked: on May 10 2018 tuned and improved the GRBL algorithm for Marlin:
+ Okay! It seems to be working good. I somewhat arbitrarily cut it off at 1mm
+ on then on anything with less sides than an octagon. With this, and the
+ reverse pass actually recalculating things, a corner acceleration value
+ of 1000 junction deviation of .05 are pretty reasonable. If the cycles
+ can be spared, a better acos could be used. For all I know, it may be
+ already calculated in a different place. */
// Unit vector of previous path line segment
static float previous_unit_vec[
@@ -1932,7 +2190,7 @@ void Planner::_buffer_steps(const int32_t (&target)[XYZE]
};
// Skip first block or when previous_nominal_speed is used as a flag for homing and offset cycles.
- if (moves_queued && !UNEAR_ZERO(previous_nominal_speed)) {
+ if (moves_queued && !UNEAR_ZERO(previous_nominal_speed_sqr)) {
// Compute cosine of angle between previous and current path. (prev_unit_vec is negative)
// NOTE: Max junction velocity is computed without sin() or acos() by trig half angle identity.
float junction_cos_theta = -previous_unit_vec[X_AXIS] * unit_vec[X_AXIS]
@@ -1946,21 +2204,33 @@ void Planner::_buffer_steps(const int32_t (&target)[XYZE]
// NOTE: Computed without any expensive trig, sin() or acos(), by trig half angle identity of cos(theta).
if (junction_cos_theta > 0.999999) {
// For a 0 degree acute junction, just set minimum junction speed.
- vmax_junction = MINIMUM_PLANNER_SPEED;
+ vmax_junction_sqr = sq(MINIMUM_PLANNER_SPEED);
}
else {
- junction_cos_theta = MAX(junction_cos_theta, -0.999999); // Check for numerical round-off to avoid divide by zero.
+ NOLESS(junction_cos_theta, -0.999999); // Check for numerical round-off to avoid divide by zero.
const float sin_theta_d2 = SQRT(0.5 * (1.0 - junction_cos_theta)); // Trig half angle identity. Always positive.
// TODO: Technically, the acceleration used in calculation needs to be limited by the minimum of the
// two junctions. However, this shouldn't be a significant problem except in extreme circumstances.
- vmax_junction = SQRT((block->acceleration * JUNCTION_DEVIATION_FACTOR * sin_theta_d2) / (1.0 - sin_theta_d2));
+ vmax_junction_sqr = (JUNCTION_ACCELERATION_FACTOR * JUNCTION_DEVIATION_FACTOR * sin_theta_d2) / (1.0 - sin_theta_d2);
+ if (block->millimeters < 1.0) {
+
+ // Fast acos approximation, minus the error bar to be safe
+ const float junction_theta = (RADIANS(-40) * sq(junction_cos_theta) - RADIANS(50)) * junction_cos_theta + RADIANS(90) - 0.18;
+
+ // If angle is greater than 135 degrees (octagon), find speed for approximate arc
+ if (junction_theta > RADIANS(135)) {
+ const float limit_sqr = block->millimeters / (RADIANS(180) - junction_theta) * JUNCTION_ACCELERATION_FACTOR;
+ NOMORE(vmax_junction_sqr, limit_sqr);
+ }
+ }
}
- vmax_junction = MIN3(vmax_junction, block->nominal_speed, previous_nominal_speed);
+ // Get the lowest speed
+ vmax_junction_sqr = MIN3(vmax_junction_sqr, block->nominal_speed_sqr, previous_nominal_speed_sqr);
}
else // Init entry speed to zero. Assume it starts from rest. Planner will correct this later.
- vmax_junction = 0.0;
+ vmax_junction_sqr = 0.0;
COPY(previous_unit_vec, unit_vec);
@@ -1976,13 +2246,15 @@ void Planner::_buffer_steps(const int32_t (&target)[XYZE]
// Exit speed limited by a jerk to full halt of a previous last segment
static float previous_safe_speed;
- float safe_speed = block->nominal_speed;
+ const float nominal_speed = SQRT(block->nominal_speed_sqr);
+ float safe_speed = nominal_speed;
+
uint8_t limited = 0;
LOOP_XYZE(i) {
const float jerk = ABS(current_speed[i]), maxj = max_jerk[i];
if (jerk > maxj) {
if (limited) {
- const float mjerk = maxj * block->nominal_speed;
+ const float mjerk = maxj * nominal_speed;
if (jerk * safe_speed > mjerk) safe_speed = mjerk / jerk;
}
else {
@@ -1992,19 +2264,21 @@ void Planner::_buffer_steps(const int32_t (&target)[XYZE]
}
}
- if (moves_queued && !UNEAR_ZERO(previous_nominal_speed)) {
+ float vmax_junction;
+ if (moves_queued && !UNEAR_ZERO(previous_nominal_speed_sqr)) {
// Estimate a maximum velocity allowed at a joint of two successive segments.
// If this maximum velocity allowed is lower than the minimum of the entry / exit safe velocities,
// then the machine is not coasting anymore and the safe entry / exit velocities shall be used.
- // The junction velocity will be shared between successive segments. Limit the junction velocity to their minimum.
- // Pick the smaller of the nominal speeds. Higher speed shall not be achieved at the junction during coasting.
- vmax_junction = MIN(block->nominal_speed, previous_nominal_speed);
-
// Factor to multiply the previous / current nominal velocities to get componentwise limited velocities.
float v_factor = 1;
limited = 0;
+ // The junction velocity will be shared between successive segments. Limit the junction velocity to their minimum.
+ // Pick the smaller of the nominal speeds. Higher speed shall not be achieved at the junction during coasting.
+ const float previous_nominal_speed = SQRT(previous_nominal_speed_sqr);
+ vmax_junction = MIN(nominal_speed, previous_nominal_speed);
+
// Now limit the jerk in all axes.
const float smaller_speed_factor = vmax_junction / previous_nominal_speed;
LOOP_XYZE(axis) {
@@ -2039,16 +2313,19 @@ void Planner::_buffer_steps(const int32_t (&target)[XYZE]
vmax_junction = safe_speed;
previous_safe_speed = safe_speed;
+ vmax_junction_sqr = sq(vmax_junction);
+
#endif // Classic Jerk Limiting
// Max entry speed of this block equals the max exit speed of the previous block.
- block->max_entry_speed = vmax_junction;
+ block->max_entry_speed_sqr = vmax_junction_sqr;
// Initialize block entry speed. Compute based on deceleration to user-defined MINIMUM_PLANNER_SPEED.
- const float v_allowable = max_allowable_speed(-block->acceleration, MINIMUM_PLANNER_SPEED, block->millimeters);
- // If stepper ISR is disabled, this indicates buffer_segment wants to add a split block.
- // In this case start with the max. allowed speed to avoid an interrupted first move.
- block->entry_speed = STEPPER_ISR_ENABLED() ? MINIMUM_PLANNER_SPEED : MIN(vmax_junction, v_allowable);
+ const float v_allowable_sqr = max_allowable_speed_sqr(-block->acceleration, sq(MINIMUM_PLANNER_SPEED), block->millimeters);
+
+ // If we are trying to add a split block, start with the
+ // max. allowed speed to avoid an interrupted first move.
+ block->entry_speed_sqr = !split_move ? sq(MINIMUM_PLANNER_SPEED) : MIN(vmax_junction_sqr, v_allowable_sqr);
// Initialize planner efficiency flags
// Set flag if block will always reach maximum junction speed regardless of entry/exit speeds.
@@ -2058,25 +2335,22 @@ void Planner::_buffer_steps(const int32_t (&target)[XYZE]
// block nominal speed limits both the current and next maximum junction speeds. Hence, in both
// the reverse and forward planners, the corresponding block junction speed will always be at the
// the maximum junction speed and may always be ignored for any speed reduction checks.
- block->flag |= block->nominal_speed <= v_allowable ? BLOCK_FLAG_RECALCULATE | BLOCK_FLAG_NOMINAL_LENGTH : BLOCK_FLAG_RECALCULATE;
+ block->flag |= block->nominal_speed_sqr <= v_allowable_sqr ? BLOCK_FLAG_RECALCULATE | BLOCK_FLAG_NOMINAL_LENGTH : BLOCK_FLAG_RECALCULATE;
// Update previous path unit_vector and nominal speed
COPY(previous_speed, current_speed);
- previous_nominal_speed = block->nominal_speed;
+ previous_nominal_speed_sqr = block->nominal_speed_sqr;
- // Move buffer head
- block_buffer_head = next_buffer_head;
-
- // Update the position (only when a move was queued)
+ // Update the position
static_assert(COUNT(target) > 1, "Parameter to _buffer_steps must be (&target)[XYZE]!");
COPY(position, target);
#if HAS_POSITION_FLOAT
COPY(position_float, target_float);
#endif
- recalculate();
-
-} // _buffer_steps()
+ // Movement was accepted
+ return true;
+} // _populate_block()
/**
* Planner::buffer_sync_block
@@ -2087,31 +2361,28 @@ void Planner::buffer_sync_block() {
uint8_t next_buffer_head;
block_t * const block = get_next_free_block(next_buffer_head);
+ // Clear block
+ memset(block, 0, sizeof(block_t));
+
block->flag = BLOCK_FLAG_SYNC_POSITION;
- block->steps[A_AXIS] = position[A_AXIS];
- block->steps[B_AXIS] = position[B_AXIS];
- block->steps[C_AXIS] = position[C_AXIS];
- block->steps[E_AXIS] = position[E_AXIS];
+ block->position[A_AXIS] = position[A_AXIS];
+ block->position[B_AXIS] = position[B_AXIS];
+ block->position[C_AXIS] = position[C_AXIS];
+ block->position[E_AXIS] = position[E_AXIS];
- #if ENABLED(LIN_ADVANCE)
- block->use_advance_lead = false;
- #endif
-
- block->nominal_speed =
- block->entry_speed =
- block->max_entry_speed =
- block->millimeters =
- block->acceleration = 0;
-
- block->step_event_count =
- block->nominal_rate =
- block->initial_rate =
- block->final_rate =
- block->acceleration_steps_per_s2 =
- block->segment_time_us = 0;
+ // If this is the first added movement, reload the delay, otherwise, cancel it.
+ if (block_buffer_head == block_buffer_tail) {
+ // If it was the first queued block, restart the 1st block delivery delay, to
+ // give the planner an opportunity to queue more movements and plan them
+ // As there are no queued movements, the Stepper ISR will not touch this
+ // variable, so there is no risk setting this here (but it MUST be done
+ // before the following line!!)
+ delay_before_delivering = BLOCK_DELAY_FOR_1ST_MOVE;
+ }
block_buffer_head = next_buffer_head;
+
stepper.wake_up();
} // buffer_sync_block()
@@ -2127,7 +2398,11 @@ void Planner::buffer_sync_block() {
* extruder - target extruder
* millimeters - the length of the movement, if known
*/
-void Planner::buffer_segment(const float &a, const float &b, const float &c, const float &e, const float &fr_mm_s, const uint8_t extruder, const float &millimeters/*=0.0*/) {
+bool Planner::buffer_segment(const float &a, const float &b, const float &c, const float &e, const float &fr_mm_s, const uint8_t extruder, const float &millimeters/*=0.0*/) {
+
+ // If we are cleaning, do not accept queuing of movements
+ if (cleaning_buffer_counter) return false;
+
// When changing extruders recalculate steps corresponding to the E position
#if ENABLED(DISTINCT_E_FACTORS)
if (last_extruder != extruder && axis_steps_per_mm[E_AXIS_N] != axis_steps_per_mm[E_AXIS + last_extruder]) {
@@ -2185,48 +2460,18 @@ void Planner::buffer_segment(const float &a, const float &b, const float &c, con
SERIAL_ECHOLNPGM(")");
//*/
- // Always split the first move into two (if not homing or probing)
- if (!has_blocks_queued()) {
-
- #define _BETWEEN(A) (position[_AXIS(A)] + target[_AXIS(A)]) >> 1
- const int32_t between[ABCE] = { _BETWEEN(A), _BETWEEN(B), _BETWEEN(C), _BETWEEN(E) };
-
- #if HAS_POSITION_FLOAT
- #define _BETWEEN_F(A) (position_float[_AXIS(A)] + target_float[_AXIS(A)]) * 0.5
- const float between_float[ABCE] = { _BETWEEN_F(A), _BETWEEN_F(B), _BETWEEN_F(C), _BETWEEN_F(E) };
- #endif
-
- DISABLE_STEPPER_DRIVER_INTERRUPT();
-
- _buffer_steps(between
- #if HAS_POSITION_FLOAT
- , between_float
- #endif
- , fr_mm_s, extruder, millimeters * 0.5
- );
-
- const uint8_t next = block_buffer_head;
-
- _buffer_steps(target
- #if HAS_POSITION_FLOAT
- , target_float
- #endif
- , fr_mm_s, extruder, millimeters * 0.5
- );
-
- SBI(block_buffer[next].flag, BLOCK_BIT_CONTINUED);
- ENABLE_STEPPER_DRIVER_INTERRUPT();
- }
- else
- _buffer_steps(target
+ // Queue the movement
+ if (
+ !_buffer_steps(target
#if HAS_POSITION_FLOAT
, target_float
#endif
, fr_mm_s, extruder, millimeters
- );
+ )
+ ) return false;
stepper.wake_up();
-
+ return true;
} // buffer_segment()
/**
@@ -2253,7 +2498,7 @@ void Planner::_set_position_mm(const float &a, const float &b, const float &c, c
position_float[C_AXIS] = c;
position_float[E_AXIS] = e;
#endif
- previous_nominal_speed = 0.0; // Resets planner junction speeds. Assumes start from rest.
+ previous_nominal_speed_sqr = 0.0; // Resets planner junction speeds. Assumes start from rest.
ZERO(previous_speed);
buffer_sync_block();
}
@@ -2273,22 +2518,6 @@ void Planner::set_position_mm_kinematic(const float (&cart)[XYZE]) {
#endif
}
-/**
- * Sync from the stepper positions. (e.g., after an interrupted move)
- */
-void Planner::sync_from_steppers() {
- LOOP_XYZE(i) {
- position[i] = stepper.position((AxisEnum)i);
- #if HAS_POSITION_FLOAT
- position_float[i] = position[i] * steps_to_mm[i
- #if ENABLED(DISTINCT_E_FACTORS)
- + (i == E_AXIS ? active_extruder : 0)
- #endif
- ];
- #endif
- }
-}
-
/**
* Setters for planner position (also setting stepper position).
*/
diff --git a/Marlin/planner.h b/Marlin/planner.h
index 35ce6c280..1c4f2a1e8 100644
--- a/Marlin/planner.h
+++ b/Marlin/planner.h
@@ -49,7 +49,7 @@ enum BlockFlagBit : char {
// from a safe speed (in consideration of jerking from zero speed).
BLOCK_BIT_NOMINAL_LENGTH,
- // The block is busy
+ // The block is busy, being interpreted by the stepper ISR
BLOCK_BIT_BUSY,
// The block is segment 2+ of a longer move
@@ -80,24 +80,35 @@ typedef struct {
uint8_t flag; // Block flags (See BlockFlag enum above)
- unsigned char active_extruder; // The extruder to move (if E move)
+ // Fields used by the motion planner to manage acceleration
+ float nominal_speed_sqr, // The nominal speed for this block in (mm/sec)^2
+ entry_speed_sqr, // Entry speed at previous-current junction in (mm/sec)^2
+ max_entry_speed_sqr, // Maximum allowable junction entry speed in (mm/sec)^2
+ millimeters, // The total travel of this block in mm
+ acceleration; // acceleration mm/sec^2
- // Fields used by the Bresenham algorithm for tracing the line
- int32_t steps[NUM_AXIS]; // Step count along each axis
+ union {
+ // Data used by all move blocks
+ struct {
+ // Fields used by the Bresenham algorithm for tracing the line
+ uint32_t steps[NUM_AXIS]; // Step count along each axis
+ };
+ // Data used by all sync blocks
+ struct {
+ int32_t position[NUM_AXIS]; // New position to force when this sync block is executed
+ };
+ };
uint32_t step_event_count; // The number of step events required to complete this block
+ uint8_t active_extruder; // The extruder to move (if E move)
+
#if ENABLED(MIXING_EXTRUDER)
uint32_t mix_event_count[MIXING_STEPPERS]; // Scaled step_event_count for the mixing steppers
#endif
// Settings for the trapezoid generator
- int32_t accelerate_until, // The index of the step event on which to stop acceleration
- decelerate_after; // The index of the step event on which to start decelerating
-
- uint32_t nominal_rate, // The nominal step rate for this block in step_events/sec
- initial_rate, // The jerk-adjusted step rate at start of block
- final_rate, // The minimal rate at exit
- acceleration_steps_per_s2; // acceleration steps/sec^2
+ uint32_t accelerate_until, // The index of the step event on which to stop acceleration
+ decelerate_after; // The index of the step event on which to start decelerating
#if ENABLED(BEZIER_JERK_CONTROL)
uint32_t cruise_rate; // The actual cruise rate to use, between end of the acceleration phase and start of deceleration phase
@@ -106,7 +117,7 @@ typedef struct {
uint32_t acceleration_time_inverse, // Inverse of acceleration and deceleration periods, expressed as integer. Scale depends on CPU being used
deceleration_time_inverse;
#else
- int32_t acceleration_rate; // The acceleration rate used for acceleration calculation
+ uint32_t acceleration_rate; // The acceleration rate used for acceleration calculation
#endif
uint8_t direction_bits; // The direction bit set for this block (refers to *_DIRECTION_BIT in config.h)
@@ -120,12 +131,10 @@ typedef struct {
float e_D_ratio;
#endif
- // Fields used by the motion planner to manage acceleration
- float nominal_speed, // The nominal speed for this block in mm/sec
- entry_speed, // Entry speed at previous-current junction in mm/sec
- max_entry_speed, // Maximum allowable junction entry speed in mm/sec
- millimeters, // The total travel of this block in mm
- acceleration; // acceleration mm/sec^2
+ uint32_t nominal_rate, // The nominal step rate for this block in step_events/sec
+ initial_rate, // The jerk-adjusted step rate at start of block
+ final_rate, // The minimal rate at exit
+ acceleration_steps_per_s2; // acceleration steps/sec^2
#if FAN_COUNT > 0
uint16_t fan_speed[FAN_COUNT];
@@ -162,6 +171,10 @@ class Planner {
static block_t block_buffer[BLOCK_BUFFER_SIZE];
static volatile uint8_t block_buffer_head, // Index of the next block to be pushed
block_buffer_tail; // Index of the busy block, if any
+ static uint16_t cleaning_buffer_counter; // A counter to disable queuing of blocks
+ static uint8_t delay_before_delivering, // This counter delays delivery of blocks when queue becomes empty to allow the opportunity of merging blocks
+ block_buffer_planned; // Index of the optimally planned block
+
#if ENABLED(DISTINCT_E_FACTORS)
static uint8_t last_extruder; // Respond to extruder change
@@ -229,6 +242,10 @@ class Planner {
#endif
#endif
+ #if ENABLED(ABORT_ON_ENDSTOP_HIT_FEATURE_ENABLED)
+ static bool abort_on_endstop_hit;
+ #endif
+
private:
/**
@@ -243,9 +260,9 @@ class Planner {
static float previous_speed[NUM_AXIS];
/**
- * Nominal speed of previous path line segment
+ * Nominal speed of previous path line segment (mm/s)^2
*/
- static float previous_nominal_speed;
+ static float previous_nominal_speed_sqr;
/**
* Limit where 64bit math is necessary for acceleration calculation
@@ -304,15 +321,6 @@ class Planner {
// Manage fans, paste pressure, etc.
static void check_axes_activity();
- /**
- * Number of moves currently in the planner
- */
- FORCE_INLINE static uint8_t movesplanned() { return BLOCK_MOD(block_buffer_head - block_buffer_tail + BLOCK_BUFFER_SIZE); }
-
- FORCE_INLINE static void clear_block_buffer() { block_buffer_head = block_buffer_tail = 0; }
-
- FORCE_INLINE static bool is_full() { return block_buffer_tail == next_block_index(block_buffer_head); }
-
// Update multipliers based on new diameter measurements
static void calculate_volumetric_multipliers();
@@ -420,16 +428,32 @@ class Planner {
#define ARG_Z const float &rz
#endif
+ // Number of moves currently in the planner
+ FORCE_INLINE static uint8_t movesplanned() { return BLOCK_MOD(block_buffer_head - block_buffer_tail); }
+
+ // Remove all blocks from the buffer
+ FORCE_INLINE static void clear_block_buffer() { block_buffer_head = block_buffer_tail = 0; }
+
+ // Check if movement queue is full
+ FORCE_INLINE static bool is_full() { return block_buffer_tail == next_block_index(block_buffer_head); }
+
+ // Get count of movement slots free
+ FORCE_INLINE static uint8_t moves_free() { return BLOCK_BUFFER_SIZE - 1 - movesplanned(); }
+
/**
* Planner::get_next_free_block
*
- * - Get the next head index (passed by reference)
- * - Wait for a space to open up in the planner
- * - Return the head block
+ * - Get the next head indices (passed by reference)
+ * - Wait for the number of spaces to open up in the planner
+ * - Return the first head block
*/
- FORCE_INLINE static block_t* get_next_free_block(uint8_t &next_buffer_head) {
+ FORCE_INLINE static block_t* get_next_free_block(uint8_t &next_buffer_head, const uint8_t count=1) {
+
+ // Wait until there are enough slots free
+ while (moves_free() < count) { idle(); }
+
+ // Return the first available block
next_buffer_head = next_block_index(block_buffer_head);
- while (block_buffer_tail == next_buffer_head) idle(); // while (is_full)
return &block_buffer[block_buffer_head];
}
@@ -442,8 +466,30 @@ class Planner {
* fr_mm_s - (target) speed of the move
* extruder - target extruder
* millimeters - the length of the movement, if known
+ *
+ * Returns true if movement was buffered, false otherwise
*/
- static void _buffer_steps(const int32_t (&target)[XYZE]
+ static bool _buffer_steps(const int32_t (&target)[XYZE]
+ #if HAS_POSITION_FLOAT
+ , const float (&target_float)[XYZE]
+ #endif
+ , float fr_mm_s, const uint8_t extruder, const float &millimeters=0.0
+ );
+
+ /**
+ * Planner::_populate_block
+ *
+ * Fills a new linear movement in the block (in terms of steps).
+ *
+ * target - target position in steps units
+ * fr_mm_s - (target) speed of the move
+ * extruder - target extruder
+ * millimeters - the length of the movement, if known
+ *
+ * Returns true is movement is acceptable, false otherwise
+ */
+ static bool _populate_block(block_t * const block, bool split_move,
+ const int32_t (&target)[XYZE]
#if HAS_POSITION_FLOAT
, const float (&target_float)[XYZE]
#endif
@@ -468,7 +514,7 @@ class Planner {
* extruder - target extruder
* millimeters - the length of the movement, if known
*/
- static void buffer_segment(const float &a, const float &b, const float &c, const float &e, const float &fr_mm_s, const uint8_t extruder, const float &millimeters=0.0);
+ static bool buffer_segment(const float &a, const float &b, const float &c, const float &e, const float &fr_mm_s, const uint8_t extruder, const float &millimeters=0.0);
static void _set_position_mm(const float &a, const float &b, const float &c, const float &e);
@@ -485,11 +531,11 @@ class Planner {
* extruder - target extruder
* millimeters - the length of the movement, if known
*/
- FORCE_INLINE static void buffer_line(ARG_X, ARG_Y, ARG_Z, const float &e, const float &fr_mm_s, const uint8_t extruder, const float millimeters = 0.0) {
+ FORCE_INLINE static bool buffer_line(ARG_X, ARG_Y, ARG_Z, const float &e, const float &fr_mm_s, const uint8_t extruder, const float millimeters = 0.0) {
#if PLANNER_LEVELING && IS_CARTESIAN
apply_leveling(rx, ry, rz);
#endif
- buffer_segment(rx, ry, rz, e, fr_mm_s, extruder, millimeters);
+ return buffer_segment(rx, ry, rz, e, fr_mm_s, extruder, millimeters);
}
/**
@@ -502,7 +548,7 @@ class Planner {
* extruder - target extruder
* millimeters - the length of the movement, if known
*/
- FORCE_INLINE static void buffer_line_kinematic(const float (&cart)[XYZE], const float &fr_mm_s, const uint8_t extruder, const float millimeters = 0.0) {
+ FORCE_INLINE static bool buffer_line_kinematic(const float (&cart)[XYZE], const float &fr_mm_s, const uint8_t extruder, const float millimeters = 0.0) {
#if PLANNER_LEVELING
float raw[XYZ] = { cart[X_AXIS], cart[Y_AXIS], cart[Z_AXIS] };
apply_leveling(raw);
@@ -511,9 +557,9 @@ class Planner {
#endif
#if IS_KINEMATIC
inverse_kinematics(raw);
- buffer_segment(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], cart[E_AXIS], fr_mm_s, extruder, millimeters);
+ return buffer_segment(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], cart[E_AXIS], fr_mm_s, extruder, millimeters);
#else
- buffer_segment(raw[X_AXIS], raw[Y_AXIS], raw[Z_AXIS], cart[E_AXIS], fr_mm_s, extruder, millimeters);
+ return buffer_segment(raw[X_AXIS], raw[Y_AXIS], raw[Z_AXIS], cart[E_AXIS], fr_mm_s, extruder, millimeters);
#endif
}
@@ -537,11 +583,6 @@ class Planner {
FORCE_INLINE static void set_z_position_mm(const float &z) { set_position_mm(Z_AXIS, z); }
FORCE_INLINE static void set_e_position_mm(const float &e) { set_position_mm(E_AXIS, e); }
- /**
- * Sync from the stepper positions. (e.g., after an interrupted move)
- */
- static void sync_from_steppers();
-
/**
* Get an axis position according to stepper position(s)
* For CORE machines apply translation from ABC to XYZ.
@@ -553,73 +594,112 @@ class Planner {
FORCE_INLINE static float get_axis_position_degrees(const AxisEnum axis) { return get_axis_position_mm(axis); }
#endif
+ // Called to force a quick stop of the machine (for example, when an emergency
+ // stop is required, or when endstops are hit)
+ static void quick_stop();
+
+ // Called when an endstop is triggered. Causes the machine to stop inmediately
+ static void endstop_triggered(const AxisEnum axis);
+
+ // Triggered position of an axis in mm (not core-savvy)
+ static float triggered_position_mm(const AxisEnum axis);
+
+ // Block until all buffered steps are executed / cleaned
+ static void synchronize();
+
+ // Wait for moves to finish and disable all steppers
+ static void finish_and_disable();
+
+ // Periodic tick to handle cleaning timeouts
+ // Called from the Temperature ISR at ~1kHz
+ static void tick() {
+ if (cleaning_buffer_counter) {
+ --cleaning_buffer_counter;
+ #if ENABLED(SD_FINISHED_STEPPERRELEASE) && defined(SD_FINISHED_RELEASECOMMAND)
+ if (!cleaning_buffer_counter) enqueue_and_echo_commands_P(PSTR(SD_FINISHED_RELEASECOMMAND));
+ #endif
+ }
+ }
+
/**
* Does the buffer have any blocks queued?
*/
FORCE_INLINE static bool has_blocks_queued() { return (block_buffer_head != block_buffer_tail); }
- //
- // Block until all buffered steps are executed
- //
- static void synchronize();
-
- /**
- * "Discard" the block and "release" the memory.
- * Called when the current block is no longer needed.
- */
- FORCE_INLINE static void discard_current_block() {
- if (has_blocks_queued())
- block_buffer_tail = BLOCK_MOD(block_buffer_tail + 1);
- }
-
- /**
- * "Discard" the next block if it's continued.
- * Called after an interrupted move to throw away the rest of the move.
- */
- FORCE_INLINE static bool discard_continued_block() {
- const bool discard = has_blocks_queued() && TEST(block_buffer[block_buffer_tail].flag, BLOCK_BIT_CONTINUED);
- if (discard) discard_current_block();
- return discard;
- }
-
/**
* The current block. NULL if the buffer is empty.
* This also marks the block as busy.
* WARNING: Called from Stepper ISR context!
*/
static block_t* get_current_block() {
- if (has_blocks_queued()) {
+
+ // Get the number of moves in the planner queue so far
+ uint8_t nr_moves = movesplanned();
+
+ // If there are any moves queued ...
+ if (nr_moves) {
+
+ // If there is still delay of delivery of blocks running, decrement it
+ if (delay_before_delivering) {
+ --delay_before_delivering;
+ // If the number of movements queued is less than 3, and there is still time
+ // to wait, do not deliver anything
+ if (nr_moves < 3 && delay_before_delivering) return NULL;
+ delay_before_delivering = 0;
+ }
+
+ // If we are here, there is no excuse to deliver the block
block_t * const block = &block_buffer[block_buffer_tail];
- // If the block has no trapezoid calculated, it's unsafe to execute.
- if (movesplanned() > 1) {
- const block_t * const next = &block_buffer[next_block_index(block_buffer_tail)];
- if (TEST(block->flag, BLOCK_BIT_RECALCULATE) || TEST(next->flag, BLOCK_BIT_RECALCULATE))
- return NULL;
- }
- else if (TEST(block->flag, BLOCK_BIT_RECALCULATE))
- return NULL;
+ // No trapezoid calculated? Don't execute yet.
+ if (TEST(block->flag, BLOCK_BIT_RECALCULATE)) return NULL;
#if ENABLED(ULTRA_LCD)
block_buffer_runtime_us -= block->segment_time_us; // We can't be sure how long an active block will take, so don't count it.
#endif
+
+ // Mark the block as busy, so the planner does not attempt to replan it
SBI(block->flag, BLOCK_BIT_BUSY);
return block;
}
- else {
- #if ENABLED(ULTRA_LCD)
- clear_block_buffer_runtime(); // paranoia. Buffer is empty now - so reset accumulated time to zero.
- #endif
- return NULL;
+
+ // The queue became empty
+ #if ENABLED(ULTRA_LCD)
+ clear_block_buffer_runtime(); // paranoia. Buffer is empty now - so reset accumulated time to zero.
+ #endif
+
+ return NULL;
+ }
+
+ /**
+ * "Discard" the block and "release" the memory.
+ * Called when the current block is no longer needed.
+ * NB: There MUST be a current block to call this function!!
+ */
+ FORCE_INLINE static void discard_current_block() {
+ if (has_blocks_queued()) { // Discard non-empty buffer.
+ uint8_t block_index = next_block_index( block_buffer_tail );
+
+ // Push block_buffer_planned pointer, if encountered.
+ if (!has_blocks_queued()) block_buffer_planned = block_index;
+
+ block_buffer_tail = block_index;
}
}
#if ENABLED(ULTRA_LCD)
static uint16_t block_buffer_runtime() {
- CRITICAL_SECTION_START
- millis_t bbru = block_buffer_runtime_us;
- CRITICAL_SECTION_END
+ // Protect the access to the variable. Only required for AVR, as
+ // any 32bit CPU offers atomic access to 32bit variables
+ bool was_enabled = STEPPER_ISR_ENABLED();
+ if (was_enabled) DISABLE_STEPPER_DRIVER_INTERRUPT();
+
+ millis_t bbru = block_buffer_runtime_us;
+
+ // Reenable Stepper ISR
+ if (was_enabled) ENABLE_STEPPER_DRIVER_INTERRUPT();
+
// To translate µs to ms a division by 1000 would be required.
// We introduce 2.4% error here by dividing by 1024.
// Doesn't matter because block_buffer_runtime_us is already too small an estimation.
@@ -630,9 +710,15 @@ class Planner {
}
static void clear_block_buffer_runtime() {
- CRITICAL_SECTION_START
- block_buffer_runtime_us = 0;
- CRITICAL_SECTION_END
+ // Protect the access to the variable. Only required for AVR, as
+ // any 32bit CPU offers atomic access to 32bit variables
+ bool was_enabled = STEPPER_ISR_ENABLED();
+ if (was_enabled) DISABLE_STEPPER_DRIVER_INTERRUPT();
+
+ block_buffer_runtime_us = 0;
+
+ // Reenable Stepper ISR
+ if (was_enabled) ENABLE_STEPPER_DRIVER_INTERRUPT();
}
#endif
@@ -649,8 +735,8 @@ class Planner {
/**
* Get the index of the next / previous block in the ring buffer
*/
- static constexpr int8_t next_block_index(const int8_t block_index) { return BLOCK_MOD(block_index + 1); }
- static constexpr int8_t prev_block_index(const int8_t block_index) { return BLOCK_MOD(block_index - 1); }
+ static constexpr uint8_t next_block_index(const uint8_t block_index) { return BLOCK_MOD(block_index + 1); }
+ static constexpr uint8_t prev_block_index(const uint8_t block_index) { return BLOCK_MOD(block_index - 1); }
/**
* Calculate the distance (not time) it takes to accelerate
@@ -675,12 +761,12 @@ class Planner {
}
/**
- * Calculate the maximum allowable speed at this point, in order
- * to reach 'target_velocity' using 'acceleration' within a given
+ * Calculate the maximum allowable speed squared at this point, in order
+ * to reach 'target_velocity_sqr' using 'acceleration' within a given
* 'distance'.
*/
- static float max_allowable_speed(const float &accel, const float &target_velocity, const float &distance) {
- return SQRT(sq(target_velocity) - 2 * accel * distance);
+ static float max_allowable_speed_sqr(const float &accel, const float &target_velocity_sqr, const float &distance) {
+ return target_velocity_sqr - 2 * accel * distance;
}
#if ENABLED(BEZIER_JERK_CONTROL)
@@ -695,7 +781,7 @@ class Planner {
static void calculate_trapezoid_for_block(block_t* const block, const float &entry_factor, const float &exit_factor);
static void reverse_pass_kernel(block_t* const current, const block_t * const next);
- static void forward_pass_kernel(const block_t * const previous, block_t* const current);
+ static void forward_pass_kernel(const block_t * const previous, block_t* const current, uint8_t block_index);
static void reverse_pass();
static void forward_pass();
diff --git a/Marlin/planner_bezier.cpp b/Marlin/planner_bezier.cpp
index 6fc80c9ad..5ed7c043c 100644
--- a/Marlin/planner_bezier.cpp
+++ b/Marlin/planner_bezier.cpp
@@ -41,8 +41,7 @@
#define MAX_STEP 0.1
#define SIGMA 0.1
-/* Compute the linear interpolation between to real numbers.
-*/
+// Compute the linear interpolation between two real numbers.
inline static float interp(float a, float b, float t) { return (1.0 - t) * a + t * b; }
/**
@@ -188,12 +187,15 @@ void cubic_b_spline(const float position[NUM_AXIS], const float target[NUM_AXIS]
bez_target[Z_AXIS] = interp(position[Z_AXIS], target[Z_AXIS], t);
bez_target[E_AXIS] = interp(position[E_AXIS], target[E_AXIS], t);
clamp_to_software_endstops(bez_target);
+
#if HAS_UBL_AND_CURVES
float pos[XYZ] = { bez_target[X_AXIS], bez_target[Y_AXIS], bez_target[Z_AXIS] };
planner.apply_leveling(pos);
- planner.buffer_segment(pos[X_AXIS], pos[Y_AXIS], pos[Z_AXIS], bez_target[E_AXIS], fr_mm_s, active_extruder);
+ if (!planner.buffer_segment(pos[X_AXIS], pos[Y_AXIS], pos[Z_AXIS], bez_target[E_AXIS], fr_mm_s, active_extruder))
+ break;
#else
- planner.buffer_line_kinematic(bez_target, fr_mm_s, extruder);
+ if (!planner.buffer_line_kinematic(bez_target, fr_mm_s, extruder))
+ break;
#endif
}
}
diff --git a/Marlin/stepper.cpp b/Marlin/stepper.cpp
index 9d7cb4df1..071e6e89b 100644
--- a/Marlin/stepper.cpp
+++ b/Marlin/stepper.cpp
@@ -50,6 +50,8 @@
* Jerk controlled movements planner added Apr 2018 by Eduardo José Tagle.
* Equations based on Synthethos TinyG2 sources, but the fixed-point
* implementation is new, as we are running the ISR with a variable period.
+ * Also implemented the Bézier velocity curve evaluation in ARM assembler,
+ * to avoid impacting ISR speed.
*/
#include "Marlin.h"
@@ -73,10 +75,6 @@ Stepper stepper; // Singleton
block_t* Stepper::current_block = NULL; // A pointer to the block currently being traced
-#if ENABLED(ABORT_ON_ENDSTOP_HIT_FEATURE_ENABLED)
- bool Stepper::abort_on_endstop_hit = false;
-#endif
-
#if ENABLED(X_DUAL_ENDSTOPS) || ENABLED(Y_DUAL_ENDSTOPS) || ENABLED(Z_DUAL_ENDSTOPS)
bool Stepper::performing_homing = false;
#endif
@@ -87,8 +85,10 @@ block_t* Stepper::current_block = NULL; // A pointer to the block currently bei
// private:
-uint8_t Stepper::last_direction_bits = 0; // The next stepping-bits to be output
-int16_t Stepper::cleaning_buffer_counter = 0;
+uint8_t Stepper::last_direction_bits = 0, // The next stepping-bits to be output
+ Stepper::last_movement_extruder = 0xFF; // Last movement extruder, as computed when the last movement was fetched from planner
+bool Stepper::abort_current_block, // Signals to the stepper that current block should be aborted
+ Stepper::last_movement_non_null[NUM_AXIS]; // Last Movement in the given direction is not null, as computed when the last movement was fetched from planner
#if ENABLED(X_DUAL_ENDSTOPS)
bool Stepper::locked_x_motor = false, Stepper::locked_x2_motor = false;
@@ -105,7 +105,7 @@ int32_t Stepper::counter_X = 0,
Stepper::counter_Z = 0,
Stepper::counter_E = 0;
-volatile uint32_t Stepper::step_events_completed = 0; // The number of step events executed in the current block
+uint32_t Stepper::step_events_completed = 0; // The number of step events executed in the current block
#if ENABLED(BEZIER_JERK_CONTROL)
int32_t __attribute__((used)) Stepper::bezier_A __asm__("bezier_A"); // A coefficient in Bézier speed curve with alias for assembler
@@ -117,16 +117,17 @@ volatile uint32_t Stepper::step_events_completed = 0; // The number of step even
bool Stepper::bezier_2nd_half; // =false If Bézier curve has been initialized or not
#endif
+uint32_t Stepper::nextMainISR = 0;
+bool Stepper::all_steps_done = false;
+
#if ENABLED(LIN_ADVANCE)
uint32_t Stepper::LA_decelerate_after;
- constexpr uint16_t ADV_NEVER = 65535;
-
- uint16_t Stepper::nextMainISR = 0,
- Stepper::nextAdvanceISR = ADV_NEVER,
- Stepper::eISR_Rate = ADV_NEVER,
- Stepper::current_adv_steps = 0,
+ constexpr uint32_t ADV_NEVER = 0xFFFFFFFF;
+ uint32_t Stepper::nextAdvanceISR = ADV_NEVER,
+ Stepper::eISR_Rate = ADV_NEVER;
+ uint16_t Stepper::current_adv_steps = 0,
Stepper::final_adv_steps,
Stepper::max_adv_steps;
@@ -142,7 +143,7 @@ volatile uint32_t Stepper::step_events_completed = 0; // The number of step even
#endif // LIN_ADVANCE
-int32_t Stepper::acceleration_time, Stepper::deceleration_time;
+uint32_t Stepper::acceleration_time, Stepper::deceleration_time;
volatile int32_t Stepper::count_position[NUM_AXIS] = { 0 };
volatile signed char Stepper::count_direction[NUM_AXIS] = { 1, 1, 1, 1 };
@@ -151,11 +152,11 @@ volatile signed char Stepper::count_direction[NUM_AXIS] = { 1, 1, 1, 1 };
int32_t Stepper::counter_m[MIXING_STEPPERS];
#endif
+uint32_t Stepper::ticks_nominal;
uint8_t Stepper::step_loops, Stepper::step_loops_nominal;
-uint16_t Stepper::OCR1A_nominal;
#if DISABLED(BEZIER_JERK_CONTROL)
- uint16_t Stepper::acc_step_rate; // needed for deceleration start point
+ uint32_t Stepper::acc_step_rate; // needed for deceleration start point
#endif
volatile int32_t Stepper::endstops_trigsteps[XYZ];
@@ -167,20 +168,20 @@ volatile int32_t Stepper::endstops_trigsteps[XYZ];
#define LOCKED_X2_MOTOR locked_x2_motor
#define LOCKED_Y2_MOTOR locked_y2_motor
#define LOCKED_Z2_MOTOR locked_z2_motor
- #define DUAL_ENDSTOP_APPLY_STEP(A,V) \
- if (performing_homing) { \
- if (A##_HOME_DIR < 0) { \
- if (!(TEST(endstops.old_endstop_bits, A##_MIN) && count_direction[_AXIS(A)] < 0) && !LOCKED_##A##_MOTOR) A##_STEP_WRITE(V); \
- if (!(TEST(endstops.old_endstop_bits, A##2_MIN) && count_direction[_AXIS(A)] < 0) && !LOCKED_##A##2_MOTOR) A##2_STEP_WRITE(V); \
- } \
- else { \
- if (!(TEST(endstops.old_endstop_bits, A##_MAX) && count_direction[_AXIS(A)] > 0) && !LOCKED_##A##_MOTOR) A##_STEP_WRITE(V); \
- if (!(TEST(endstops.old_endstop_bits, A##2_MAX) && count_direction[_AXIS(A)] > 0) && !LOCKED_##A##2_MOTOR) A##2_STEP_WRITE(V); \
- } \
- } \
- else { \
- A##_STEP_WRITE(V); \
- A##2_STEP_WRITE(V); \
+ #define DUAL_ENDSTOP_APPLY_STEP(A,V) \
+ if (performing_homing) { \
+ if (A##_HOME_DIR < 0) { \
+ if (!(TEST(endstops.current_endstop_bits, A##_MIN) && count_direction[_AXIS(A)] < 0) && !LOCKED_##A##_MOTOR) A##_STEP_WRITE(V); \
+ if (!(TEST(endstops.current_endstop_bits, A##2_MIN) && count_direction[_AXIS(A)] < 0) && !LOCKED_##A##2_MOTOR) A##2_STEP_WRITE(V); \
+ } \
+ else { \
+ if (!(TEST(endstops.current_endstop_bits, A##_MAX) && count_direction[_AXIS(A)] > 0) && !LOCKED_##A##_MOTOR) A##_STEP_WRITE(V); \
+ if (!(TEST(endstops.current_endstop_bits, A##2_MAX) && count_direction[_AXIS(A)] > 0) && !LOCKED_##A##2_MOTOR) A##2_STEP_WRITE(V); \
+ } \
+ } \
+ else { \
+ A##_STEP_WRITE(V); \
+ A##2_STEP_WRITE(V); \
}
#endif
@@ -426,7 +427,7 @@ void Stepper::set_directions() {
*
* Floating point arithmetic execution time cost is prohibitive, so we will transform the math to
* use fixed point values to be able to evaluate it in realtime. Assuming a maximum of 250000 steps
- * per second (driver pulses should at least be 2uS hi/2uS lo), and allocating 2 bits to avoid
+ * per second (driver pulses should at least be 2µS hi/2µS lo), and allocating 2 bits to avoid
* overflows on the evaluation of the Bézier curve, means we can use
*
* t: unsigned Q0.32 (0 <= t < 1) |range 0 to 0xFFFFFFFF unsigned
@@ -632,65 +633,65 @@ void Stepper::set_directions() {
/* Store initial velocity*/
A("sts bezier_F, %0")
A("sts bezier_F+1, %1")
- A("sts bezier_F+2, %10") /* bezier_F = %10:%1:%0 = v0 */
+ A("sts bezier_F+2, %10") /* bezier_F = %10:%1:%0 = v0 */
/* Get delta speed */
- A("ldi %2,-1") /* %2 = 0xFF, means A_negative = true */
- A("clr %8") /* %8 = 0 */
+ A("ldi %2,-1") /* %2 = 0xFF, means A_negative = true */
+ A("clr %8") /* %8 = 0 */
A("sub %0,%3")
A("sbc %1,%4")
- A("sbc %10,%5") /* v0 -= v1, C=1 if result is negative */
- A("brcc 1f") /* branch if result is positive (C=0), that means v0 >= v1 */
+ A("sbc %10,%5") /* v0 -= v1, C=1 if result is negative */
+ A("brcc 1f") /* branch if result is positive (C=0), that means v0 >= v1 */
/* Result was negative, get the absolute value*/
A("com %10")
A("com %1")
A("neg %0")
A("sbc %1,%2")
- A("sbc %10,%2") /* %10:%1:%0 +1 -> %10:%1:%0 = -(v0 - v1) = (v1 - v0) */
- A("clr %2") /* %2 = 0, means A_negative = false */
+ A("sbc %10,%2") /* %10:%1:%0 +1 -> %10:%1:%0 = -(v0 - v1) = (v1 - v0) */
+ A("clr %2") /* %2 = 0, means A_negative = false */
/* Store negative flag*/
L("1")
- A("sts A_negative, %2") /* Store negative flag */
+ A("sts A_negative, %2") /* Store negative flag */
/* Compute coefficients A,B and C [20 cycles worst case]*/
- A("ldi %9,6") /* %9 = 6 */
- A("mul %0,%9") /* r1:r0 = 6*LO(v0-v1) */
+ A("ldi %9,6") /* %9 = 6 */
+ A("mul %0,%9") /* r1:r0 = 6*LO(v0-v1) */
A("sts bezier_A, r0")
A("mov %6,r1")
- A("clr %7") /* %7:%6:r0 = 6*LO(v0-v1) */
- A("mul %1,%9") /* r1:r0 = 6*MI(v0-v1) */
+ A("clr %7") /* %7:%6:r0 = 6*LO(v0-v1) */
+ A("mul %1,%9") /* r1:r0 = 6*MI(v0-v1) */
A("add %6,r0")
- A("adc %7,r1") /* %7:%6:?? += 6*MI(v0-v1) << 8 */
- A("mul %10,%9") /* r1:r0 = 6*HI(v0-v1) */
- A("add %7,r0") /* %7:%6:?? += 6*HI(v0-v1) << 16 */
+ A("adc %7,r1") /* %7:%6:?? += 6*MI(v0-v1) << 8 */
+ A("mul %10,%9") /* r1:r0 = 6*HI(v0-v1) */
+ A("add %7,r0") /* %7:%6:?? += 6*HI(v0-v1) << 16 */
A("sts bezier_A+1, %6")
- A("sts bezier_A+2, %7") /* bezier_A = %7:%6:?? = 6*(v0-v1) [35 cycles worst] */
+ A("sts bezier_A+2, %7") /* bezier_A = %7:%6:?? = 6*(v0-v1) [35 cycles worst] */
- A("ldi %9,15") /* %9 = 15 */
- A("mul %0,%9") /* r1:r0 = 5*LO(v0-v1) */
+ A("ldi %9,15") /* %9 = 15 */
+ A("mul %0,%9") /* r1:r0 = 5*LO(v0-v1) */
A("sts bezier_B, r0")
A("mov %6,r1")
- A("clr %7") /* %7:%6:?? = 5*LO(v0-v1) */
- A("mul %1,%9") /* r1:r0 = 5*MI(v0-v1) */
+ A("clr %7") /* %7:%6:?? = 5*LO(v0-v1) */
+ A("mul %1,%9") /* r1:r0 = 5*MI(v0-v1) */
A("add %6,r0")
- A("adc %7,r1") /* %7:%6:?? += 5*MI(v0-v1) << 8 */
- A("mul %10,%9") /* r1:r0 = 5*HI(v0-v1) */
- A("add %7,r0") /* %7:%6:?? += 5*HI(v0-v1) << 16 */
+ A("adc %7,r1") /* %7:%6:?? += 5*MI(v0-v1) << 8 */
+ A("mul %10,%9") /* r1:r0 = 5*HI(v0-v1) */
+ A("add %7,r0") /* %7:%6:?? += 5*HI(v0-v1) << 16 */
A("sts bezier_B+1, %6")
- A("sts bezier_B+2, %7") /* bezier_B = %7:%6:?? = 5*(v0-v1) [50 cycles worst] */
+ A("sts bezier_B+2, %7") /* bezier_B = %7:%6:?? = 5*(v0-v1) [50 cycles worst] */
- A("ldi %9,10") /* %9 = 10 */
- A("mul %0,%9") /* r1:r0 = 10*LO(v0-v1) */
+ A("ldi %9,10") /* %9 = 10 */
+ A("mul %0,%9") /* r1:r0 = 10*LO(v0-v1) */
A("sts bezier_C, r0")
A("mov %6,r1")
- A("clr %7") /* %7:%6:?? = 10*LO(v0-v1) */
- A("mul %1,%9") /* r1:r0 = 10*MI(v0-v1) */
+ A("clr %7") /* %7:%6:?? = 10*LO(v0-v1) */
+ A("mul %1,%9") /* r1:r0 = 10*MI(v0-v1) */
A("add %6,r0")
- A("adc %7,r1") /* %7:%6:?? += 10*MI(v0-v1) << 8 */
- A("mul %10,%9") /* r1:r0 = 10*HI(v0-v1) */
- A("add %7,r0") /* %7:%6:?? += 10*HI(v0-v1) << 16 */
+ A("adc %7,r1") /* %7:%6:?? += 10*MI(v0-v1) << 8 */
+ A("mul %10,%9") /* r1:r0 = 10*HI(v0-v1) */
+ A("add %7,r0") /* %7:%6:?? += 10*HI(v0-v1) << 16 */
A("sts bezier_C+1, %6")
" sts bezier_C+2, %7" /* bezier_C = %7:%6:?? = 10*(v0-v1) [65 cycles worst] */
: "+r" (r2),
@@ -723,357 +724,357 @@ void Stepper::set_directions() {
__asm__ __volatile(
/* umul24x24to16hi(t, bezier_AV, curr_step); t: Range 0 - 1^16 = 16 bits*/
- A("lds %9,bezier_AV") /* %9 = LO(AV)*/
- A("mul %9,%2") /* r1:r0 = LO(bezier_AV)*LO(curr_step)*/
- A("mov %7,r1") /* %7 = LO(bezier_AV)*LO(curr_step) >> 8*/
- A("clr %8") /* %8:%7 = LO(bezier_AV)*LO(curr_step) >> 8*/
- A("lds %10,bezier_AV+1") /* %10 = MI(AV)*/
- A("mul %10,%2") /* r1:r0 = MI(bezier_AV)*LO(curr_step)*/
+ A("lds %9,bezier_AV") /* %9 = LO(AV)*/
+ A("mul %9,%2") /* r1:r0 = LO(bezier_AV)*LO(curr_step)*/
+ A("mov %7,r1") /* %7 = LO(bezier_AV)*LO(curr_step) >> 8*/
+ A("clr %8") /* %8:%7 = LO(bezier_AV)*LO(curr_step) >> 8*/
+ A("lds %10,bezier_AV+1") /* %10 = MI(AV)*/
+ A("mul %10,%2") /* r1:r0 = MI(bezier_AV)*LO(curr_step)*/
A("add %7,r0")
- A("adc %8,r1") /* %8:%7 += MI(bezier_AV)*LO(curr_step)*/
- A("lds r1,bezier_AV+2") /* r11 = HI(AV)*/
- A("mul r1,%2") /* r1:r0 = HI(bezier_AV)*LO(curr_step)*/
- A("add %8,r0") /* %8:%7 += HI(bezier_AV)*LO(curr_step) << 8*/
- A("mul %9,%3") /* r1:r0 = LO(bezier_AV)*MI(curr_step)*/
+ A("adc %8,r1") /* %8:%7 += MI(bezier_AV)*LO(curr_step)*/
+ A("lds r1,bezier_AV+2") /* r11 = HI(AV)*/
+ A("mul r1,%2") /* r1:r0 = HI(bezier_AV)*LO(curr_step)*/
+ A("add %8,r0") /* %8:%7 += HI(bezier_AV)*LO(curr_step) << 8*/
+ A("mul %9,%3") /* r1:r0 = LO(bezier_AV)*MI(curr_step)*/
A("add %7,r0")
- A("adc %8,r1") /* %8:%7 += LO(bezier_AV)*MI(curr_step)*/
- A("mul %10,%3") /* r1:r0 = MI(bezier_AV)*MI(curr_step)*/
- A("add %8,r0") /* %8:%7 += LO(bezier_AV)*MI(curr_step) << 8*/
- A("mul %9,%4") /* r1:r0 = LO(bezier_AV)*HI(curr_step)*/
- A("add %8,r0") /* %8:%7 += LO(bezier_AV)*HI(curr_step) << 8*/
+ A("adc %8,r1") /* %8:%7 += LO(bezier_AV)*MI(curr_step)*/
+ A("mul %10,%3") /* r1:r0 = MI(bezier_AV)*MI(curr_step)*/
+ A("add %8,r0") /* %8:%7 += LO(bezier_AV)*MI(curr_step) << 8*/
+ A("mul %9,%4") /* r1:r0 = LO(bezier_AV)*HI(curr_step)*/
+ A("add %8,r0") /* %8:%7 += LO(bezier_AV)*HI(curr_step) << 8*/
/* %8:%7 = t*/
/* uint16_t f = t;*/
- A("mov %5,%7") /* %6:%5 = f*/
+ A("mov %5,%7") /* %6:%5 = f*/
A("mov %6,%8")
/* %6:%5 = f*/
/* umul16x16to16hi(f, f, t); / Range 16 bits (unsigned) [17] */
- A("mul %5,%7") /* r1:r0 = LO(f) * LO(t)*/
- A("mov %9,r1") /* store MIL(LO(f) * LO(t)) in %9, we need it for rounding*/
- A("clr %10") /* %10 = 0*/
- A("clr %11") /* %11 = 0*/
- A("mul %5,%8") /* r1:r0 = LO(f) * HI(t)*/
- A("add %9,r0") /* %9 += LO(LO(f) * HI(t))*/
- A("adc %10,r1") /* %10 = HI(LO(f) * HI(t))*/
- A("adc %11,%0") /* %11 += carry*/
- A("mul %6,%7") /* r1:r0 = HI(f) * LO(t)*/
- A("add %9,r0") /* %9 += LO(HI(f) * LO(t))*/
- A("adc %10,r1") /* %10 += HI(HI(f) * LO(t)) */
- A("adc %11,%0") /* %11 += carry*/
- A("mul %6,%8") /* r1:r0 = HI(f) * HI(t)*/
- A("add %10,r0") /* %10 += LO(HI(f) * HI(t))*/
- A("adc %11,r1") /* %11 += HI(HI(f) * HI(t))*/
- A("mov %5,%10") /* %6:%5 = */
- A("mov %6,%11") /* f = %10:%11*/
+ A("mul %5,%7") /* r1:r0 = LO(f) * LO(t)*/
+ A("mov %9,r1") /* store MIL(LO(f) * LO(t)) in %9, we need it for rounding*/
+ A("clr %10") /* %10 = 0*/
+ A("clr %11") /* %11 = 0*/
+ A("mul %5,%8") /* r1:r0 = LO(f) * HI(t)*/
+ A("add %9,r0") /* %9 += LO(LO(f) * HI(t))*/
+ A("adc %10,r1") /* %10 = HI(LO(f) * HI(t))*/
+ A("adc %11,%0") /* %11 += carry*/
+ A("mul %6,%7") /* r1:r0 = HI(f) * LO(t)*/
+ A("add %9,r0") /* %9 += LO(HI(f) * LO(t))*/
+ A("adc %10,r1") /* %10 += HI(HI(f) * LO(t)) */
+ A("adc %11,%0") /* %11 += carry*/
+ A("mul %6,%8") /* r1:r0 = HI(f) * HI(t)*/
+ A("add %10,r0") /* %10 += LO(HI(f) * HI(t))*/
+ A("adc %11,r1") /* %11 += HI(HI(f) * HI(t))*/
+ A("mov %5,%10") /* %6:%5 = */
+ A("mov %6,%11") /* f = %10:%11*/
/* umul16x16to16hi(f, f, t); / Range 16 bits : f = t^3 (unsigned) [17]*/
- A("mul %5,%7") /* r1:r0 = LO(f) * LO(t)*/
- A("mov %1,r1") /* store MIL(LO(f) * LO(t)) in %1, we need it for rounding*/
- A("clr %10") /* %10 = 0*/
- A("clr %11") /* %11 = 0*/
- A("mul %5,%8") /* r1:r0 = LO(f) * HI(t)*/
- A("add %1,r0") /* %1 += LO(LO(f) * HI(t))*/
- A("adc %10,r1") /* %10 = HI(LO(f) * HI(t))*/
- A("adc %11,%0") /* %11 += carry*/
- A("mul %6,%7") /* r1:r0 = HI(f) * LO(t)*/
- A("add %1,r0") /* %1 += LO(HI(f) * LO(t))*/
- A("adc %10,r1") /* %10 += HI(HI(f) * LO(t))*/
- A("adc %11,%0") /* %11 += carry*/
- A("mul %6,%8") /* r1:r0 = HI(f) * HI(t)*/
- A("add %10,r0") /* %10 += LO(HI(f) * HI(t))*/
- A("adc %11,r1") /* %11 += HI(HI(f) * HI(t))*/
- A("mov %5,%10") /* %6:%5 =*/
- A("mov %6,%11") /* f = %10:%11*/
+ A("mul %5,%7") /* r1:r0 = LO(f) * LO(t)*/
+ A("mov %1,r1") /* store MIL(LO(f) * LO(t)) in %1, we need it for rounding*/
+ A("clr %10") /* %10 = 0*/
+ A("clr %11") /* %11 = 0*/
+ A("mul %5,%8") /* r1:r0 = LO(f) * HI(t)*/
+ A("add %1,r0") /* %1 += LO(LO(f) * HI(t))*/
+ A("adc %10,r1") /* %10 = HI(LO(f) * HI(t))*/
+ A("adc %11,%0") /* %11 += carry*/
+ A("mul %6,%7") /* r1:r0 = HI(f) * LO(t)*/
+ A("add %1,r0") /* %1 += LO(HI(f) * LO(t))*/
+ A("adc %10,r1") /* %10 += HI(HI(f) * LO(t))*/
+ A("adc %11,%0") /* %11 += carry*/
+ A("mul %6,%8") /* r1:r0 = HI(f) * HI(t)*/
+ A("add %10,r0") /* %10 += LO(HI(f) * HI(t))*/
+ A("adc %11,r1") /* %11 += HI(HI(f) * HI(t))*/
+ A("mov %5,%10") /* %6:%5 =*/
+ A("mov %6,%11") /* f = %10:%11*/
/* [15 +17*2] = [49]*/
/* %4:%3:%2 will be acc from now on*/
/* uint24_t acc = bezier_F; / Range 20 bits (unsigned)*/
- A("clr %9") /* "decimal place we get for free"*/
+ A("clr %9") /* "decimal place we get for free"*/
A("lds %2,bezier_F")
A("lds %3,bezier_F+1")
- A("lds %4,bezier_F+2") /* %4:%3:%2 = acc*/
+ A("lds %4,bezier_F+2") /* %4:%3:%2 = acc*/
/* if (A_negative) {*/
A("lds r0,A_negative")
- A("or r0,%0") /* Is flag signalling negative? */
- A("brne 3f") /* If yes, Skip next instruction if A was negative*/
- A("rjmp 1f") /* Otherwise, jump */
+ A("or r0,%0") /* Is flag signalling negative? */
+ A("brne 3f") /* If yes, Skip next instruction if A was negative*/
+ A("rjmp 1f") /* Otherwise, jump */
/* uint24_t v; */
/* umul16x24to24hi(v, f, bezier_C); / Range 21bits [29] */
/* acc -= v; */
L("3")
- A("lds %10, bezier_C") /* %10 = LO(bezier_C)*/
- A("mul %10,%5") /* r1:r0 = LO(bezier_C) * LO(f)*/
+ A("lds %10, bezier_C") /* %10 = LO(bezier_C)*/
+ A("mul %10,%5") /* r1:r0 = LO(bezier_C) * LO(f)*/
A("sub %9,r1")
A("sbc %2,%0")
A("sbc %3,%0")
- A("sbc %4,%0") /* %4:%3:%2:%9 -= HI(LO(bezier_C) * LO(f))*/
- A("lds %11, bezier_C+1") /* %11 = MI(bezier_C)*/
- A("mul %11,%5") /* r1:r0 = MI(bezier_C) * LO(f)*/
+ A("sbc %4,%0") /* %4:%3:%2:%9 -= HI(LO(bezier_C) * LO(f))*/
+ A("lds %11, bezier_C+1") /* %11 = MI(bezier_C)*/
+ A("mul %11,%5") /* r1:r0 = MI(bezier_C) * LO(f)*/
A("sub %9,r0")
A("sbc %2,r1")
A("sbc %3,%0")
- A("sbc %4,%0") /* %4:%3:%2:%9 -= MI(bezier_C) * LO(f)*/
- A("lds %1, bezier_C+2") /* %1 = HI(bezier_C)*/
- A("mul %1,%5") /* r1:r0 = MI(bezier_C) * LO(f)*/
+ A("sbc %4,%0") /* %4:%3:%2:%9 -= MI(bezier_C) * LO(f)*/
+ A("lds %1, bezier_C+2") /* %1 = HI(bezier_C)*/
+ A("mul %1,%5") /* r1:r0 = MI(bezier_C) * LO(f)*/
A("sub %2,r0")
A("sbc %3,r1")
- A("sbc %4,%0") /* %4:%3:%2:%9 -= HI(bezier_C) * LO(f) << 8*/
- A("mul %10,%6") /* r1:r0 = LO(bezier_C) * MI(f)*/
+ A("sbc %4,%0") /* %4:%3:%2:%9 -= HI(bezier_C) * LO(f) << 8*/
+ A("mul %10,%6") /* r1:r0 = LO(bezier_C) * MI(f)*/
A("sub %9,r0")
A("sbc %2,r1")
A("sbc %3,%0")
- A("sbc %4,%0") /* %4:%3:%2:%9 -= LO(bezier_C) * MI(f)*/
- A("mul %11,%6") /* r1:r0 = MI(bezier_C) * MI(f)*/
+ A("sbc %4,%0") /* %4:%3:%2:%9 -= LO(bezier_C) * MI(f)*/
+ A("mul %11,%6") /* r1:r0 = MI(bezier_C) * MI(f)*/
A("sub %2,r0")
A("sbc %3,r1")
- A("sbc %4,%0") /* %4:%3:%2:%9 -= MI(bezier_C) * MI(f) << 8*/
- A("mul %1,%6") /* r1:r0 = HI(bezier_C) * LO(f)*/
+ A("sbc %4,%0") /* %4:%3:%2:%9 -= MI(bezier_C) * MI(f) << 8*/
+ A("mul %1,%6") /* r1:r0 = HI(bezier_C) * LO(f)*/
A("sub %3,r0")
- A("sbc %4,r1") /* %4:%3:%2:%9 -= HI(bezier_C) * LO(f) << 16*/
+ A("sbc %4,r1") /* %4:%3:%2:%9 -= HI(bezier_C) * LO(f) << 16*/
/* umul16x16to16hi(f, f, t); / Range 16 bits : f = t^3 (unsigned) [17]*/
- A("mul %5,%7") /* r1:r0 = LO(f) * LO(t)*/
- A("mov %1,r1") /* store MIL(LO(f) * LO(t)) in %1, we need it for rounding*/
- A("clr %10") /* %10 = 0*/
- A("clr %11") /* %11 = 0*/
- A("mul %5,%8") /* r1:r0 = LO(f) * HI(t)*/
- A("add %1,r0") /* %1 += LO(LO(f) * HI(t))*/
- A("adc %10,r1") /* %10 = HI(LO(f) * HI(t))*/
- A("adc %11,%0") /* %11 += carry*/
- A("mul %6,%7") /* r1:r0 = HI(f) * LO(t)*/
- A("add %1,r0") /* %1 += LO(HI(f) * LO(t))*/
- A("adc %10,r1") /* %10 += HI(HI(f) * LO(t))*/
- A("adc %11,%0") /* %11 += carry*/
- A("mul %6,%8") /* r1:r0 = HI(f) * HI(t)*/
- A("add %10,r0") /* %10 += LO(HI(f) * HI(t))*/
- A("adc %11,r1") /* %11 += HI(HI(f) * HI(t))*/
- A("mov %5,%10") /* %6:%5 =*/
- A("mov %6,%11") /* f = %10:%11*/
+ A("mul %5,%7") /* r1:r0 = LO(f) * LO(t)*/
+ A("mov %1,r1") /* store MIL(LO(f) * LO(t)) in %1, we need it for rounding*/
+ A("clr %10") /* %10 = 0*/
+ A("clr %11") /* %11 = 0*/
+ A("mul %5,%8") /* r1:r0 = LO(f) * HI(t)*/
+ A("add %1,r0") /* %1 += LO(LO(f) * HI(t))*/
+ A("adc %10,r1") /* %10 = HI(LO(f) * HI(t))*/
+ A("adc %11,%0") /* %11 += carry*/
+ A("mul %6,%7") /* r1:r0 = HI(f) * LO(t)*/
+ A("add %1,r0") /* %1 += LO(HI(f) * LO(t))*/
+ A("adc %10,r1") /* %10 += HI(HI(f) * LO(t))*/
+ A("adc %11,%0") /* %11 += carry*/
+ A("mul %6,%8") /* r1:r0 = HI(f) * HI(t)*/
+ A("add %10,r0") /* %10 += LO(HI(f) * HI(t))*/
+ A("adc %11,r1") /* %11 += HI(HI(f) * HI(t))*/
+ A("mov %5,%10") /* %6:%5 =*/
+ A("mov %6,%11") /* f = %10:%11*/
/* umul16x24to24hi(v, f, bezier_B); / Range 22bits [29]*/
/* acc += v; */
- A("lds %10, bezier_B") /* %10 = LO(bezier_B)*/
- A("mul %10,%5") /* r1:r0 = LO(bezier_B) * LO(f)*/
+ A("lds %10, bezier_B") /* %10 = LO(bezier_B)*/
+ A("mul %10,%5") /* r1:r0 = LO(bezier_B) * LO(f)*/
A("add %9,r1")
A("adc %2,%0")
A("adc %3,%0")
- A("adc %4,%0") /* %4:%3:%2:%9 += HI(LO(bezier_B) * LO(f))*/
- A("lds %11, bezier_B+1") /* %11 = MI(bezier_B)*/
- A("mul %11,%5") /* r1:r0 = MI(bezier_B) * LO(f)*/
+ A("adc %4,%0") /* %4:%3:%2:%9 += HI(LO(bezier_B) * LO(f))*/
+ A("lds %11, bezier_B+1") /* %11 = MI(bezier_B)*/
+ A("mul %11,%5") /* r1:r0 = MI(bezier_B) * LO(f)*/
A("add %9,r0")
A("adc %2,r1")
A("adc %3,%0")
- A("adc %4,%0") /* %4:%3:%2:%9 += MI(bezier_B) * LO(f)*/
- A("lds %1, bezier_B+2") /* %1 = HI(bezier_B)*/
- A("mul %1,%5") /* r1:r0 = MI(bezier_B) * LO(f)*/
+ A("adc %4,%0") /* %4:%3:%2:%9 += MI(bezier_B) * LO(f)*/
+ A("lds %1, bezier_B+2") /* %1 = HI(bezier_B)*/
+ A("mul %1,%5") /* r1:r0 = MI(bezier_B) * LO(f)*/
A("add %2,r0")
A("adc %3,r1")
- A("adc %4,%0") /* %4:%3:%2:%9 += HI(bezier_B) * LO(f) << 8*/
- A("mul %10,%6") /* r1:r0 = LO(bezier_B) * MI(f)*/
+ A("adc %4,%0") /* %4:%3:%2:%9 += HI(bezier_B) * LO(f) << 8*/
+ A("mul %10,%6") /* r1:r0 = LO(bezier_B) * MI(f)*/
A("add %9,r0")
A("adc %2,r1")
A("adc %3,%0")
- A("adc %4,%0") /* %4:%3:%2:%9 += LO(bezier_B) * MI(f)*/
- A("mul %11,%6") /* r1:r0 = MI(bezier_B) * MI(f)*/
+ A("adc %4,%0") /* %4:%3:%2:%9 += LO(bezier_B) * MI(f)*/
+ A("mul %11,%6") /* r1:r0 = MI(bezier_B) * MI(f)*/
A("add %2,r0")
A("adc %3,r1")
- A("adc %4,%0") /* %4:%3:%2:%9 += MI(bezier_B) * MI(f) << 8*/
- A("mul %1,%6") /* r1:r0 = HI(bezier_B) * LO(f)*/
+ A("adc %4,%0") /* %4:%3:%2:%9 += MI(bezier_B) * MI(f) << 8*/
+ A("mul %1,%6") /* r1:r0 = HI(bezier_B) * LO(f)*/
A("add %3,r0")
- A("adc %4,r1") /* %4:%3:%2:%9 += HI(bezier_B) * LO(f) << 16*/
+ A("adc %4,r1") /* %4:%3:%2:%9 += HI(bezier_B) * LO(f) << 16*/
/* umul16x16to16hi(f, f, t); / Range 16 bits : f = t^5 (unsigned) [17]*/
- A("mul %5,%7") /* r1:r0 = LO(f) * LO(t)*/
- A("mov %1,r1") /* store MIL(LO(f) * LO(t)) in %1, we need it for rounding*/
- A("clr %10") /* %10 = 0*/
- A("clr %11") /* %11 = 0*/
- A("mul %5,%8") /* r1:r0 = LO(f) * HI(t)*/
- A("add %1,r0") /* %1 += LO(LO(f) * HI(t))*/
- A("adc %10,r1") /* %10 = HI(LO(f) * HI(t))*/
- A("adc %11,%0") /* %11 += carry*/
- A("mul %6,%7") /* r1:r0 = HI(f) * LO(t)*/
- A("add %1,r0") /* %1 += LO(HI(f) * LO(t))*/
- A("adc %10,r1") /* %10 += HI(HI(f) * LO(t))*/
- A("adc %11,%0") /* %11 += carry*/
- A("mul %6,%8") /* r1:r0 = HI(f) * HI(t)*/
- A("add %10,r0") /* %10 += LO(HI(f) * HI(t))*/
- A("adc %11,r1") /* %11 += HI(HI(f) * HI(t))*/
- A("mov %5,%10") /* %6:%5 =*/
- A("mov %6,%11") /* f = %10:%11*/
+ A("mul %5,%7") /* r1:r0 = LO(f) * LO(t)*/
+ A("mov %1,r1") /* store MIL(LO(f) * LO(t)) in %1, we need it for rounding*/
+ A("clr %10") /* %10 = 0*/
+ A("clr %11") /* %11 = 0*/
+ A("mul %5,%8") /* r1:r0 = LO(f) * HI(t)*/
+ A("add %1,r0") /* %1 += LO(LO(f) * HI(t))*/
+ A("adc %10,r1") /* %10 = HI(LO(f) * HI(t))*/
+ A("adc %11,%0") /* %11 += carry*/
+ A("mul %6,%7") /* r1:r0 = HI(f) * LO(t)*/
+ A("add %1,r0") /* %1 += LO(HI(f) * LO(t))*/
+ A("adc %10,r1") /* %10 += HI(HI(f) * LO(t))*/
+ A("adc %11,%0") /* %11 += carry*/
+ A("mul %6,%8") /* r1:r0 = HI(f) * HI(t)*/
+ A("add %10,r0") /* %10 += LO(HI(f) * HI(t))*/
+ A("adc %11,r1") /* %11 += HI(HI(f) * HI(t))*/
+ A("mov %5,%10") /* %6:%5 =*/
+ A("mov %6,%11") /* f = %10:%11*/
/* umul16x24to24hi(v, f, bezier_A); / Range 21bits [29]*/
/* acc -= v; */
- A("lds %10, bezier_A") /* %10 = LO(bezier_A)*/
- A("mul %10,%5") /* r1:r0 = LO(bezier_A) * LO(f)*/
+ A("lds %10, bezier_A") /* %10 = LO(bezier_A)*/
+ A("mul %10,%5") /* r1:r0 = LO(bezier_A) * LO(f)*/
A("sub %9,r1")
A("sbc %2,%0")
A("sbc %3,%0")
- A("sbc %4,%0") /* %4:%3:%2:%9 -= HI(LO(bezier_A) * LO(f))*/
- A("lds %11, bezier_A+1") /* %11 = MI(bezier_A)*/
- A("mul %11,%5") /* r1:r0 = MI(bezier_A) * LO(f)*/
+ A("sbc %4,%0") /* %4:%3:%2:%9 -= HI(LO(bezier_A) * LO(f))*/
+ A("lds %11, bezier_A+1") /* %11 = MI(bezier_A)*/
+ A("mul %11,%5") /* r1:r0 = MI(bezier_A) * LO(f)*/
A("sub %9,r0")
A("sbc %2,r1")
A("sbc %3,%0")
- A("sbc %4,%0") /* %4:%3:%2:%9 -= MI(bezier_A) * LO(f)*/
- A("lds %1, bezier_A+2") /* %1 = HI(bezier_A)*/
- A("mul %1,%5") /* r1:r0 = MI(bezier_A) * LO(f)*/
+ A("sbc %4,%0") /* %4:%3:%2:%9 -= MI(bezier_A) * LO(f)*/
+ A("lds %1, bezier_A+2") /* %1 = HI(bezier_A)*/
+ A("mul %1,%5") /* r1:r0 = MI(bezier_A) * LO(f)*/
A("sub %2,r0")
A("sbc %3,r1")
- A("sbc %4,%0") /* %4:%3:%2:%9 -= HI(bezier_A) * LO(f) << 8*/
- A("mul %10,%6") /* r1:r0 = LO(bezier_A) * MI(f)*/
+ A("sbc %4,%0") /* %4:%3:%2:%9 -= HI(bezier_A) * LO(f) << 8*/
+ A("mul %10,%6") /* r1:r0 = LO(bezier_A) * MI(f)*/
A("sub %9,r0")
A("sbc %2,r1")
A("sbc %3,%0")
- A("sbc %4,%0") /* %4:%3:%2:%9 -= LO(bezier_A) * MI(f)*/
- A("mul %11,%6") /* r1:r0 = MI(bezier_A) * MI(f)*/
+ A("sbc %4,%0") /* %4:%3:%2:%9 -= LO(bezier_A) * MI(f)*/
+ A("mul %11,%6") /* r1:r0 = MI(bezier_A) * MI(f)*/
A("sub %2,r0")
A("sbc %3,r1")
- A("sbc %4,%0") /* %4:%3:%2:%9 -= MI(bezier_A) * MI(f) << 8*/
- A("mul %1,%6") /* r1:r0 = HI(bezier_A) * LO(f)*/
+ A("sbc %4,%0") /* %4:%3:%2:%9 -= MI(bezier_A) * MI(f) << 8*/
+ A("mul %1,%6") /* r1:r0 = HI(bezier_A) * LO(f)*/
A("sub %3,r0")
- A("sbc %4,r1") /* %4:%3:%2:%9 -= HI(bezier_A) * LO(f) << 16*/
- A("jmp 2f") /* Done!*/
+ A("sbc %4,r1") /* %4:%3:%2:%9 -= HI(bezier_A) * LO(f) << 16*/
+ A("jmp 2f") /* Done!*/
L("1")
/* uint24_t v; */
/* umul16x24to24hi(v, f, bezier_C); / Range 21bits [29]*/
/* acc += v; */
- A("lds %10, bezier_C") /* %10 = LO(bezier_C)*/
- A("mul %10,%5") /* r1:r0 = LO(bezier_C) * LO(f)*/
+ A("lds %10, bezier_C") /* %10 = LO(bezier_C)*/
+ A("mul %10,%5") /* r1:r0 = LO(bezier_C) * LO(f)*/
A("add %9,r1")
A("adc %2,%0")
A("adc %3,%0")
- A("adc %4,%0") /* %4:%3:%2:%9 += HI(LO(bezier_C) * LO(f))*/
- A("lds %11, bezier_C+1") /* %11 = MI(bezier_C)*/
- A("mul %11,%5") /* r1:r0 = MI(bezier_C) * LO(f)*/
+ A("adc %4,%0") /* %4:%3:%2:%9 += HI(LO(bezier_C) * LO(f))*/
+ A("lds %11, bezier_C+1") /* %11 = MI(bezier_C)*/
+ A("mul %11,%5") /* r1:r0 = MI(bezier_C) * LO(f)*/
A("add %9,r0")
A("adc %2,r1")
A("adc %3,%0")
- A("adc %4,%0") /* %4:%3:%2:%9 += MI(bezier_C) * LO(f)*/
- A("lds %1, bezier_C+2") /* %1 = HI(bezier_C)*/
- A("mul %1,%5") /* r1:r0 = MI(bezier_C) * LO(f)*/
+ A("adc %4,%0") /* %4:%3:%2:%9 += MI(bezier_C) * LO(f)*/
+ A("lds %1, bezier_C+2") /* %1 = HI(bezier_C)*/
+ A("mul %1,%5") /* r1:r0 = MI(bezier_C) * LO(f)*/
A("add %2,r0")
A("adc %3,r1")
- A("adc %4,%0") /* %4:%3:%2:%9 += HI(bezier_C) * LO(f) << 8*/
- A("mul %10,%6") /* r1:r0 = LO(bezier_C) * MI(f)*/
+ A("adc %4,%0") /* %4:%3:%2:%9 += HI(bezier_C) * LO(f) << 8*/
+ A("mul %10,%6") /* r1:r0 = LO(bezier_C) * MI(f)*/
A("add %9,r0")
A("adc %2,r1")
A("adc %3,%0")
- A("adc %4,%0") /* %4:%3:%2:%9 += LO(bezier_C) * MI(f)*/
- A("mul %11,%6") /* r1:r0 = MI(bezier_C) * MI(f)*/
+ A("adc %4,%0") /* %4:%3:%2:%9 += LO(bezier_C) * MI(f)*/
+ A("mul %11,%6") /* r1:r0 = MI(bezier_C) * MI(f)*/
A("add %2,r0")
A("adc %3,r1")
- A("adc %4,%0") /* %4:%3:%2:%9 += MI(bezier_C) * MI(f) << 8*/
- A("mul %1,%6") /* r1:r0 = HI(bezier_C) * LO(f)*/
+ A("adc %4,%0") /* %4:%3:%2:%9 += MI(bezier_C) * MI(f) << 8*/
+ A("mul %1,%6") /* r1:r0 = HI(bezier_C) * LO(f)*/
A("add %3,r0")
- A("adc %4,r1") /* %4:%3:%2:%9 += HI(bezier_C) * LO(f) << 16*/
+ A("adc %4,r1") /* %4:%3:%2:%9 += HI(bezier_C) * LO(f) << 16*/
/* umul16x16to16hi(f, f, t); / Range 16 bits : f = t^3 (unsigned) [17]*/
- A("mul %5,%7") /* r1:r0 = LO(f) * LO(t)*/
- A("mov %1,r1") /* store MIL(LO(f) * LO(t)) in %1, we need it for rounding*/
- A("clr %10") /* %10 = 0*/
- A("clr %11") /* %11 = 0*/
- A("mul %5,%8") /* r1:r0 = LO(f) * HI(t)*/
- A("add %1,r0") /* %1 += LO(LO(f) * HI(t))*/
- A("adc %10,r1") /* %10 = HI(LO(f) * HI(t))*/
- A("adc %11,%0") /* %11 += carry*/
- A("mul %6,%7") /* r1:r0 = HI(f) * LO(t)*/
- A("add %1,r0") /* %1 += LO(HI(f) * LO(t))*/
- A("adc %10,r1") /* %10 += HI(HI(f) * LO(t))*/
- A("adc %11,%0") /* %11 += carry*/
- A("mul %6,%8") /* r1:r0 = HI(f) * HI(t)*/
- A("add %10,r0") /* %10 += LO(HI(f) * HI(t))*/
- A("adc %11,r1") /* %11 += HI(HI(f) * HI(t))*/
- A("mov %5,%10") /* %6:%5 =*/
- A("mov %6,%11") /* f = %10:%11*/
+ A("mul %5,%7") /* r1:r0 = LO(f) * LO(t)*/
+ A("mov %1,r1") /* store MIL(LO(f) * LO(t)) in %1, we need it for rounding*/
+ A("clr %10") /* %10 = 0*/
+ A("clr %11") /* %11 = 0*/
+ A("mul %5,%8") /* r1:r0 = LO(f) * HI(t)*/
+ A("add %1,r0") /* %1 += LO(LO(f) * HI(t))*/
+ A("adc %10,r1") /* %10 = HI(LO(f) * HI(t))*/
+ A("adc %11,%0") /* %11 += carry*/
+ A("mul %6,%7") /* r1:r0 = HI(f) * LO(t)*/
+ A("add %1,r0") /* %1 += LO(HI(f) * LO(t))*/
+ A("adc %10,r1") /* %10 += HI(HI(f) * LO(t))*/
+ A("adc %11,%0") /* %11 += carry*/
+ A("mul %6,%8") /* r1:r0 = HI(f) * HI(t)*/
+ A("add %10,r0") /* %10 += LO(HI(f) * HI(t))*/
+ A("adc %11,r1") /* %11 += HI(HI(f) * HI(t))*/
+ A("mov %5,%10") /* %6:%5 =*/
+ A("mov %6,%11") /* f = %10:%11*/
/* umul16x24to24hi(v, f, bezier_B); / Range 22bits [29]*/
/* acc -= v;*/
- A("lds %10, bezier_B") /* %10 = LO(bezier_B)*/
- A("mul %10,%5") /* r1:r0 = LO(bezier_B) * LO(f)*/
+ A("lds %10, bezier_B") /* %10 = LO(bezier_B)*/
+ A("mul %10,%5") /* r1:r0 = LO(bezier_B) * LO(f)*/
A("sub %9,r1")
A("sbc %2,%0")
A("sbc %3,%0")
- A("sbc %4,%0") /* %4:%3:%2:%9 -= HI(LO(bezier_B) * LO(f))*/
- A("lds %11, bezier_B+1") /* %11 = MI(bezier_B)*/
- A("mul %11,%5") /* r1:r0 = MI(bezier_B) * LO(f)*/
+ A("sbc %4,%0") /* %4:%3:%2:%9 -= HI(LO(bezier_B) * LO(f))*/
+ A("lds %11, bezier_B+1") /* %11 = MI(bezier_B)*/
+ A("mul %11,%5") /* r1:r0 = MI(bezier_B) * LO(f)*/
A("sub %9,r0")
A("sbc %2,r1")
A("sbc %3,%0")
- A("sbc %4,%0") /* %4:%3:%2:%9 -= MI(bezier_B) * LO(f)*/
- A("lds %1, bezier_B+2") /* %1 = HI(bezier_B)*/
- A("mul %1,%5") /* r1:r0 = MI(bezier_B) * LO(f)*/
+ A("sbc %4,%0") /* %4:%3:%2:%9 -= MI(bezier_B) * LO(f)*/
+ A("lds %1, bezier_B+2") /* %1 = HI(bezier_B)*/
+ A("mul %1,%5") /* r1:r0 = MI(bezier_B) * LO(f)*/
A("sub %2,r0")
A("sbc %3,r1")
- A("sbc %4,%0") /* %4:%3:%2:%9 -= HI(bezier_B) * LO(f) << 8*/
- A("mul %10,%6") /* r1:r0 = LO(bezier_B) * MI(f)*/
+ A("sbc %4,%0") /* %4:%3:%2:%9 -= HI(bezier_B) * LO(f) << 8*/
+ A("mul %10,%6") /* r1:r0 = LO(bezier_B) * MI(f)*/
A("sub %9,r0")
A("sbc %2,r1")
A("sbc %3,%0")
- A("sbc %4,%0") /* %4:%3:%2:%9 -= LO(bezier_B) * MI(f)*/
- A("mul %11,%6") /* r1:r0 = MI(bezier_B) * MI(f)*/
+ A("sbc %4,%0") /* %4:%3:%2:%9 -= LO(bezier_B) * MI(f)*/
+ A("mul %11,%6") /* r1:r0 = MI(bezier_B) * MI(f)*/
A("sub %2,r0")
A("sbc %3,r1")
- A("sbc %4,%0") /* %4:%3:%2:%9 -= MI(bezier_B) * MI(f) << 8*/
- A("mul %1,%6") /* r1:r0 = HI(bezier_B) * LO(f)*/
+ A("sbc %4,%0") /* %4:%3:%2:%9 -= MI(bezier_B) * MI(f) << 8*/
+ A("mul %1,%6") /* r1:r0 = HI(bezier_B) * LO(f)*/
A("sub %3,r0")
- A("sbc %4,r1") /* %4:%3:%2:%9 -= HI(bezier_B) * LO(f) << 16*/
+ A("sbc %4,r1") /* %4:%3:%2:%9 -= HI(bezier_B) * LO(f) << 16*/
/* umul16x16to16hi(f, f, t); / Range 16 bits : f = t^5 (unsigned) [17]*/
- A("mul %5,%7") /* r1:r0 = LO(f) * LO(t)*/
- A("mov %1,r1") /* store MIL(LO(f) * LO(t)) in %1, we need it for rounding*/
- A("clr %10") /* %10 = 0*/
- A("clr %11") /* %11 = 0*/
- A("mul %5,%8") /* r1:r0 = LO(f) * HI(t)*/
- A("add %1,r0") /* %1 += LO(LO(f) * HI(t))*/
- A("adc %10,r1") /* %10 = HI(LO(f) * HI(t))*/
- A("adc %11,%0") /* %11 += carry*/
- A("mul %6,%7") /* r1:r0 = HI(f) * LO(t)*/
- A("add %1,r0") /* %1 += LO(HI(f) * LO(t))*/
- A("adc %10,r1") /* %10 += HI(HI(f) * LO(t))*/
- A("adc %11,%0") /* %11 += carry*/
- A("mul %6,%8") /* r1:r0 = HI(f) * HI(t)*/
- A("add %10,r0") /* %10 += LO(HI(f) * HI(t))*/
- A("adc %11,r1") /* %11 += HI(HI(f) * HI(t))*/
- A("mov %5,%10") /* %6:%5 =*/
- A("mov %6,%11") /* f = %10:%11*/
+ A("mul %5,%7") /* r1:r0 = LO(f) * LO(t)*/
+ A("mov %1,r1") /* store MIL(LO(f) * LO(t)) in %1, we need it for rounding*/
+ A("clr %10") /* %10 = 0*/
+ A("clr %11") /* %11 = 0*/
+ A("mul %5,%8") /* r1:r0 = LO(f) * HI(t)*/
+ A("add %1,r0") /* %1 += LO(LO(f) * HI(t))*/
+ A("adc %10,r1") /* %10 = HI(LO(f) * HI(t))*/
+ A("adc %11,%0") /* %11 += carry*/
+ A("mul %6,%7") /* r1:r0 = HI(f) * LO(t)*/
+ A("add %1,r0") /* %1 += LO(HI(f) * LO(t))*/
+ A("adc %10,r1") /* %10 += HI(HI(f) * LO(t))*/
+ A("adc %11,%0") /* %11 += carry*/
+ A("mul %6,%8") /* r1:r0 = HI(f) * HI(t)*/
+ A("add %10,r0") /* %10 += LO(HI(f) * HI(t))*/
+ A("adc %11,r1") /* %11 += HI(HI(f) * HI(t))*/
+ A("mov %5,%10") /* %6:%5 =*/
+ A("mov %6,%11") /* f = %10:%11*/
/* umul16x24to24hi(v, f, bezier_A); / Range 21bits [29]*/
/* acc += v; */
- A("lds %10, bezier_A") /* %10 = LO(bezier_A)*/
- A("mul %10,%5") /* r1:r0 = LO(bezier_A) * LO(f)*/
+ A("lds %10, bezier_A") /* %10 = LO(bezier_A)*/
+ A("mul %10,%5") /* r1:r0 = LO(bezier_A) * LO(f)*/
A("add %9,r1")
A("adc %2,%0")
A("adc %3,%0")
- A("adc %4,%0") /* %4:%3:%2:%9 += HI(LO(bezier_A) * LO(f))*/
- A("lds %11, bezier_A+1") /* %11 = MI(bezier_A)*/
- A("mul %11,%5") /* r1:r0 = MI(bezier_A) * LO(f)*/
+ A("adc %4,%0") /* %4:%3:%2:%9 += HI(LO(bezier_A) * LO(f))*/
+ A("lds %11, bezier_A+1") /* %11 = MI(bezier_A)*/
+ A("mul %11,%5") /* r1:r0 = MI(bezier_A) * LO(f)*/
A("add %9,r0")
A("adc %2,r1")
A("adc %3,%0")
- A("adc %4,%0") /* %4:%3:%2:%9 += MI(bezier_A) * LO(f)*/
- A("lds %1, bezier_A+2") /* %1 = HI(bezier_A)*/
- A("mul %1,%5") /* r1:r0 = MI(bezier_A) * LO(f)*/
+ A("adc %4,%0") /* %4:%3:%2:%9 += MI(bezier_A) * LO(f)*/
+ A("lds %1, bezier_A+2") /* %1 = HI(bezier_A)*/
+ A("mul %1,%5") /* r1:r0 = MI(bezier_A) * LO(f)*/
A("add %2,r0")
A("adc %3,r1")
- A("adc %4,%0") /* %4:%3:%2:%9 += HI(bezier_A) * LO(f) << 8*/
- A("mul %10,%6") /* r1:r0 = LO(bezier_A) * MI(f)*/
+ A("adc %4,%0") /* %4:%3:%2:%9 += HI(bezier_A) * LO(f) << 8*/
+ A("mul %10,%6") /* r1:r0 = LO(bezier_A) * MI(f)*/
A("add %9,r0")
A("adc %2,r1")
A("adc %3,%0")
- A("adc %4,%0") /* %4:%3:%2:%9 += LO(bezier_A) * MI(f)*/
- A("mul %11,%6") /* r1:r0 = MI(bezier_A) * MI(f)*/
+ A("adc %4,%0") /* %4:%3:%2:%9 += LO(bezier_A) * MI(f)*/
+ A("mul %11,%6") /* r1:r0 = MI(bezier_A) * MI(f)*/
A("add %2,r0")
A("adc %3,r1")
- A("adc %4,%0") /* %4:%3:%2:%9 += MI(bezier_A) * MI(f) << 8*/
- A("mul %1,%6") /* r1:r0 = HI(bezier_A) * LO(f)*/
+ A("adc %4,%0") /* %4:%3:%2:%9 += MI(bezier_A) * MI(f) << 8*/
+ A("mul %1,%6") /* r1:r0 = HI(bezier_A) * LO(f)*/
A("add %3,r0")
- A("adc %4,r1") /* %4:%3:%2:%9 += HI(bezier_A) * LO(f) << 16*/
+ A("adc %4,r1") /* %4:%3:%2:%9 += HI(bezier_A) * LO(f) << 16*/
L("2")
" clr __zero_reg__" /* C runtime expects r1 = __zero_reg__ = 0 */
: "+r"(r0),
@@ -1110,201 +1111,104 @@ void Stepper::set_directions() {
* 2000 1 KHz - sleep rate
* 4000 500 Hz - init rate
*/
-ISR(TIMER1_COMPA_vect) {
- /**
- * On AVR there is no hardware prioritization and preemption of
- * interrupts, so this emulates it. The UART has first priority
- * (otherwise, characters will be lost due to UART overflow).
- * Then: Stepper, Endstops, Temperature, and -finally- all others.
- *
- * This ISR needs to run with as little preemption as possible, so
- * the Temperature ISR is disabled here. Now only the UART, Endstops,
- * and Arduino-defined interrupts can preempt.
- */
- const bool temp_isr_was_enabled = TEMPERATURE_ISR_ENABLED();
- DISABLE_TEMPERATURE_INTERRUPT();
- DISABLE_STEPPER_DRIVER_INTERRUPT();
- sei();
- #if ENABLED(LIN_ADVANCE)
- Stepper::advance_isr_scheduler();
- #else
- Stepper::isr();
- #endif
+HAL_STEP_TIMER_ISR {
+ HAL_timer_isr_prologue(STEP_TIMER_NUM);
- // Disable global interrupts and reenable this ISR
- cli();
- ENABLE_STEPPER_DRIVER_INTERRUPT();
- // Reenable the temperature ISR (if it was enabled)
- if (temp_isr_was_enabled) ENABLE_TEMPERATURE_INTERRUPT();
+ // Program timer compare for the maximum period, so it does NOT
+ // flag an interrupt while this ISR is running - So changes from small
+ // periods to big periods are respected and the timer does not reset to 0
+ HAL_timer_set_compare(STEP_TIMER_NUM, HAL_TIMER_TYPE_MAX);
+
+ // Call the ISR scheduler
+ hal_timer_t ticks = Stepper::isr_scheduler();
+
+ // Now 'ticks' contains the period to the next Stepper ISR.
+ // Potential problem: Since the timer continues to run, the requested
+ // compare value may already have passed.
+ //
+ // Assuming at least 6µs between calls to this ISR...
+ // On AVR the ISR epilogue is estimated at 40 instructions - close to 2.5µS.
+ // On ARM the ISR epilogue is estimated at 10 instructions - close to 200nS.
+ // In either case leave at least 4µS for other tasks to execute.
+ const hal_timer_t minticks = HAL_timer_get_count(STEP_TIMER_NUM) + hal_timer_t((HAL_TICKS_PER_US) * 4); // ISR never takes more than 1ms, so this shouldn't cause trouble
+ NOLESS(ticks, MAX(minticks, hal_timer_t((STEP_TIMER_MIN_INTERVAL) * (HAL_TICKS_PER_US))));
+
+ // Set the next ISR to fire at the proper time
+ HAL_timer_set_compare(STEP_TIMER_NUM, ticks);
+
+ HAL_timer_isr_epilogue(STEP_TIMER_NUM);
}
-void Stepper::isr() {
+#define STEP_MULTIPLY(A,B) MultiU24X32toH16(A, B)
- uint16_t ocr_val;
+hal_timer_t Stepper::isr_scheduler() {
+ uint32_t interval;
- #define ENDSTOP_NOMINAL_OCR_VAL 3000 // Check endstops every 1.5ms to guarantee two stepper ISRs within 5ms for BLTouch
- #define OCR_VAL_TOLERANCE 1000 // First max delay is 2.0ms, last min delay is 0.5ms, all others 1.5ms
+ // Run main stepping pulse phase ISR if we have to
+ if (!nextMainISR) Stepper::stepper_pulse_phase_isr();
- #define _SPLIT(L) (ocr_val = (uint16_t)L)
- #if ENABLED(ENDSTOP_INTERRUPTS_FEATURE)
-
- #define SPLIT(L) _SPLIT(L)
-
- #else // !ENDSTOP_INTERRUPTS_FEATURE : Sample endstops between stepping ISRs
-
- static uint32_t step_remaining = 0;
-
- #define SPLIT(L) do { \
- _SPLIT(L); \
- if (ENDSTOPS_ENABLED && L > ENDSTOP_NOMINAL_OCR_VAL) { \
- const uint16_t remainder = (uint16_t)L % (ENDSTOP_NOMINAL_OCR_VAL); \
- ocr_val = (remainder < OCR_VAL_TOLERANCE) ? ENDSTOP_NOMINAL_OCR_VAL + remainder : ENDSTOP_NOMINAL_OCR_VAL; \
- step_remaining = (uint16_t)L - ocr_val; \
- } \
- }while(0)
-
- if (step_remaining && ENDSTOPS_ENABLED) { // Just check endstops - not yet time for a step
- endstops.update();
-
- // Next ISR either for endstops or stepping
- ocr_val = step_remaining <= ENDSTOP_NOMINAL_OCR_VAL ? step_remaining : ENDSTOP_NOMINAL_OCR_VAL;
- step_remaining -= ocr_val;
- _NEXT_ISR(ocr_val);
- NOLESS(OCR1A, TCNT1 + 16);
- return;
- }
-
- #endif // !ENDSTOP_INTERRUPTS_FEATURE
-
- //
- // When cleaning, discard the current block and run fast
- //
- if (cleaning_buffer_counter) {
- if (cleaning_buffer_counter < 0) { // Count up for endstop hit
- if (current_block) planner.discard_current_block(); // Discard the active block that led to the trigger
- if (!planner.discard_continued_block()) // Discard next CONTINUED block
- cleaning_buffer_counter = 0; // Keep discarding until non-CONTINUED
- }
- else {
- planner.discard_current_block();
- --cleaning_buffer_counter; // Count down for abort print
- #if ENABLED(SD_FINISHED_STEPPERRELEASE) && defined(SD_FINISHED_RELEASECOMMAND)
- if (!cleaning_buffer_counter) enqueue_and_echo_commands_P(PSTR(SD_FINISHED_RELEASECOMMAND));
- #endif
- }
- current_block = NULL; // Prep to get a new block after cleaning
- _NEXT_ISR(200); // Run at max speed - 10 KHz
- return;
- }
-
- // If there is no current block, attempt to pop one from the buffer
- if (!current_block) {
-
- // Anything in the buffer?
- if ((current_block = planner.get_current_block())) {
-
- // Sync block? Sync the stepper counts and return
- while (TEST(current_block->flag, BLOCK_BIT_SYNC_POSITION)) {
- _set_position(
- current_block->steps[A_AXIS], current_block->steps[B_AXIS],
- current_block->steps[C_AXIS], current_block->steps[E_AXIS]
- );
- planner.discard_current_block();
- if (!(current_block = planner.get_current_block())) return;
- }
-
- // Initialize the trapezoid generator from the current block.
- static int8_t last_extruder = -1;
-
- #if ENABLED(LIN_ADVANCE)
- #if E_STEPPERS > 1
- if (current_block->active_extruder != last_extruder) {
- current_adv_steps = 0; // If the now active extruder wasn't in use during the last move, its pressure is most likely gone.
- LA_active_extruder = current_block->active_extruder;
- }
- #endif
-
- if ((use_advance_lead = current_block->use_advance_lead)) {
- LA_decelerate_after = current_block->decelerate_after;
- final_adv_steps = current_block->final_adv_steps;
- max_adv_steps = current_block->max_adv_steps;
- }
- #endif
-
- if (current_block->direction_bits != last_direction_bits || current_block->active_extruder != last_extruder) {
- last_direction_bits = current_block->direction_bits;
- last_extruder = current_block->active_extruder;
- set_directions();
- }
-
- // No acceleration / deceleration time elapsed so far
- acceleration_time = deceleration_time = 0;
-
- // No step events completed so far
- step_events_completed = 0;
-
- // step_rate to timer interval
- OCR1A_nominal = calc_timer_interval(current_block->nominal_rate);
-
- // make a note of the number of step loops required at nominal speed
- step_loops_nominal = step_loops;
-
- #if DISABLED(BEZIER_JERK_CONTROL)
- // Set as deceleration point the initial rate of the block
- acc_step_rate = current_block->initial_rate;
- #endif
-
- #if ENABLED(BEZIER_JERK_CONTROL)
- // Initialize the Bézier speed curve
- _calc_bezier_curve_coeffs(current_block->initial_rate, current_block->cruise_rate, current_block->acceleration_time_inverse);
-
- // We have not started the 2nd half of the trapezoid
- bezier_2nd_half = false;
- #endif
-
- // Initialize Bresenham counters to 1/2 the ceiling
- counter_X = counter_Y = counter_Z = counter_E = -(current_block->step_event_count >> 1);
- #if ENABLED(MIXING_EXTRUDER)
- MIXING_STEPPERS_LOOP(i)
- counter_m[i] = -(current_block->mix_event_count[i] >> 1);
- #endif
-
- // No step events completed so far
- step_events_completed = 0;
-
- #if ENABLED(ENDSTOP_INTERRUPTS_FEATURE)
- e_hit = 2; // Needed for the case an endstop is already triggered before the new move begins.
- // No 'change' can be detected.
- #endif
-
- #if ENABLED(Z_LATE_ENABLE)
- // If delayed Z enable, postpone move for 1mS
- if (current_block->steps[Z_AXIS] > 0) {
- enable_Z();
- _NEXT_ISR(2000); // Run at slow speed - 1 KHz
- return;
- }
- #endif
- }
- else {
- _NEXT_ISR(2000); // Run at slow speed - 1 KHz
- return;
- }
- }
-
- // Update endstops state, if enabled
- #if ENABLED(ENDSTOP_INTERRUPTS_FEATURE)
- if (e_hit && ENDSTOPS_ENABLED) {
- endstops.update();
- e_hit--;
- }
- #else
- if (ENDSTOPS_ENABLED) endstops.update();
+ #if ENABLED(LIN_ADVANCE)
+ // Run linear advance stepper ISR if we have to
+ if (!nextAdvanceISR) nextAdvanceISR = Stepper::advance_isr();
#endif
+ // ^== Time critical. NOTHING besides pulse generation should be above here!!!
+
+ // Run main stepping block processing ISR if we have to
+ if (!nextMainISR) nextMainISR = Stepper::stepper_block_phase_isr();
+
+ #if ENABLED(LIN_ADVANCE)
+ // Select the closest interval in time
+ interval = (nextAdvanceISR <= nextMainISR)
+ ? nextAdvanceISR
+ : nextMainISR;
+
+ #else // !ENABLED(LIN_ADVANCE)
+
+ // The interval is just the remaining time to the stepper ISR
+ interval = nextMainISR;
+ #endif
+
+ // Limit the value to the maximum possible value of the timer
+ if (interval > HAL_TIMER_TYPE_MAX)
+ interval = HAL_TIMER_TYPE_MAX;
+
+ // Compute the time remaining for the main isr
+ nextMainISR -= interval;
+
+ #if ENABLED(LIN_ADVANCE)
+ // Compute the time remaining for the advance isr
+ if (nextAdvanceISR != ADV_NEVER)
+ nextAdvanceISR -= interval;
+ #endif
+
+ return (hal_timer_t)interval;
+}
+
+// This part of the ISR should ONLY create the pulses for the steppers
+// -- Nothing more, nothing less -- We want to avoid jitter from where
+// the pulses should be generated (when the interrupt triggers) to the
+// time pulses are actually created. So, PLEASE DO NOT PLACE ANY CODE
+// above this line that can conditionally change that time (we are trying
+// to keep the delay between the interrupt triggering and pulse generation
+// as constant as possible!!!!
+void Stepper::stepper_pulse_phase_isr() {
+
+ // If we must abort the current block, do so!
+ if (abort_current_block) {
+ abort_current_block = false;
+ if (current_block) {
+ current_block = NULL;
+ planner.discard_current_block();
+ }
+ }
+
+ // If there is no current block, do nothing
+ if (!current_block) return;
+
// Take multiple steps per interrupt (For high speed moves)
- bool all_steps_done = false;
+ all_steps_done = false;
for (uint8_t i = step_loops; i--;) {
#define _COUNTER(AXIS) counter_## AXIS
@@ -1392,7 +1296,7 @@ void Stepper::isr() {
* 10µs = 160 or 200 cycles.
*/
#if EXTRA_CYCLES_XYZE > 20
- uint32_t pulse_start = TCNT0;
+ hal_timer_t pulse_start = HAL_timer_get_count(PULSE_TIMER_NUM);
#endif
#if HAS_X_STEP
@@ -1457,8 +1361,8 @@ void Stepper::isr() {
// For minimum pulse time wait before stopping pulses
#if EXTRA_CYCLES_XYZE > 20
- while (EXTRA_CYCLES_XYZE > (uint32_t)(TCNT0 - pulse_start) * (INT0_PRESCALER)) { /* nada */ }
- pulse_start = TCNT0;
+ while (EXTRA_CYCLES_XYZE > (uint32_t)(HAL_timer_get_count(PULSE_TIMER_NUM) - pulse_start) * (PULSE_TIMER_PRESCALE)) { /* nada */ }
+ pulse_start = HAL_timer_get_count(PULSE_TIMER_NUM);
#elif EXTRA_CYCLES_XYZE > 0
DELAY_NS(EXTRA_CYCLES_XYZE * NANOSECONDS_PER_CYCLE);
#endif
@@ -1493,120 +1397,219 @@ void Stepper::isr() {
// For minimum pulse time wait after stopping pulses also
#if EXTRA_CYCLES_XYZE > 20
- if (i) while (EXTRA_CYCLES_XYZE > (uint32_t)(TCNT0 - pulse_start) * (INT0_PRESCALER)) { /* nada */ }
+ if (i) while (EXTRA_CYCLES_XYZE > (uint32_t)(HAL_timer_get_count(PULSE_TIMER_NUM) - pulse_start) * (PULSE_TIMER_PRESCALE)) { /* nada */ }
#elif EXTRA_CYCLES_XYZE > 0
if (i) DELAY_NS(EXTRA_CYCLES_XYZE * NANOSECONDS_PER_CYCLE);
#endif
} // steps_loop
+}
- // Calculate new timer value
- if (step_events_completed <= (uint32_t)current_block->accelerate_until) {
+// This is the last half of the stepper interrupt: This one processes and
+// properly schedules blocks from the planner. This is executed after creating
+// the step pulses, so it is not time critical, as pulses are already done.
- #if ENABLED(BEZIER_JERK_CONTROL)
- // Get the next speed to use (Jerk limited!)
- uint16_t acc_step_rate =
- acceleration_time < current_block->acceleration_time
- ? _eval_bezier_curve(acceleration_time)
- : current_block->cruise_rate;
- #else
- acc_step_rate = MultiU24X32toH16(acceleration_time, current_block->acceleration_rate) + current_block->initial_rate;
- NOMORE(acc_step_rate, current_block->nominal_rate);
- #endif
+uint32_t Stepper::stepper_block_phase_isr() {
- // step_rate to timer interval
- const uint16_t interval = calc_timer_interval(acc_step_rate);
+ // If no queued movements, just wait 1ms for the next move
+ uint32_t interval = (HAL_STEPPER_TIMER_RATE / 1000);
- SPLIT(interval); // split step into multiple ISRs if larger than ENDSTOP_NOMINAL_OCR_VAL
- _NEXT_ISR(ocr_val);
+ // If there is a current block
+ if (current_block) {
- acceleration_time += interval;
+ // Calculate new timer value
+ if (step_events_completed <= current_block->accelerate_until) {
- #if ENABLED(LIN_ADVANCE)
- if (current_block->use_advance_lead) {
- if (step_events_completed == step_loops || (e_steps && eISR_Rate != current_block->advance_speed)) {
- nextAdvanceISR = 0; // Wake up eISR on first acceleration loop and fire ISR if final adv_rate is reached
- eISR_Rate = current_block->advance_speed;
+ #if ENABLED(BEZIER_JERK_CONTROL)
+ // Get the next speed to use (Jerk limited!)
+ uint32_t acc_step_rate =
+ acceleration_time < current_block->acceleration_time
+ ? _eval_bezier_curve(acceleration_time)
+ : current_block->cruise_rate;
+ #else
+ acc_step_rate = STEP_MULTIPLY(acceleration_time, current_block->acceleration_rate) + current_block->initial_rate;
+ NOMORE(acc_step_rate, current_block->nominal_rate);
+ #endif
+
+ // step_rate to timer interval
+ interval = calc_timer_interval(acc_step_rate);
+ acceleration_time += interval;
+
+ #if ENABLED(LIN_ADVANCE)
+ if (current_block->use_advance_lead) {
+ if (step_events_completed == step_loops || (e_steps && eISR_Rate != current_block->advance_speed)) {
+ nextAdvanceISR = 0; // Wake up eISR on first acceleration loop and fire ISR if final adv_rate is reached
+ eISR_Rate = current_block->advance_speed;
+ }
}
- }
- else {
- eISR_Rate = ADV_NEVER;
- if (e_steps) nextAdvanceISR = 0;
- }
- #endif // LIN_ADVANCE
- }
- else if (step_events_completed > (uint32_t)current_block->decelerate_after) {
- uint16_t step_rate;
+ else {
+ eISR_Rate = ADV_NEVER;
+ if (e_steps) nextAdvanceISR = 0;
+ }
+ #endif // LIN_ADVANCE
+ }
+ else if (step_events_completed > current_block->decelerate_after) {
+ uint32_t step_rate;
- #if ENABLED(BEZIER_JERK_CONTROL)
- // If this is the 1st time we process the 2nd half of the trapezoid...
- if (!bezier_2nd_half) {
+ #if ENABLED(BEZIER_JERK_CONTROL)
+ // If this is the 1st time we process the 2nd half of the trapezoid...
+ if (!bezier_2nd_half) {
+ // Initialize the Bézier speed curve
+ _calc_bezier_curve_coeffs(current_block->cruise_rate, current_block->final_rate, current_block->deceleration_time_inverse);
+ bezier_2nd_half = true;
+ }
+
+ // Calculate the next speed to use
+ step_rate = deceleration_time < current_block->deceleration_time
+ ? _eval_bezier_curve(deceleration_time)
+ : current_block->final_rate;
+ #else
+
+ // Using the old trapezoidal control
+ step_rate = STEP_MULTIPLY(deceleration_time, current_block->acceleration_rate);
+ if (step_rate < acc_step_rate) { // Still decelerating?
+ step_rate = acc_step_rate - step_rate;
+ NOLESS(step_rate, current_block->final_rate);
+ }
+ else
+ step_rate = current_block->final_rate;
+ #endif
+
+ // step_rate to timer interval
+ interval = calc_timer_interval(step_rate);
+ deceleration_time += interval;
+
+ #if ENABLED(LIN_ADVANCE)
+ if (current_block->use_advance_lead) {
+ if (step_events_completed <= current_block->decelerate_after + step_loops || (e_steps && eISR_Rate != current_block->advance_speed)) {
+ nextAdvanceISR = 0; // Wake up eISR on first deceleration loop
+ eISR_Rate = current_block->advance_speed;
+ }
+ }
+ else {
+ eISR_Rate = ADV_NEVER;
+ if (e_steps) nextAdvanceISR = 0;
+ }
+ #endif // LIN_ADVANCE
+ }
+ else {
+
+ #if ENABLED(LIN_ADVANCE)
+ // If there are any esteps, fire the next advance_isr "now"
+ if (e_steps && eISR_Rate != current_block->advance_speed) nextAdvanceISR = 0;
+ #endif
+
+ // The timer interval is just the nominal value for the nominal speed
+ interval = ticks_nominal;
+
+ // Ensure this runs at the correct step rate, even if it just came off an acceleration
+ step_loops = step_loops_nominal;
+ }
+
+ // If current block is finished, reset pointer
+ if (all_steps_done) {
+ current_block = NULL;
+ planner.discard_current_block();
+ }
+ }
+
+ // If there is no current block at this point, attempt to pop one from the buffer
+ // and prepare its movement
+ if (!current_block) {
+
+ // Anything in the buffer?
+ if ((current_block = planner.get_current_block())) {
+
+ // Sync block? Sync the stepper counts and return
+ while (TEST(current_block->flag, BLOCK_BIT_SYNC_POSITION)) {
+ _set_position(
+ current_block->position[A_AXIS], current_block->position[B_AXIS],
+ current_block->position[C_AXIS], current_block->position[E_AXIS]
+ );
+ planner.discard_current_block();
+
+ // Try to get a new block
+ if (!(current_block = planner.get_current_block()))
+ return interval; // No more queued movements!
+ }
+
+ // Compute movement direction for proper endstop handling
+ LOOP_NA(i) last_movement_non_null[i] = !!current_block->steps[i];
+
+ // Initialize the trapezoid generator from the current block.
+ #if ENABLED(LIN_ADVANCE)
+ #if E_STEPPERS > 1
+ if (current_block->active_extruder != last_movement_extruder) {
+ current_adv_steps = 0; // If the now active extruder wasn't in use during the last move, its pressure is most likely gone.
+ LA_active_extruder = current_block->active_extruder;
+ }
+ #endif
+
+ if ((use_advance_lead = current_block->use_advance_lead)) {
+ LA_decelerate_after = current_block->decelerate_after;
+ final_adv_steps = current_block->final_adv_steps;
+ max_adv_steps = current_block->max_adv_steps;
+ }
+ #endif
+
+ if (current_block->direction_bits != last_direction_bits || current_block->active_extruder != last_movement_extruder) {
+ last_direction_bits = current_block->direction_bits;
+ last_movement_extruder = current_block->active_extruder;
+ set_directions();
+ }
+
+ // At this point, we must ensure the movement about to execute isn't
+ // trying to force the head against a limit switch. If using interrupt-
+ // driven change detection, and already against a limit then no call to
+ // the endstop_triggered method will be done and the movement will be
+ // done against the endstop. So, check the limits here: If the movement
+ // is against the limits, the block will be marked as to be killed, and
+ // on the next call to this ISR, will be discarded.
+ endstops.check_possible_change();
+
+ // No acceleration / deceleration time elapsed so far
+ acceleration_time = deceleration_time = 0;
+
+ // No step events completed so far
+ step_events_completed = 0;
+
+ // step_rate to timer interval for the nominal speed
+ ticks_nominal = calc_timer_interval(current_block->nominal_rate);
+
+ // make a note of the number of step loops required at nominal speed
+ step_loops_nominal = step_loops;
+
+ #if DISABLED(BEZIER_JERK_CONTROL)
+ // Set as deceleration point the initial rate of the block
+ acc_step_rate = current_block->initial_rate;
+ #endif
+
+ #if ENABLED(BEZIER_JERK_CONTROL)
// Initialize the Bézier speed curve
- _calc_bezier_curve_coeffs(current_block->cruise_rate, current_block->final_rate, current_block->deceleration_time_inverse);
- bezier_2nd_half = true;
- }
+ _calc_bezier_curve_coeffs(current_block->initial_rate, current_block->cruise_rate, current_block->acceleration_time_inverse);
- // Calculate the next speed to use
- step_rate = deceleration_time < current_block->deceleration_time
- ? _eval_bezier_curve(deceleration_time)
- : current_block->final_rate;
- #else
+ // We have not started the 2nd half of the trapezoid
+ bezier_2nd_half = false;
+ #endif
- // Using the old trapezoidal control
- step_rate = MultiU24X32toH16(deceleration_time, current_block->acceleration_rate);
- if (step_rate < acc_step_rate) { // Still decelerating?
- step_rate = acc_step_rate - step_rate;
- NOLESS(step_rate, current_block->final_rate);
- }
- else
- step_rate = current_block->final_rate;
+ // Initialize Bresenham counters to 1/2 the ceiling
+ counter_X = counter_Y = counter_Z = counter_E = -((int32_t)(current_block->step_event_count >> 1));
+ #if ENABLED(MIXING_EXTRUDER)
+ MIXING_STEPPERS_LOOP(i)
+ counter_m[i] = -(current_block->mix_event_count[i] >> 1);
+ #endif
- #endif
-
- // step_rate to timer interval
- const uint16_t interval = calc_timer_interval(step_rate);
-
- SPLIT(interval); // split step into multiple ISRs if larger than ENDSTOP_NOMINAL_OCR_VAL
- _NEXT_ISR(ocr_val);
-
- deceleration_time += interval;
-
- #if ENABLED(LIN_ADVANCE)
- if (current_block->use_advance_lead) {
- if (step_events_completed <= (uint32_t)current_block->decelerate_after + step_loops || (e_steps && eISR_Rate != current_block->advance_speed)) {
- nextAdvanceISR = 0; // Wake up eISR on first deceleration loop
- eISR_Rate = current_block->advance_speed;
- }
- }
- else {
- eISR_Rate = ADV_NEVER;
- if (e_steps) nextAdvanceISR = 0;
- }
- #endif // LIN_ADVANCE
- }
- else {
-
- #if ENABLED(LIN_ADVANCE)
- // If we have esteps to execute, fire the next advance_isr "now"
- if (e_steps && eISR_Rate != current_block->advance_speed) nextAdvanceISR = 0;
- #endif
-
- SPLIT(OCR1A_nominal); // split step into multiple ISRs if larger than ENDSTOP_NOMINAL_OCR_VAL
- _NEXT_ISR(ocr_val);
-
- // ensure we're running at the correct step rate, even if we just came off an acceleration
- step_loops = step_loops_nominal;
+ #if ENABLED(Z_LATE_ENABLE)
+ // If delayed Z enable, enable it now. This option will severely interfere with
+ // timing between pulses when chaining motion between blocks, and it could lead
+ // to lost steps in both X and Y axis, so avoid using it unless strictly necessary!!
+ if (current_block->steps[Z_AXIS]) enable_Z();
+ #endif
+ }
}
- #if DISABLED(LIN_ADVANCE)
- NOLESS(OCR1A, TCNT1 + 16);
- #endif
-
- // If current block is finished, reset pointer
- if (all_steps_done) {
- current_block = NULL;
- planner.discard_current_block();
- }
+ // Return the interval to wait
+ return interval;
}
#if ENABLED(LIN_ADVANCE)
@@ -1615,8 +1618,8 @@ void Stepper::isr() {
#define EXTRA_CYCLES_E (STEP_PULSE_CYCLES - (CYCLES_EATEN_E))
// Timer interrupt for E. e_steps is set in the main routine;
-
- void Stepper::advance_isr() {
+ uint32_t Stepper::advance_isr() {
+ uint32_t interval;
#if ENABLED(MK2_MULTIPLEXER) // For SNMM even-numbered steppers are reversed
#define SET_E_STEP_DIR(INDEX) do{ if (e_steps) E0_DIR_WRITE(e_steps < 0 ? !INVERT_E## INDEX ##_DIR ^ TEST(INDEX, 0) : INVERT_E## INDEX ##_DIR ^ TEST(INDEX, 0)); }while(0)
@@ -1677,21 +1680,21 @@ void Stepper::isr() {
if (step_events_completed > LA_decelerate_after && current_adv_steps > final_adv_steps) {
e_steps--;
current_adv_steps--;
- nextAdvanceISR = eISR_Rate;
+ interval = eISR_Rate;
}
else if (step_events_completed < LA_decelerate_after && current_adv_steps < max_adv_steps) {
//step_events_completed <= (uint32_t)current_block->accelerate_until) {
e_steps++;
current_adv_steps++;
- nextAdvanceISR = eISR_Rate;
+ interval = eISR_Rate;
}
else {
- nextAdvanceISR = ADV_NEVER;
+ interval = ADV_NEVER;
eISR_Rate = ADV_NEVER;
}
}
else
- nextAdvanceISR = ADV_NEVER;
+ interval = ADV_NEVER;
switch (LA_active_extruder) {
case 0: SET_E_STEP_DIR(0); break;
@@ -1713,7 +1716,7 @@ void Stepper::isr() {
while (e_steps) {
#if EXTRA_CYCLES_E > 20
- uint32_t pulse_start = TCNT0;
+ hal_timer_t pulse_start = HAL_timer_get_count(PULSE_TIMER_NUM);
#endif
switch (LA_active_extruder) {
@@ -1734,8 +1737,8 @@ void Stepper::isr() {
// For minimum pulse time wait before stopping pulses
#if EXTRA_CYCLES_E > 20
- while (EXTRA_CYCLES_E > (uint32_t)(TCNT0 - pulse_start) * (INT0_PRESCALER)) { /* nada */ }
- pulse_start = TCNT0;
+ while (EXTRA_CYCLES_E > (hal_timer_t)(HAL_timer_get_count(PULSE_TIMER_NUM) - pulse_start) * (PULSE_TIMER_PRESCALE)) { /* nada */ }
+ pulse_start = HAL_timer_get_count(PULSE_TIMER_NUM);
#elif EXTRA_CYCLES_E > 0
DELAY_NS(EXTRA_CYCLES_E * NANOSECONDS_PER_CYCLE);
#endif
@@ -1758,45 +1761,15 @@ void Stepper::isr() {
// For minimum pulse time wait before looping
#if EXTRA_CYCLES_E > 20
- if (e_steps) while (EXTRA_CYCLES_E > (uint32_t)(TCNT0 - pulse_start) * (INT0_PRESCALER)) { /* nada */ }
+ if (e_steps) while (EXTRA_CYCLES_E > (hal_timer_t)(HAL_timer_get_count(PULSE_TIMER_NUM) - pulse_start) * (PULSE_TIMER_PRESCALE)) { /* nada */ }
#elif EXTRA_CYCLES_E > 0
if (e_steps) DELAY_NS(EXTRA_CYCLES_E * NANOSECONDS_PER_CYCLE);
#endif
} // e_steps
+
+ return interval;
}
-
- void Stepper::advance_isr_scheduler() {
-
- // Run main stepping ISR if flagged
- if (!nextMainISR) isr();
-
- // Run Advance stepping ISR if flagged
- if (!nextAdvanceISR) advance_isr();
-
- // Is the next advance ISR scheduled before the next main ISR?
- if (nextAdvanceISR <= nextMainISR) {
- // Set up the next interrupt
- OCR1A = nextAdvanceISR;
- // New interval for the next main ISR
- if (nextMainISR) nextMainISR -= nextAdvanceISR;
- // Will call Stepper::advance_isr on the next interrupt
- nextAdvanceISR = 0;
- }
- else {
- // The next main ISR comes first
- OCR1A = nextMainISR;
- // New interval for the next advance ISR, if any
- if (nextAdvanceISR && nextAdvanceISR != ADV_NEVER)
- nextAdvanceISR -= nextMainISR;
- // Will call Stepper::isr on the next interrupt
- nextMainISR = 0;
- }
-
- // Don't run the ISR faster than possible
- NOLESS(OCR1A, TCNT1 + 16);
- }
-
#endif // LIN_ADVANCE
void Stepper::init() {
@@ -1892,9 +1865,6 @@ void Stepper::init() {
if (!E_ENABLE_ON) E4_ENABLE_WRITE(HIGH);
#endif
- // Init endstops and pullups
- endstops.init();
-
#define _STEP_INIT(AXIS) AXIS ##_STEP_INIT
#define _WRITE_STEP(AXIS, HIGHLOW) AXIS ##_STEP_WRITE(HIGHLOW)
#define _DISABLE(AXIS) disable_## AXIS()
@@ -2011,31 +1981,29 @@ void Stepper::_set_position(const int32_t &a, const int32_t &b, const int32_t &c
* Get a stepper's position in steps.
*/
int32_t Stepper::position(const AxisEnum axis) {
- CRITICAL_SECTION_START;
- const int32_t count_pos = count_position[axis];
- CRITICAL_SECTION_END;
- return count_pos;
-}
-
-void Stepper::finish_and_disable() {
- planner.synchronize();
- disable_all_steppers();
-}
-
-void Stepper::quick_stop() {
- DISABLE_STEPPER_DRIVER_INTERRUPT();
- kill_current_block();
- current_block = NULL;
- cleaning_buffer_counter = 5000;
- planner.clear_block_buffer();
- ENABLE_STEPPER_DRIVER_INTERRUPT();
- #if ENABLED(ULTRA_LCD)
- planner.clear_block_buffer_runtime();
- #endif
+ // Protect the access to the position. Only required for AVR, as
+ // any 32bit CPU offers atomic access to 32bit variables
+ const bool was_enabled = STEPPER_ISR_ENABLED();
+ if (was_enabled) DISABLE_STEPPER_DRIVER_INTERRUPT();
+
+ const int32_t v = count_position[axis];
+
+ // Reenable Stepper ISR
+ if (was_enabled) ENABLE_STEPPER_DRIVER_INTERRUPT();
+ return v;
}
+// Signal endstops were triggered - This function can be called from
+// an ISR context (Temperature, Stepper or limits ISR), so we must
+// be very careful here. If the interrupt being preempted was the
+// Stepper ISR (this CAN happen with the endstop limits ISR) then
+// when the stepper ISR resumes, we must be very sure that the movement
+// is properly cancelled
void Stepper::endstop_triggered(const AxisEnum axis) {
+ const bool was_enabled = STEPPER_ISR_ENABLED();
+ if (was_enabled) DISABLE_STEPPER_DRIVER_INTERRUPT();
+
#if IS_CORE
endstops_trigsteps[axis] = 0.5f * (
@@ -2049,16 +2017,37 @@ void Stepper::endstop_triggered(const AxisEnum axis) {
#endif // !COREXY && !COREXZ && !COREYZ
- kill_current_block();
- cleaning_buffer_counter = -1; // Discard the rest of the move
+ // Discard the rest of the move if there is a current block
+ quick_stop();
+
+ if (was_enabled) ENABLE_STEPPER_DRIVER_INTERRUPT();
+}
+
+int32_t Stepper::triggered_position(const AxisEnum axis) {
+ // Protect the access to the position. Only required for AVR, as
+ // any 32bit CPU offers atomic access to 32bit variables
+ const bool was_enabled = STEPPER_ISR_ENABLED();
+ if (was_enabled) DISABLE_STEPPER_DRIVER_INTERRUPT();
+
+ const int32_t v = endstops_trigsteps[axis];
+
+ // Reenable Stepper ISR
+ if (was_enabled) ENABLE_STEPPER_DRIVER_INTERRUPT();
+
+ return v;
}
void Stepper::report_positions() {
- CRITICAL_SECTION_START;
+
+ // Protect the access to the position.
+ const bool was_enabled = STEPPER_ISR_ENABLED();
+ if (was_enabled) DISABLE_STEPPER_DRIVER_INTERRUPT();
+
const int32_t xpos = count_position[X_AXIS],
ypos = count_position[Y_AXIS],
zpos = count_position[Z_AXIS];
- CRITICAL_SECTION_END;
+
+ if (was_enabled) ENABLE_STEPPER_DRIVER_INTERRUPT();
#if CORE_IS_XY || CORE_IS_XZ || IS_DELTA || IS_SCARA
SERIAL_PROTOCOLPGM(MSG_COUNT_A);
@@ -2099,8 +2088,8 @@ void Stepper::report_positions() {
#define _APPLY_DIR(AXIS, INVERT) AXIS ##_APPLY_DIR(INVERT, true)
#if EXTRA_CYCLES_BABYSTEP > 20
- #define _SAVE_START const uint32_t pulse_start = TCNT0
- #define _PULSE_WAIT while (EXTRA_CYCLES_BABYSTEP > (uint32_t)(TCNT0 - pulse_start) * (INT0_PRESCALER)) { /* nada */ }
+ #define _SAVE_START const hal_timer_t pulse_start = HAL_timer_get_count(STEP_TIMER_NUM)
+ #define _PULSE_WAIT while (EXTRA_CYCLES_BABYSTEP > (uint32_t)(HAL_timer_get_count(STEP_TIMER_NUM) - pulse_start) * (PULSE_TIMER_PRESCALE)) { /* nada */ }
#else
#define _SAVE_START NOOP
#if EXTRA_CYCLES_BABYSTEP > 0
diff --git a/Marlin/stepper.h b/Marlin/stepper.h
index 5dec78390..1be3f94c9 100644
--- a/Marlin/stepper.h
+++ b/Marlin/stepper.h
@@ -52,11 +52,6 @@
class Stepper;
extern Stepper stepper;
-#define ENABLE_STEPPER_DRIVER_INTERRUPT() SBI(TIMSK1, OCIE1A)
-#define DISABLE_STEPPER_DRIVER_INTERRUPT() CBI(TIMSK1, OCIE1A)
-#define STEPPER_ISR_ENABLED() TEST(TIMSK1, OCIE1A)
-#define HAL_STEPPER_TIMER_RATE ((F_CPU) * 0.125)
-
// intRes = intIn1 * intIn2 >> 16
// uses:
// r26 to store 0
@@ -90,10 +85,6 @@ class Stepper {
static block_t* current_block; // A pointer to the block currently being traced
- #if ENABLED(ABORT_ON_ENDSTOP_HIT_FEATURE_ENABLED)
- static bool abort_on_endstop_hit;
- #endif
-
#if ENABLED(X_DUAL_ENDSTOPS) || ENABLED(Y_DUAL_ENDSTOPS) || ENABLED(Z_DUAL_ENDSTOPS)
static bool performing_homing;
#endif
@@ -105,11 +96,12 @@ class Stepper {
static uint32_t motor_current_setting[3];
#endif
- static int16_t cleaning_buffer_counter;
-
private:
- static uint8_t last_direction_bits; // The next stepping-bits to be output
+ static uint8_t last_direction_bits, // The next stepping-bits to be output
+ last_movement_extruder; // Last movement extruder, as computed when the last movement was fetched from planner
+ static bool abort_current_block, // Signals to the stepper that current block should be aborted
+ last_movement_non_null[NUM_AXIS]; // Last Movement in the given direction is not null, as computed when the last movement was fetched from planner
#if ENABLED(X_DUAL_ENDSTOPS)
static bool locked_x_motor, locked_x2_motor;
@@ -123,7 +115,7 @@ class Stepper {
// Counter variables for the Bresenham line tracer
static int32_t counter_X, counter_Y, counter_Z, counter_E;
- static volatile uint32_t step_events_completed; // The number of step events executed in the current block
+ static uint32_t step_events_completed; // The number of step events executed in the current block
#if ENABLED(BEZIER_JERK_CONTROL)
static int32_t bezier_A, // A coefficient in Bézier speed curve
@@ -135,12 +127,14 @@ class Stepper {
bezier_2nd_half; // If Bézier curve has been initialized or not
#endif
+ static uint32_t nextMainISR; // time remaining for the next Step ISR
+ static bool all_steps_done; // all steps done
+
#if ENABLED(LIN_ADVANCE)
static uint32_t LA_decelerate_after; // Copy from current executed block. Needed because current_block is set to NULL "too early".
- static uint16_t nextMainISR, nextAdvanceISR, eISR_Rate, current_adv_steps,
- final_adv_steps, max_adv_steps; // Copy from current executed block. Needed because current_block is set to NULL "too early".
- #define _NEXT_ISR(T) nextMainISR = T
+ static uint32_t nextAdvanceISR, eISR_Rate;
+ static uint16_t current_adv_steps, final_adv_steps, max_adv_steps; // Copy from current executed block. Needed because current_block is set to NULL "too early".
static int8_t e_steps;
static bool use_advance_lead;
#if E_STEPPERS > 1
@@ -149,18 +143,14 @@ class Stepper {
static constexpr int8_t LA_active_extruder = 0;
#endif
- #else // !LIN_ADVANCE
+ #endif // LIN_ADVANCE
- #define _NEXT_ISR(T) OCR1A = T
-
- #endif // !LIN_ADVANCE
-
- static int32_t acceleration_time, deceleration_time;
+ static uint32_t acceleration_time, deceleration_time;
static uint8_t step_loops, step_loops_nominal;
- static uint16_t OCR1A_nominal;
+ static uint32_t ticks_nominal;
#if DISABLED(BEZIER_JERK_CONTROL)
- static uint16_t acc_step_rate; // needed for deceleration start point
+ static uint32_t acc_step_rate; // needed for deceleration start point
#endif
static volatile int32_t endstops_trigsteps[XYZ];
@@ -193,88 +183,53 @@ class Stepper {
//
Stepper() { };
- //
// Initialize stepper hardware
- //
static void init();
- //
// Interrupt Service Routines
- //
- static void isr();
+ // The ISR scheduler
+ static hal_timer_t isr_scheduler();
+
+ // The stepper pulse phase ISR
+ static void stepper_pulse_phase_isr();
+
+ // The stepper block processing phase ISR
+ static uint32_t stepper_block_phase_isr();
#if ENABLED(LIN_ADVANCE)
- static void advance_isr();
- static void advance_isr_scheduler();
+ // The Linear advance stepper ISR
+ static uint32_t advance_isr();
#endif
- //
- // Set the current position in steps
- //
- static void _set_position(const int32_t &a, const int32_t &b, const int32_t &c, const int32_t &e);
-
- FORCE_INLINE static void _set_position(const AxisEnum a, const int32_t &v) { count_position[a] = v; }
-
- FORCE_INLINE static void set_position(const int32_t &a, const int32_t &b, const int32_t &c, const int32_t &e) {
- planner.synchronize();
- CRITICAL_SECTION_START;
- _set_position(a, b, c, e);
- CRITICAL_SECTION_END;
- }
-
- static void set_position(const AxisEnum a, const int32_t &v) {
- planner.synchronize();
- CRITICAL_SECTION_START;
- count_position[a] = v;
- CRITICAL_SECTION_END;
- }
-
- FORCE_INLINE static void _set_e_position(const int32_t &e) { count_position[E_AXIS] = e; }
-
- static void set_e_position(const int32_t &e) {
- planner.synchronize();
- CRITICAL_SECTION_START;
- count_position[E_AXIS] = e;
- CRITICAL_SECTION_END;
- }
-
- //
- // Set direction bits for all steppers
- //
- static void set_directions();
-
- //
// Get the position of a stepper, in steps
- //
static int32_t position(const AxisEnum axis);
- //
// Report the positions of the steppers, in steps
- //
static void report_positions();
- //
// The stepper subsystem goes to sleep when it runs out of things to execute. Call this
// to notify the subsystem that it is time to go to work.
- //
static void wake_up();
- //
- // Wait for moves to finish and disable all steppers
- //
- static void finish_and_disable();
+ // Quickly stop all steppers
+ FORCE_INLINE static void quick_stop() { abort_current_block = true; }
- //
- // Quickly stop all steppers and clear the blocks queue
- //
- static void quick_stop();
-
- //
// The direction of a single motor
- //
FORCE_INLINE static bool motor_direction(const AxisEnum axis) { return TEST(last_direction_bits, axis); }
+ // The last movement direction was not null on the specified axis. Note that motor direction is not necessarily the same.
+ FORCE_INLINE static bool movement_non_null(const AxisEnum axis) { return last_movement_non_null[axis]; }
+
+ // The extruder associated to the last movement
+ FORCE_INLINE static uint8_t movement_extruder() { return last_movement_extruder; }
+
+ // Handle a triggered endstop
+ static void endstop_triggered(const AxisEnum axis);
+
+ // Triggered position of an axis in steps
+ static int32_t triggered_position(const AxisEnum axis);
+
#if HAS_DIGIPOTSS || HAS_MOTOR_CURRENT_PWM
static void digitalPotWrite(const int16_t address, const int16_t value);
static void digipot_current(const uint8_t driver, const int16_t current);
@@ -306,32 +261,22 @@ class Stepper {
static void babystep(const AxisEnum axis, const bool direction); // perform a short step with a single stepper motor, outside of any convention
#endif
- static inline void kill_current_block() {
- step_events_completed = current_block->step_event_count;
- }
-
- //
- // Handle a triggered endstop
- //
- static void endstop_triggered(const AxisEnum axis);
-
- //
- // Triggered position of an axis in mm (not core-savvy)
- //
- FORCE_INLINE static float triggered_position_mm(const AxisEnum axis) {
- return endstops_trigsteps[axis] * planner.steps_to_mm[axis];
- }
-
#if HAS_MOTOR_CURRENT_PWM
static void refresh_motor_power();
#endif
private:
- FORCE_INLINE static uint16_t calc_timer_interval(uint16_t step_rate) {
- uint16_t timer;
+ // Set the current position in steps
+ static void _set_position(const int32_t &a, const int32_t &b, const int32_t &c, const int32_t &e);
- NOMORE(step_rate, MAX_STEP_FREQUENCY);
+ // Set direction bits for all steppers
+ static void set_directions();
+
+ FORCE_INLINE static uint32_t calc_timer_interval(uint32_t step_rate) {
+ uint32_t timer;
+
+ NOMORE(step_rate, uint32_t(MAX_STEP_FREQUENCY));
if (step_rate > 20000) { // If steprate > 20kHz >> step 4 times
step_rate >>= 2;
@@ -345,12 +290,14 @@ class Stepper {
step_loops = 1;
}
- NOLESS(step_rate, F_CPU / 500000);
+ NOLESS(step_rate, uint32_t(F_CPU / 500000U));
step_rate -= F_CPU / 500000; // Correct for minimal speed
if (step_rate >= (8 * 256)) { // higher step rate
- uint16_t table_address = (uint16_t)&speed_lookuptable_fast[(uint8_t)(step_rate >> 8)][0],
- gain = (uint16_t)pgm_read_word_near(table_address + 2);
- timer = (uint16_t)pgm_read_word_near(table_address) - MultiU16X8toH16(step_rate & 0x00FF, gain);
+ const uint8_t tmp_step_rate = (step_rate & 0x00FF);
+ const uint16_t table_address = (uint16_t)&speed_lookuptable_fast[(uint8_t)(step_rate >> 8)][0],
+ gain = (uint16_t)pgm_read_word_near(table_address + 2);
+ timer = MultiU16X8toH16(tmp_step_rate, gain);
+ timer = (uint16_t)pgm_read_word_near(table_address) - timer;
}
else { // lower step rates
uint16_t table_address = (uint16_t)&speed_lookuptable_slow[0][0];
@@ -360,9 +307,9 @@ class Stepper {
}
if (timer < 100) { // (20kHz - this should never happen)
timer = 100;
- SERIAL_PROTOCOL(MSG_STEPPER_TOO_HIGH);
- SERIAL_PROTOCOLLN(step_rate);
+ SERIAL_ECHOLNPAIR(MSG_STEPPER_TOO_HIGH, step_rate);
}
+
return timer;
}
diff --git a/Marlin/temperature.cpp b/Marlin/temperature.cpp
index e94c6b03b..f2bdfab9c 100644
--- a/Marlin/temperature.cpp
+++ b/Marlin/temperature.cpp
@@ -32,6 +32,7 @@
#include "language.h"
#include "printcounter.h"
#include "delay.h"
+#include "endstops.h"
#if ENABLED(HEATER_0_USES_MAX6675)
#include "MarlinSPI.h"
@@ -41,10 +42,6 @@
#include "stepper.h"
#endif
-#if ENABLED(ENDSTOP_INTERRUPTS_FEATURE)
- #include "endstops.h"
-#endif
-
#if ENABLED(USE_WATCHDOG)
#include "watchdog.h"
#endif
@@ -1066,9 +1063,7 @@ void Temperature::updateTemperaturesFromRawValues() {
watchdog_reset();
#endif
- CRITICAL_SECTION_START;
temp_meas_ready = false;
- CRITICAL_SECTION_END;
}
@@ -1179,43 +1174,38 @@ void Temperature::init() {
#endif // HEATER_0_USES_MAX6675
- #ifdef DIDR2
- #define ANALOG_SELECT(pin) do{ if (pin < 8) SBI(DIDR0, pin); else SBI(DIDR2, pin & 0x07); }while(0)
- #else
- #define ANALOG_SELECT(pin) do{ SBI(DIDR0, pin); }while(0)
- #endif
+ HAL_adc_init();
- // Set analog inputs
- ADCSRA = _BV(ADEN) | _BV(ADSC) | _BV(ADIF) | 0x07;
- DIDR0 = 0;
- #ifdef DIDR2
- DIDR2 = 0;
- #endif
#if HAS_TEMP_ADC_0
- ANALOG_SELECT(TEMP_0_PIN);
+ HAL_ANALOG_SELECT(TEMP_0_PIN);
#endif
#if HAS_TEMP_ADC_1
- ANALOG_SELECT(TEMP_1_PIN);
+ HAL_ANALOG_SELECT(TEMP_1_PIN);
#endif
#if HAS_TEMP_ADC_2
- ANALOG_SELECT(TEMP_2_PIN);
+ HAL_ANALOG_SELECT(TEMP_2_PIN);
#endif
#if HAS_TEMP_ADC_3
- ANALOG_SELECT(TEMP_3_PIN);
+ HAL_ANALOG_SELECT(TEMP_3_PIN);
#endif
#if HAS_TEMP_ADC_4
- ANALOG_SELECT(TEMP_4_PIN);
+ HAL_ANALOG_SELECT(TEMP_4_PIN);
#endif
#if HAS_HEATED_BED
- ANALOG_SELECT(TEMP_BED_PIN);
+ HAL_ANALOG_SELECT(TEMP_BED_PIN);
#endif
#if HAS_TEMP_CHAMBER
- ANALOG_SELECT(TEMP_CHAMBER_PIN);
+ HAL_ANALOG_SELECT(TEMP_CHAMBER_PIN);
#endif
#if ENABLED(FILAMENT_WIDTH_SENSOR)
- ANALOG_SELECT(FILWIDTH_PIN);
+ HAL_ANALOG_SELECT(FILWIDTH_PIN);
#endif
+ // Use timer0 for temperature measurement
+ // Interleave temperature interrupt with millies interrupt
+ OCR0B = 128;
+ ENABLE_TEMPERATURE_INTERRUPT();
+
#if HAS_AUTO_FAN_0
#if E0_AUTO_FAN_PIN == FAN1_PIN
SET_OUTPUT(E0_AUTO_FAN_PIN);
@@ -1277,11 +1267,6 @@ void Temperature::init() {
#endif
#endif
- // Use timer0 for temperature measurement
- // Interleave temperature interrupt with millies interrupt
- OCR0B = 128;
- ENABLE_TEMPERATURE_INTERRUPT();
-
// Wait for temperature measurement to settle
delay(250);
@@ -1792,24 +1777,14 @@ void Temperature::set_current_temp_raw() {
* - Step the babysteps value for each axis towards 0
* - For PINS_DEBUGGING, monitor and report endstop pins
* - For ENDSTOP_INTERRUPTS_FEATURE check endstops if flagged
+ * - Call planner.tick to count down its "ignore" time
*/
-ISR(TIMER0_COMPB_vect) {
- /**
- * AVR has no hardware interrupt preemption, so emulate priorization
- * and preemption of this ISR by all others by disabling the timer
- * interrupt generation capability and reenabling global interrupts.
- * Any interrupt can then interrupt this handler and preempt it.
- * This ISR becomes the lowest priority one so the UART, Endstops
- * and Stepper ISRs can all preempt it.
- */
- DISABLE_TEMPERATURE_INTERRUPT();
- sei();
+HAL_TEMP_TIMER_ISR {
+ HAL_timer_isr_prologue(TEMP_TIMER_NUM);
Temperature::isr();
- // Disable global interrupts and reenable this ISR
- cli();
- ENABLE_TEMPERATURE_INTERRUPT();
+ HAL_timer_isr_epilogue(TEMP_TIMER_NUM);
}
void Temperature::isr() {
@@ -2107,13 +2082,6 @@ void Temperature::isr() {
* This gives each ADC 0.9765ms to charge up.
*/
- #define SET_ADMUX_ADCSRA(pin) ADMUX = _BV(REFS0) | (pin & 0x07); SBI(ADCSRA, ADSC)
- #ifdef MUX5
- #define START_ADC(pin) if (pin > 7) ADCSRB = _BV(MUX5); else ADCSRB = 0; SET_ADMUX_ADCSRA(pin)
- #else
- #define START_ADC(pin) ADCSRB = 0; SET_ADMUX_ADCSRA(pin)
- #endif
-
switch (adc_sensor_state) {
case SensorsReady: {
@@ -2133,25 +2101,25 @@ void Temperature::isr() {
#if HAS_TEMP_ADC_0
case PrepareTemp_0:
- START_ADC(TEMP_0_PIN);
+ HAL_START_ADC(TEMP_0_PIN);
break;
case MeasureTemp_0:
- raw_temp_value[0] += ADC;
+ raw_temp_value[0] += HAL_READ_ADC;
break;
#endif
#if HAS_HEATED_BED
case PrepareTemp_BED:
- START_ADC(TEMP_BED_PIN);
+ HAL_START_ADC(TEMP_BED_PIN);
break;
case MeasureTemp_BED:
- raw_temp_bed_value += ADC;
+ raw_temp_bed_value += HAL_READ_ADC;
break;
#endif
#if HAS_TEMP_CHAMBER
case PrepareTemp_CHAMBER:
- START_ADC(TEMP_CHAMBER_PIN);
+ HAL_START_ADC(TEMP_CHAMBER_PIN);
break;
case MeasureTemp_CHAMBER:
raw_temp_chamber_value += ADC;
@@ -2160,55 +2128,55 @@ void Temperature::isr() {
#if HAS_TEMP_ADC_1
case PrepareTemp_1:
- START_ADC(TEMP_1_PIN);
+ HAL_START_ADC(TEMP_1_PIN);
break;
case MeasureTemp_1:
- raw_temp_value[1] += ADC;
+ raw_temp_value[1] += HAL_READ_ADC;
break;
#endif
#if HAS_TEMP_ADC_2
case PrepareTemp_2:
- START_ADC(TEMP_2_PIN);
+ HAL_START_ADC(TEMP_2_PIN);
break;
case MeasureTemp_2:
- raw_temp_value[2] += ADC;
+ raw_temp_value[2] += HAL_READ_ADC;
break;
#endif
#if HAS_TEMP_ADC_3
case PrepareTemp_3:
- START_ADC(TEMP_3_PIN);
+ HAL_START_ADC(TEMP_3_PIN);
break;
case MeasureTemp_3:
- raw_temp_value[3] += ADC;
+ raw_temp_value[3] += HAL_READ_ADC;
break;
#endif
#if HAS_TEMP_ADC_4
case PrepareTemp_4:
- START_ADC(TEMP_4_PIN);
+ HAL_START_ADC(TEMP_4_PIN);
break;
case MeasureTemp_4:
- raw_temp_value[4] += ADC;
+ raw_temp_value[4] += HAL_READ_ADC;
break;
#endif
#if ENABLED(FILAMENT_WIDTH_SENSOR)
case Prepare_FILWIDTH:
- START_ADC(FILWIDTH_PIN);
+ HAL_START_ADC(FILWIDTH_PIN);
break;
case Measure_FILWIDTH:
- if (ADC > 102) { // Make sure ADC is reading > 0.5 volts, otherwise don't read.
+ if (HAL_READ_ADC > 102) { // Make sure ADC is reading > 0.5 volts, otherwise don't read.
raw_filwidth_value -= (raw_filwidth_value >> 7); // Subtract 1/128th of the raw_filwidth_value
- raw_filwidth_value += ((unsigned long)ADC << 7); // Add new ADC reading, scaled by 128
+ raw_filwidth_value += ((unsigned long)HAL_READ_ADC << 7); // Add new ADC reading, scaled by 128
}
break;
#endif
#if ENABLED(ADC_KEYPAD)
case Prepare_ADC_KEY:
- START_ADC(ADC_KEYPAD_PIN);
+ HAL_START_ADC(ADC_KEYPAD_PIN);
break;
case Measure_ADC_KEY:
if (ADCKey_count < 16) {
@@ -2330,26 +2298,11 @@ void Temperature::isr() {
}
#endif // BABYSTEPPING
- #if ENABLED(PINS_DEBUGGING)
- extern bool endstop_monitor_flag;
- // run the endstop monitor at 15Hz
- static uint8_t endstop_monitor_count = 16; // offset this check from the others
- if (endstop_monitor_flag) {
- endstop_monitor_count += _BV(1); // 15 Hz
- endstop_monitor_count &= 0x7F;
- if (!endstop_monitor_count) endstop_monitor(); // report changes in endstop status
- }
- #endif
+ // Poll endstops state, if required
+ endstops.poll();
- #if ENABLED(ENDSTOP_INTERRUPTS_FEATURE)
-
- extern volatile uint8_t e_hit;
-
- if (e_hit && ENDSTOPS_ENABLED) {
- endstops.update(); // call endstop update routine
- e_hit--;
- }
- #endif
+ // Periodically call the planner timer
+ planner.tick();
}
#if HAS_TEMP_SENSOR
diff --git a/Marlin/ubl_motion.cpp b/Marlin/ubl_motion.cpp
index 9ffdff3bb..0e8e7b909 100644
--- a/Marlin/ubl_motion.cpp
+++ b/Marlin/ubl_motion.cpp
@@ -257,7 +257,8 @@
z_position = end[Z_AXIS];
}
- planner.buffer_segment(rx, ry, z_position + z0, e_position, feed_rate, extruder);
+ if (!planner.buffer_segment(rx, ry, z_position + z0, e_position, feed_rate, extruder))
+ break;
} //else printf("FIRST MOVE PRUNED ");
}
@@ -314,7 +315,8 @@
e_position = end[E_AXIS];
z_position = end[Z_AXIS];
}
- planner.buffer_segment(rx, next_mesh_line_y, z_position + z0, e_position, feed_rate, extruder);
+ if (!planner.buffer_segment(rx, next_mesh_line_y, z_position + z0, e_position, feed_rate, extruder))
+ break;
current_yi += dyi;
yi_cnt--;
}
@@ -337,7 +339,8 @@
z_position = end[Z_AXIS];
}
- planner.buffer_segment(next_mesh_line_x, ry, z_position + z0, e_position, feed_rate, extruder);
+ if (!planner.buffer_segment(next_mesh_line_x, ry, z_position + z0, e_position, feed_rate, extruder))
+ break;
current_xi += dxi;
xi_cnt--;
}
@@ -366,7 +369,7 @@
inline void _O2 ubl_buffer_segment_raw(const float (&in_raw)[XYZE], const float &fr) {
#if ENABLED(SKEW_CORRECTION)
- float raw[XYZE] = { in_raw[X_AXIS], in_raw[Y_AXIS], in_raw[Z_AXIS], in_raw[E_AXIS] };
+ float raw[XYZE] = { in_raw[X_AXIS], in_raw[Y_AXIS], in_raw[Z_AXIS] };
planner.skew(raw[X_AXIS], raw[Y_AXIS], raw[Z_AXIS]);
#else
const float (&raw)[XYZE] = in_raw;
@@ -438,7 +441,7 @@
uint16_t segments = lroundf(cartesian_xy_mm * (1.0 / (DELTA_SEGMENT_MIN_LENGTH))); // cartesian fixed segment length
#endif
- NOLESS(segments, 1); // must have at least one segment
+ NOLESS(segments, 1U); // must have at least one segment
const float inv_segments = 1.0 / segments; // divide once, multiply thereafter
#if IS_SCARA // scale the feed rate from mm/s to degrees/s
diff --git a/Marlin/ultralcd.cpp b/Marlin/ultralcd.cpp
index 95bef8e11..2940cbc0d 100644
--- a/Marlin/ultralcd.cpp
+++ b/Marlin/ultralcd.cpp
@@ -2393,12 +2393,10 @@ void lcd_quick_feedback(const bool clear_buttons) {
void _lcd_do_nothing() {}
void _lcd_hard_stop() {
- stepper.quick_stop();
const screenFunc_t old_screen = currentScreen;
currentScreen = _lcd_do_nothing;
- while (planner.movesplanned()) idle();
+ planner.quick_stop();
currentScreen = old_screen;
- stepper.cleaning_buffer_counter = 0;
set_current_from_steppers_for_axis(ALL_AXES);
sync_plan_position();
}
@@ -3806,7 +3804,7 @@ void lcd_quick_feedback(const bool clear_buttons) {
// M540 S - Abort on endstop hit when SD printing
#if ENABLED(ABORT_ON_ENDSTOP_HIT_FEATURE_ENABLED)
- MENU_ITEM_EDIT(bool, MSG_ENDSTOP_ABORT, &stepper.abort_on_endstop_hit);
+ MENU_ITEM_EDIT(bool, MSG_ENDSTOP_ABORT, &planner.abort_on_endstop_hit);
#endif
END_MENU();