Add Resonance Test to standard Motion

Marlin/src/feature/resonance/resonance_generator.cpp

@@ -24,55 +24,102 @@
 
 #if ENABLED(RESONANCE_TEST)
 
-#include "../../module/ft_motion.h"
+#if ENABLED(FT_MOTION)
+  #include "../../module/ft_motion.h"
+#endif
+
+#if HAS_STANDARD_MOTION
+  #include "../../module/stepper.h"
+#endif
+
 #include "resonance_generator.h"
 #include "../../gcode/gcode.h" // for home_all_axes
 
-#include <math.h>
-
 resonance_test_params_t ResonanceGenerator::rt_params;     // Resonance test parameters
 
 bool ResonanceGenerator::active = false;                       // Resonance test active
 bool ResonanceGenerator::done = false;                         // Resonance test done
-float ResonanceGenerator::rt_time = FTM_TS;                    // Resonance test timer
+float ResonanceGenerator::rt_time;                             // Resonance test timer
 float ResonanceGenerator::timeline = 0.0f;
 float ResonanceGenerator::amplitude_precalc;
-float ResonanceGenerator::phase = 0.0f;
 float ResonanceGenerator::freq_mul;
+float ResonanceGenerator::phase = 0.0f;
+
+#if HAS_STANDARD_MOTION
+  block_t ResonanceGenerator::block;
+#endif
 
 ResonanceGenerator rtg;
 
 ResonanceGenerator::ResonanceGenerator() {}
 
 void ResonanceGenerator::start() {
-  gcode.home_all_axes(); // For safety and ensure known axes
-
-  // Safe Acceleration per Hz for Z axis
-  if (rt_params.axis == Z_AXIS && rt_params.accel_per_hz > 15.0f)
-    rt_params.accel_per_hz = 15.0f;
+  gcode.home_all_axes(); // Always home axes first
 
   // Always move to the center of the bed
   do_blocking_move_to_xy(X_CENTER, Y_CENTER, Z_CLEARANCE_FOR_HOMING);
       
   rt_params.start_pos = current_position;
-  rt_time = FTM_TS;
   active = true;
   done = false;
+
+  #if ENABLED(FT_MOTION)
+    if (ftMotion.cfg.active)
+      rt_time = FTM_TS;
+    #if HAS_STANDARD_MOTION
+      else
+        rt_time = 0.001f;
+    #endif
+  #else
+    rt_time = 0.001f;
+  #endif
+
+  // Safe Acceleration per Hz for Z axis
+  if (rt_params.axis == Z_AXIS && rt_params.accel_per_hz > 15.0f)
+    rt_params.accel_per_hz = 15.0f;
+
+  #if HAS_STANDARD_MOTION
+    if (TERN1(FT_MOTION, !ftMotion.cfg.active)){ 
+      // Constant speed to be adjusted if necessary
+      block.reset();
+      block.initial_rate = (rt_params.axis == Z_AXIS) ? 2000 : 8000;
+    }
+  #endif
+ 
   // Precompute sine sweep const
   amplitude_precalc = (rt_params.amplitude_correction * rt_params.accel_per_hz * 0.25f) / sq(M_PI);
   current_freq = rt_params.min_freq;
-  const float inv_octave_duration = 1.0f / rt_params.octave_duration;
-  freq_mul = exp2f(FTM_TS * inv_octave_duration);
+  freq_mul = exp2f(rt_time / rt_params.octave_duration);
 }
 
 void ResonanceGenerator::abort() {
   reset();
-  ftMotion.reset();
+  #if(HAS_STANDARD_MOTION)
+    if (!TERN0(FT_MOTION, ftMotion.cfg.active))
+      return;
+  #endif
+  #if ENABLED(FT_MOTION)
+    ftMotion.reset();
+  #endif    
 }
 
 void ResonanceGenerator::reset() {
   rt_params = resonance_test_params_t();
-  rt_time = FTM_TS;
+
+  #if ENABLED(FT_MOTION)
+    if (ftMotion.cfg.active) {
+      rt_time = FTM_TS;
+    }
+    #if HAS_STANDARD_MOTION
+      else {
+        rt_time = 0.001f;
+        block.reset();
+      }
+    #endif
+  #else 
+    rt_time = 0.001f;
+    block.reset();
+  #endif
   active = false;
   done = false;
 }
@@ -92,23 +139,59 @@ float ResonanceGenerator::calc_next_pos() {
   return rt_params.start_pos[rt_params.axis] + amplitude * r * (1.0f - 0.101321184f * r2);
 }
 
-void ResonanceGenerator::fill_stepper_plan_buffer() {
-  xyze_float_t traj_coords = rt_params.start_pos;
+#if ENABLED(FT_MOTION)
+  void ResonanceGenerator::fill_stepper_plan_buffer() {
+    xyze_pos_t traj_coords = rt_params.start_pos;
 
-  while (!ftMotion.stepping.is_full()) {
+    while (!ftMotion.stepping.is_full()) {
+      // Calculate current frequency
+      current_freq *= freq_mul;
+      if (current_freq > rt_params.max_freq) {
+        done = true;
+        return;
+      }
+
+      // Resonate the axis being tested
+      traj_coords[rt_params.axis] = calc_next_pos();
+
+      // Store in buffer
+      ftMotion.stepping_enqueue(traj_coords);
+    }
+  }
+#endif
+
+#if HAS_STANDARD_MOTION
+  block_t *ResonanceGenerator::generate_resonance_block() {
+    const float step_mm = planner.settings.axis_steps_per_mm[rt_params.axis];
+    static float last_pos = 0.0f;
+    
     // Calculate current frequency
     current_freq *= freq_mul;
     if (current_freq > rt_params.max_freq) {
       done = true;
-      return;
+      return nullptr;
     }
+    
+    // Calculate next point position
+    const float target_pos = calc_next_pos();
 
-    // Resonate the axis being tested
-    traj_coords[rt_params.axis] = calc_next_pos();
+    // mm → steps
+    const float delta_mm = (target_pos - last_pos);
+    const int32_t delta_steps = abs(lround(delta_mm * step_mm));
+    last_pos = target_pos;
+    // Steps for target axis
+    block.steps[rt_params.axis] = delta_steps;
+    // Total event count
+    block.step_event_count = delta_steps;
 
-    // Store in buffer
-    ftMotion.stepping_enqueue(traj_coords);
-  }
-}
+    // Direction
+    if (delta_mm > 0)
+      block.direction_bits &= ~(1 << rt_params.axis);
+    else
+      block.direction_bits |= (1 << rt_params.axis);
+
+    return &block;
+  } 
+#endif
 
 #endif // RESONANCE_TEST

Marlin/src/feature/resonance/resonance_generator.h

@@ -25,6 +25,10 @@
 
 #include <math.h>
 
+#if HAS_STANDARD_MOTION
+#include "../../module/planner.h"
+#endif
+
 #ifndef M_TAU
   #define M_TAU (2.0f * M_PI)
 #endif
@@ -41,7 +45,7 @@ typedef struct ResonanceTestParams {
 
 class ResonanceGenerator {
   public:
-    static resonance_test_params_t rt_params; // Resonance test parameters
+    static resonance_test_params_t rt_params;     // Resonance test parameters
     static float timeline;                        // Timeline Value to calculate resonance frequency
 
     ResonanceGenerator();
@@ -56,7 +60,13 @@ class ResonanceGenerator {
       return rt_params.min_freq * exp2f(timeline / rt_params.octave_duration);
     }
 
-    void fill_stepper_plan_buffer();                // Fill stepper plan buffer with trajectory points
+    #if HAS_STANDARD_MOTION
+      block_t *generate_resonance_block();  // Generate planner block for standard motion
+    #endif
+
+    #if ENABLED(FT_MOTION)
+      void fill_stepper_plan_buffer();                // Fill stepper plan buffer with trajectory points
+    #endif
 
     void setActive(const bool state) { active = state; }
     bool isActive() const { return active; }
@@ -67,12 +77,15 @@ class ResonanceGenerator {
     void abort();             // Abort resonance test
 
   private:
-    float calc_next_pos();            // Calculate next position
+    float calc_next_pos();            // Calculate next position point based on current frequency
     static float rt_time;             // Test timer
     static float freq_mul;            // Frequency multiplier for sine sweeping
     static float amplitude_precalc;   // Precalculated part of amplitude formula
     float current_freq;               // Current frequency being generated in sinusoidal motion
     static float phase;               // Current phase in radians
+    #if HAS_STANDARD_MOTION
+      static block_t block;
+    #endif
     static bool active;               // Resonance test active
     static bool done;                 // Resonance test done
 };

Marlin/src/gcode/feature/resonance/M495_M496.cpp

@@ -26,7 +26,6 @@
 
 #include "../../gcode.h"
 #include "../../../lcd/marlinui.h"
-#include "../../../module/ft_motion.h"
 #include "../../../feature/resonance/resonance_generator.h"
 
 void say_resonance_test() {
@@ -146,23 +145,18 @@ void GcodeSuite::M495() {
   }
 
   if (parser.seen_test('S')) {
-    if (ftMotion.cfg.active) {
-      if (p.axis != NO_AXIS_ENUM) {
-        if (p.max_freq > p.min_freq) {
-          SERIAL_ECHOLN(F("Starting "), F("Resonance Test"));
-          rtg.start();
-          // The function returns immediately, the test runs in the background.
-        }
-        else {
-          SERIAL_ECHOLNPGM("?End Frequency must be greater than Start Frequency");
-        }
+    if (p.axis != NO_AXIS_ENUM) {
+      if (p.max_freq > p.min_freq) {
+        SERIAL_ECHOLN(F("Starting "), F("Resonance Test"));
+        rtg.start();
+        // The function returns immediately, the test runs in the background.
       }
       else {
-        SERIAL_ECHOLN(F("?Specify X, Y, or Z axis"), F(" first"));
+        SERIAL_ECHOLNPGM("?End Frequency must be greater than Start Frequency");
       }
     }
     else {
-      SERIAL_ECHOLN(F("?Activate FT Motion to run the "), F("Resonance Test"));
+      SERIAL_ECHOLN(F("?Specify X, Y, or Z axis"), F(" first"));
     }
   }
 }

Marlin/src/lcd/menu/menu_motion.cpp

@@ -559,7 +559,6 @@ void menu_move() {
       EDIT_ITEM(bool, MSG_FTM_AXIS_SYNC, &editable.state, []{
         queue.inject(TS(F("M493"), IAXIS_CHAR(MenuItemBase::itemIndex), 'T', int(editable.state)));
       });
-
     }
 
     END_MENU();

Marlin/src/module/stepper.cpp

@@ -81,6 +81,10 @@ Stepper stepper; // Singleton
   #include "ft_motion.h"
 #endif
 
+#if (ENABLED(RESONANCE_TEST) && HAS_STANDARD_MOTION)
+  #include "../feature/resonance/resonance_generator.h"
+#endif
+
 #include "../lcd/marlinui.h"
 #include "../gcode/queue.h"
 #include "../sd/cardreader.h"
@@ -618,6 +622,103 @@ bool Stepper::disable_axis(const AxisEnum axis) {
   return can_disable;
 }
 
+#if (ENABLED(RESONANCE_TEST) && HAS_STANDARD_MOTION)
+  hal_timer_t Stepper::resonance_block_phase_isr() {
+
+    const hal_timer_t time_spent = HAL_timer_get_count(MF_TIMER_STEP);
+    #if MULTISTEPPING_LIMIT > 1
+      if (steps_per_isr > 1 && time_spent_out_isr >= time_spent_in_isr + time_spent)
+        steps_per_isr >>= 1;
+    #endif
+    time_spent_in_isr = -time_spent;    // Unsigned but guaranteed to be +ve when needed
+    time_spent_out_isr = 0;
+
+    hal_timer_t interval = 0;
+
+    // If current block is finished, reset pointer and finalize state
+    if (step_events_completed >= step_event_count)
+      discard_current_block();
+    else
+      return calc_multistep_timer_interval(current_block->initial_rate);
+
+    // If there is no current block at this point, generate a new block
+    if (!current_block){
+      if ((current_block = rtg.generate_resonance_block())) {
+        
+        //Apply direction
+        DIR_WAIT_BEFORE();
+        const uint8_t axis = rtg.rt_params.axis;
+        const bool fwd = current_block->direction_bits[axis];
+        switch (axis) {
+          case X_AXIS: X_APPLY_DIR(fwd, false);
+            break;
+          case Y_AXIS: Y_APPLY_DIR(fwd, false);
+            break;
+          case Z_AXIS: Z_APPLY_DIR(fwd, false);
+            break;
+        }
+
+        step_event_count = current_block->step_event_count;
+
+        // No step events completed so far
+        step_events_completed = 0;
+        interval = calc_multistep_timer_interval(current_block->initial_rate);
+      }
+      else
+        rtg.abort();
+    }
+    // Return the interval to wait
+    return interval;
+  }
+
+  void Stepper::resonance_pulse_phase_isr() {
+
+    // If there is no current block, do nothing
+    if (!current_block || step_events_completed >= step_event_count) return;
+
+    // Count of pending loops and events for this iteration
+    const uint32_t pending_events = step_event_count - step_events_completed;
+    uint8_t events_to_do = _MIN(pending_events, steps_per_isr);
+
+    // Just update the value we will get at the end of the loop
+    step_events_completed += events_to_do;
+
+    USING_TIMED_PULSE();
+
+    // Take multiple steps per interrupt. For high speed moves.
+    bool firstStep = true;
+    const uint8_t axis = rtg.rt_params.axis;
+
+    do {
+
+      if (firstStep)
+        firstStep = false;
+      
+      switch (axis) {
+      case X_AXIS:
+        X_APPLY_STEP(STEP_STATE_X, false);
+        START_TIMED_PULSE();
+        AWAIT_HIGH_PULSE();
+        X_APPLY_STEP(!STEP_STATE_X, false);
+        break;
+      case Y_AXIS:
+        Y_APPLY_STEP(STEP_STATE_Y, false);
+        START_TIMED_PULSE();
+        AWAIT_HIGH_PULSE();
+        Y_APPLY_STEP(!STEP_STATE_Y, false);
+        break;
+      case Z_AXIS:
+        Z_APPLY_STEP(STEP_STATE_Z, false);
+        START_TIMED_PULSE();
+        AWAIT_HIGH_PULSE();
+        Z_APPLY_STEP(!STEP_STATE_Z, false);
+        break;
+      }
+
+    } while (--events_to_do);
+  }
+#endif
+
 #if HAS_EXTRUDERS
 
   void Stepper::enable_extruder(E_TERN_(const uint8_t eindex)) {
@@ -1646,6 +1747,19 @@ void Stepper::isr() {
     #if HAS_STANDARD_MOTION
 
       if (!using_ftMotion) {
+        #if ENABLED(RESONANCE_TEST)
+          if (rtg.isActive()) {
+            if (!nextMainISR) resonance_pulse_phase_isr();
+
+            hal.isr_on();
+            if (!nextMainISR) nextMainISR = resonance_block_phase_isr();
+
+            interval = hal_timer_t(STEPPER_TIMER_RATE * 0.03);                  // Max wait of 30ms regardless of stepper timer frequency
+            NOMORE(interval, nextMainISR);                                      // Time until the next Pulse / Block phase
+            nextMainISR -= interval;
+          }
+          else {
+        #endif
 
         TERN_(HAS_ZV_SHAPING, shaping_isr());               // Do Shaper stepping, if needed
 
@@ -1709,6 +1823,7 @@ void Stepper::isr() {
         TERN_(BABYSTEPPING, if (nextBabystepISR != BABYSTEP_NEVER) nextBabystepISR -= interval);
 
       }
+    TERN_(RESONANCE_TEST, })
 
     #endif // HAS_STANDARD_MOTION
 

Marlin/src/module/stepper.h

@@ -579,6 +579,11 @@ class Stepper {
       static hal_timer_t block_phase_isr();
     #endif
 
+    #if (ENABLED(RESONANCE_TEST) && HAS_STANDARD_MOTION)
+      static void resonance_pulse_phase_isr();
+      static hal_timer_t resonance_block_phase_isr();
+    #endif
+
     #if HAS_ZV_SHAPING
       static void shaping_isr();
     #endif