queue and stepper init functions return bool and accept engine

gin66 · gin66 · commit 69a661606e6c · 2025-05-11T19:32:24.000+02:00
diff --git a/src/FastAccelStepper.cpp b/src/FastAccelStepper.cpp
@@ -119,8 +119,11 @@ FastAccelStepper* FastAccelStepperEngine::stepperConnectToPin(
   _stepper_cnt++;
 
   FastAccelStepper* s = &fas_stepper[fas_stepper_num];
+  bool success = s->init(this, fas_stepper_num, step_pin);
+  if (!success) {
+    return NULL;
+  }
   _stepper[fas_stepper_num] = s;
-  s->init(this, fas_stepper_num, step_pin);
   for (uint8_t i = 0; i < MAX_STEPPER; i++) {
     FastAccelStepper* sx = _stepper[i];
     if (sx) {
@@ -501,7 +504,7 @@ bool FastAccelStepper::usesAutoEnablePin(uint8_t pin) {
   return false;
 }
 
-void FastAccelStepper::init(FastAccelStepperEngine* engine, uint8_t num,
+bool FastAccelStepper::init(FastAccelStepperEngine* engine, uint8_t num,
                             uint8_t step_pin) {
 #if (TEST_MEASURE_ISR_SINGLE_FILL == 1)
   // For run time measurement
@@ -522,20 +525,14 @@ void FastAccelStepper::init(FastAccelStepperEngine* engine, uint8_t num,
   _rg.init();
 
   _queue_num = num;
-  fas_queue[_queue_num].init(_queue_num, step_pin);
-#if defined(SUPPORT_RP_PICO)
-  bool ok = fas_queue[_queue_num].claim_pio_sm(engine);
-  if (ok) {
-    fas_queue[_queue_num].setupSM();
-    fas_queue[_queue_num].connect();
-  }
-#endif
+  bool success = fas_queue[_queue_num].init(engine, _queue_num, step_pin);
 #if defined(SUPPORT_ESP32_PULSE_COUNTER) && (ESP_IDF_VERSION_MAJOR == 5)
   _attached_pulse_unit = NULL;
 #endif
 #if defined(SUPPORT_ESP32_PULSE_COUNTER) && (ESP_IDF_VERSION_MAJOR == 4)
   _attached_pulse_cnt_unit = -1;
 #endif
+  return success;
 }
 uint8_t FastAccelStepper::getStepPin() { return _stepPin; }
 void FastAccelStepper::setDirectionPin(uint8_t dirPin, bool dirHighCountsUp,
diff --git a/src/FastAccelStepper.h b/src/FastAccelStepper.h
@@ -270,7 +270,7 @@ class FastAccelStepper {
 #else
  private:
 #endif
-  void init(FastAccelStepperEngine* engine, uint8_t num, uint8_t step_pin);
+  bool init(FastAccelStepperEngine* engine, uint8_t num, uint8_t step_pin);
 
  public:
   // ## Step Pin
diff --git a/src/StepperISR.h b/src/StepperISR.h
@@ -122,7 +122,7 @@ class StepperQueue {
   struct queue_end_s queue_end;
   uint16_t max_speed_in_ticks;
 
-  void init(uint8_t queue_num, uint8_t step_pin);
+  bool init(FastAccelStepperEngine *engine, uint8_t queue_num, uint8_t step_pin);
   inline uint8_t queueEntries() {
     fasDisableInterrupts();
     uint8_t rp = read_idx;
@@ -162,7 +162,7 @@ class StepperQueue {
 
 #ifdef SUPPORT_ESP32_MCPWM_PCNT
   bool isReadyForCommands_mcpwm_pcnt();
-  void init_mcpwm_pcnt(uint8_t channel_num, uint8_t step_pin);
+  bool init_mcpwm_pcnt(uint8_t channel_num, uint8_t step_pin);
   void startQueue_mcpwm_pcnt();
   void forceStop_mcpwm_pcnt();
   uint16_t _getPerformedPulses_mcpwm_pcnt();
@@ -171,7 +171,7 @@ class StepperQueue {
 #endif
 #ifdef SUPPORT_ESP32_RMT
   bool isReadyForCommands_rmt();
-  void init_rmt(uint8_t channel_num, uint8_t step_pin);
+  bool init_rmt(uint8_t channel_num, uint8_t step_pin);
   void startQueue_rmt();
 #if ESP_IDF_VERSION_MAJOR == 4
   void stop_rmt(bool both);
diff --git a/src/StepperISR_avr.cpp b/src/StepperISR_avr.cpp
@@ -115,7 +115,7 @@ StepperQueue fas_queue[NUM_QUEUES];
     /* ensure cyclic interrupt is running */       \
     EnableOverflowInterrupt(T);                    \
   }
-void StepperQueue::init(uint8_t queue_num, uint8_t step_pin) {
+bool StepperQueue::init(FastAccelStepperEngine *engine, uint8_t queue_num, uint8_t step_pin) {
   prepareISRtimeMeasurement();
   _initVars();
   digitalWrite(step_pin, LOW);
@@ -124,16 +124,20 @@ void StepperQueue::init(uint8_t queue_num, uint8_t step_pin) {
     channel = channelA;
     AVR_INIT(FAS_TIMER_MODULE, A)
   }
-  if (step_pin == stepPinStepperB) {
+  else if (step_pin == stepPinStepperB) {
     channel = channelB;
     AVR_INIT(FAS_TIMER_MODULE, B)
   }
 #ifdef stepPinStepperC
-  if (step_pin == stepPinStepperC) {
+  else if (step_pin == stepPinStepperC) {
     channel = channelC;
     AVR_INIT(FAS_TIMER_MODULE, C)
   }
 #endif
+  else {
+    return false;
+  }
+  return true;
 }
 
 // The interrupt is called on compare event, which eventually
diff --git a/src/StepperISR_due.cpp b/src/StepperISR_due.cpp
@@ -452,7 +452,7 @@ inline uint32_t pinToChannel(uint32_t pin) {
   return 0xFFFFFFFF;
 }
 
-void StepperQueue::init(uint8_t queue_num, uint8_t step_pin) {
+bool StepperQueue::init(FastAccelStepperEngine *engine, uint8_t queue_num, uint8_t step_pin) {
   _queue_num = queue_num;
   driver_data = (void*)&gChannelMap[queue_num];
   _initVars();
@@ -521,6 +521,7 @@ void StepperQueue::init(uint8_t queue_num, uint8_t step_pin) {
   PWM_INTERFACE->PWM_IDR1 = 0x00FFFF0F;
 
   connect();
+  return true;
 }
 
 void StepperQueue::connect() {
diff --git a/src/StepperISR_esp32.cpp b/src/StepperISR_esp32.cpp
@@ -5,20 +5,20 @@
 // Here are the global variables to interface with the interrupts
 StepperQueue fas_queue[NUM_QUEUES];
 
-void StepperQueue::init(uint8_t queue_num, uint8_t step_pin) {
+bool StepperQueue::init(FastAccelStepperEngine *engine, uint8_t queue_num, uint8_t step_pin) {
   uint8_t channel = queue_num;
 #ifdef SUPPORT_ESP32_MCPWM_PCNT
   if (channel < QUEUES_MCPWM_PCNT) {
     use_rmt = false;
-    init_mcpwm_pcnt(channel, step_pin);
-    return;
+    return init_mcpwm_pcnt(channel, step_pin);
   }
   channel -= QUEUES_MCPWM_PCNT;
 #endif
 #ifdef SUPPORT_ESP32_RMT
   use_rmt = true;
-  init_rmt(channel, step_pin);
+  return init_rmt(channel, step_pin);
 #endif
+  return false;
 }
 
 void StepperQueue::connect() {
diff --git a/src/StepperISR_idf4_esp32_mcpwm_pcnt.cpp b/src/StepperISR_idf4_esp32_mcpwm_pcnt.cpp
@@ -344,7 +344,7 @@ static void IRAM_ATTR mcpwm1_isr_service(void *arg) {
 #endif
 }
 
-void StepperQueue::init_mcpwm_pcnt(uint8_t channel_num, uint8_t step_pin) {
+bool StepperQueue::init_mcpwm_pcnt(uint8_t channel_num, uint8_t step_pin) {
 #ifdef TEST_PROBE
   pinMode(TEST_PROBE, OUTPUT);
 #endif
@@ -474,6 +474,7 @@ void StepperQueue::init_mcpwm_pcnt(uint8_t channel_num, uint8_t step_pin) {
 
   // at last, link mcpwm to output pin and back into pcnt input
   connect();
+  return true;
 }
 
 void StepperQueue::connect_mcpwm_pcnt() {
diff --git a/src/StepperISR_idf4_esp32_rmt.cpp b/src/StepperISR_idf4_esp32_rmt.cpp
@@ -257,7 +257,7 @@ static void IRAM_ATTR tx_intr_handler(void *arg) {
 #endif
 }
 
-void StepperQueue::init_rmt(uint8_t channel_num, uint8_t step_pin) {
+bool StepperQueue::init_rmt(uint8_t channel_num, uint8_t step_pin) {
 #ifdef TEST_PROBE_1
   if (channel_num == 0) {
     pinMode(TEST_PROBE_1, OUTPUT);
@@ -359,6 +359,7 @@ void StepperQueue::init_rmt(uint8_t channel_num, uint8_t step_pin) {
     }
   }
 #endif
+  return true;
 }
 
 void StepperQueue::connect_rmt() {
diff --git a/src/StepperISR_idf4_esp32c3_rmt.cpp b/src/StepperISR_idf4_esp32c3_rmt.cpp
@@ -318,7 +318,7 @@ static void IRAM_ATTR tx_intr_handler(void *arg) {
 #endif
 }
 
-void StepperQueue::init_rmt(uint8_t channel_num, uint8_t step_pin) {
+bool StepperQueue::init_rmt(uint8_t channel_num, uint8_t step_pin) {
 #ifdef TEST_PROBE_1
   if (channel_num == 0) {
     pinMode(TEST_PROBE_1, OUTPUT);
@@ -444,6 +444,7 @@ void StepperQueue::init_rmt(uint8_t channel_num, uint8_t step_pin) {
     }
   }
 #endif
+  return true;
 }
 
 void StepperQueue::connect_rmt() {
diff --git a/src/StepperISR_idf4_esp32s3_rmt.cpp b/src/StepperISR_idf4_esp32s3_rmt.cpp
@@ -318,7 +318,7 @@ static void IRAM_ATTR tx_intr_handler(void *arg) {
 #endif
 }
 
-void StepperQueue::init_rmt(uint8_t channel_num, uint8_t step_pin) {
+bool StepperQueue::init_rmt(uint8_t channel_num, uint8_t step_pin) {
 #ifdef TEST_PROBE_1
   if (channel_num == 0) {
     pinMode(TEST_PROBE_1, OUTPUT);
@@ -444,6 +444,7 @@ void StepperQueue::init_rmt(uint8_t channel_num, uint8_t step_pin) {
     }
   }
 #endif
+  return true;
 }
 
 void StepperQueue::connect_rmt() {
diff --git a/src/StepperISR_idf5_esp32_rmt.cpp b/src/StepperISR_idf5_esp32_rmt.cpp
@@ -186,7 +186,7 @@ static size_t IRAM_ATTR encode_commands(const void *data, size_t data_size,
   return PART_SIZE;
 }
 
-void StepperQueue::init_rmt(uint8_t channel_num, uint8_t step_pin) {
+bool StepperQueue::init_rmt(uint8_t channel_num, uint8_t step_pin) {
 #ifdef TEST_PROBE_1
   if (channel_num == 0) {
     pinMode(TEST_PROBE_1, OUTPUT);
@@ -223,6 +223,7 @@ void StepperQueue::init_rmt(uint8_t channel_num, uint8_t step_pin) {
   connect_rmt();
   _isRunning = false;
   _rmtStopped = true;
+  return true;
 }
 
 void StepperQueue::connect_rmt() {
diff --git a/src/StepperISR_rp_pico.cpp b/src/StepperISR_rp_pico.cpp
@@ -10,9 +10,15 @@
 StepperQueue fas_queue[NUM_QUEUES];
 static stepper_pio_program *program;
 
-void StepperQueue::init(uint8_t queue_num, uint8_t step_pin) {
+bool StepperQueue::init(FastAccelStepperEngine *engine, uint8_t queue_num, uint8_t step_pin) {
   uint8_t channel = queue_num;
   _step_pin = step_pin;
+  bool ok = claim_pio_sm(engine);
+  if (ok) {
+    setupSM();
+    connect();
+  }
+  return ok;
 }
 
 bool StepperQueue::claim_pio_sm(FastAccelStepperEngine *engine) {
@@ -83,14 +89,14 @@ bool StepperQueue::isReadyForCommands() {
   return true;
 }
 
-static void push_command(StepperQueue *q) {
+static bool push_command(StepperQueue *q) {
   uint8_t rp = q->read_idx;
   if (rp == q->next_write_idx) {
     // no command in queue
-    return;
+    return false;
   }
   if (pio_sm_is_tx_fifo_full(q->pio,q->sm)) {
-    return;
+    return false;
   }
   // Process command
   struct queue_entry *e_curr = &q->entry[rp & QUEUE_LEN_MASK];
@@ -103,17 +109,14 @@ static void push_command(StepperQueue *q) {
   pio_sm_put(q->pio, q->sm, entry);
   rp++;
   q->read_idx = rp;
+  return true;
 }
 
 void StepperQueue::startQueue() {
   pio_sm_set_enabled(pio, sm, true); // sm is running, otherwise loop() stops
   pio_sm_clear_fifos(pio, sm);
   pio_sm_restart(pio, sm);
-  push_command(this);
-  push_command(this);
-  push_command(this);
-  push_command(this);
-  push_command(this);
+  while(push_command(this)) {};
 }
 void StepperQueue::forceStop() {
   pio_sm_set_enabled(pio, sm, false);
@@ -145,7 +148,7 @@ void FastAccelStepperEngine::pushCommands() {
       FastAccelStepper* s = _stepper[i];
       if (s) {
         StepperQueue* q = &fas_queue[s->_queue_num];
-        push_command(q);
+        while(push_command(q)) {};
       }
     }
 }

Original file line number	Diff line number	Diff line change
`@@ -452,7 +452,7 @@ inline uint32_t pinToChannel(uint32_t pin) {`
`452`	`452`	`return 0xFFFFFFFF;`
`453`	`453`	`}`
`454`	`454`
`455`		`-void StepperQueue::init(uint8_t queue_num, uint8_t step_pin) {`
	`455`	`+bool StepperQueue::init(FastAccelStepperEngine *engine, uint8_t queue_num, uint8_t step_pin) {`
`456`	`456`	`_queue_num = queue_num;`
`457`	`457`	`driver_data = (void*)&gChannelMap[queue_num];`
`458`	`458`	`_initVars();`
`@@ -521,6 +521,7 @@ void StepperQueue::init(uint8_t queue_num, uint8_t step_pin) {`
`521`	`521`	`PWM_INTERFACE->PWM_IDR1 = 0x00FFFF0F;`
`522`	`522`
`523`	`523`	`connect();`
	`524`	`+ return true;`
`524`	`525`	`}`
`525`	`526`
`526`	`527`	`void StepperQueue::connect() {`
Original file line number	Diff line number	Diff line change
`@@ -344,7 +344,7 @@ static void IRAM_ATTR mcpwm1_isr_service(void *arg) {`
`344`	`344`	`#endif`
`345`	`345`	`}`
`346`	`346`
`347`		`-void StepperQueue::init_mcpwm_pcnt(uint8_t channel_num, uint8_t step_pin) {`
	`347`	`+bool StepperQueue::init_mcpwm_pcnt(uint8_t channel_num, uint8_t step_pin) {`
`348`	`348`	`#ifdef TEST_PROBE`
`349`	`349`	`pinMode(TEST_PROBE, OUTPUT);`
`350`	`350`	`#endif`
`@@ -474,6 +474,7 @@ void StepperQueue::init_mcpwm_pcnt(uint8_t channel_num, uint8_t step_pin) {`
`474`	`474`
`475`	`475`	`// at last, link mcpwm to output pin and back into pcnt input`
`476`	`476`	`connect();`
	`477`	`+ return true;`
`477`	`478`	`}`
`478`	`479`
`479`	`480`	`void StepperQueue::connect_mcpwm_pcnt() {`
Original file line number	Diff line number	Diff line change
`@@ -257,7 +257,7 @@ static void IRAM_ATTR tx_intr_handler(void *arg) {`
`257`	`257`	`#endif`
`258`	`258`	`}`
`259`	`259`
`260`		`-void StepperQueue::init_rmt(uint8_t channel_num, uint8_t step_pin) {`
	`260`	`+bool StepperQueue::init_rmt(uint8_t channel_num, uint8_t step_pin) {`
`261`	`261`	`#ifdef TEST_PROBE_1`
`262`	`262`	`if (channel_num == 0) {`
`263`	`263`	`pinMode(TEST_PROBE_1, OUTPUT);`
`@@ -359,6 +359,7 @@ void StepperQueue::init_rmt(uint8_t channel_num, uint8_t step_pin) {`
`359`	`359`	`}`
`360`	`360`	`}`
`361`	`361`	`#endif`
	`362`	`+ return true;`
`362`	`363`	`}`
`363`	`364`
`364`	`365`	`void StepperQueue::connect_rmt() {`
Original file line number	Diff line number	Diff line change
`@@ -318,7 +318,7 @@ static void IRAM_ATTR tx_intr_handler(void *arg) {`
`318`	`318`	`#endif`
`319`	`319`	`}`
`320`	`320`
`321`		`-void StepperQueue::init_rmt(uint8_t channel_num, uint8_t step_pin) {`
	`321`	`+bool StepperQueue::init_rmt(uint8_t channel_num, uint8_t step_pin) {`
`322`	`322`	`#ifdef TEST_PROBE_1`
`323`	`323`	`if (channel_num == 0) {`
`324`	`324`	`pinMode(TEST_PROBE_1, OUTPUT);`
`@@ -444,6 +444,7 @@ void StepperQueue::init_rmt(uint8_t channel_num, uint8_t step_pin) {`
`444`	`444`	`}`
`445`	`445`	`}`
`446`	`446`	`#endif`
	`447`	`+ return true;`
`447`	`448`	`}`
`448`	`449`
`449`	`450`	`void StepperQueue::connect_rmt() {`