From 28f46a4d645517753cc777621e6220e4648b535e Mon Sep 17 00:00:00 2001 From: Beslan Date: Sat, 2 May 2026 12:41:52 +0300 Subject: [PATCH] =?UTF-8?q?=D0=9C=D0=B8=D0=B3=D1=80=D0=B0=D1=86=D0=B8?= =?UTF-8?q?=D1=8F=20FastAccelStepper=20=D0=BD=D0=B0=20ESP-IDF=206?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- CMakeLists.txt | 24 +- extras/scripts/build-idf-platformio.sh | 0 extras/scripts/build-pio-dirs.sh | 0 extras/scripts/build-platformio.sh | 0 extras/scripts/format_code.sh | 0 extras/scripts/header2markdown.sh | 0 extras/tests/esp32_hw_based/seq_01a.sh | 0 extras/tests/esp32_hw_based/seq_01b.sh | 0 extras/tests/esp32_hw_based/seq_01c.sh | 0 extras/tests/esp32_hw_based/seq_02.sh | 0 extras/tests/esp32_hw_based/seq_03.sh | 0 extras/tests/esp32_hw_based/seq_04.sh | 0 extras/tests/esp32_hw_based/seq_05.sh | 0 extras/tests/esp32_hw_based/seq_06.sh | 0 extras/tests/esp32_hw_based/seq_07a.sh | 0 extras/tests/esp32_hw_based/seq_07b.sh | 0 extras/tests/esp32_hw_based/seq_07c.sh | 0 extras/tests/esp32_hw_based/seq_xxx.sh.off | 0 src/FastAccelStepper.cpp | 301 ++++++++++++++---- src/FastAccelStepper.h | 175 +++++++--- src/FastAccelStepper_idf4_esp32_pcnt.cpp | 11 +- src/FastAccelStepper_idf5_esp32_pcnt.cpp | 13 +- ...oorManFloat.cpp => Log2Representation.cpp} | 34 +- src/Log2Representation.h | 45 +++ src/Log2RepresentationConst.h | 73 +++++ src/PoorManFloat.h | 45 --- src/PoorManFloatConst.h | 73 ----- src/RampCalculator.cpp | 30 +- src/RampCalculator.h | 94 +++--- ...pConstAcceleration.cpp => RampControl.cpp} | 24 +- ...{RampConstAcceleration.h => RampControl.h} | 4 +- src/RampGenerator.cpp | 21 +- src/RampGenerator.h | 18 +- src/StepperISR.cpp | 37 +-- src/StepperISR.h | 41 ++- src/StepperISR_avr.cpp | 39 ++- src/StepperISR_due.cpp | 8 +- src/StepperISR_esp32.cpp | 28 +- src/StepperISR_esp32xx_rmt.cpp | 254 +++++++++++++++ src/StepperISR_idf4_esp32_mcpwm_pcnt.cpp | 3 +- src/StepperISR_idf4_esp32_rmt.cpp | 167 +--------- src/StepperISR_idf4_esp32c3_rmt.cpp | 185 +---------- src/StepperISR_idf4_esp32s3_rmt.cpp | 184 +---------- src/StepperISR_idf5_esp32_rmt.cpp | 163 ++-------- src/StepperISR_rp_pico.cpp | 228 +++++++++++++ src/fas_arch/arduino_rp_pico.h | 13 + src/fas_arch/common.h | 15 +- src/fas_arch/common_esp32.h | 7 +- src/fas_arch/common_esp32_idf4.h | 9 + src/fas_arch/common_esp32_idf5.h | 30 +- src/fas_arch/common_esp32_idf6.h | 174 ++++++++++ src/fas_arch/common_rp_pico.h | 40 +++ src/fas_arch/espidf_esp32.h | 7 +- src/fas_arch/result_codes.h | 171 ++++++++++ src/fas_arch/test_pc.h | 3 +- src/fas_arch/test_probe.h | 19 +- src/pico_pio.cpp | 181 +++++++++++ src/pico_pio.h | 13 + 58 files changed, 1909 insertions(+), 1095 deletions(-) mode change 100755 => 100644 extras/scripts/build-idf-platformio.sh mode change 100755 => 100644 extras/scripts/build-pio-dirs.sh mode change 100755 => 100644 extras/scripts/build-platformio.sh mode change 100755 => 100644 extras/scripts/format_code.sh mode change 100755 => 100644 extras/scripts/header2markdown.sh mode change 100755 => 100644 extras/tests/esp32_hw_based/seq_01a.sh mode change 100755 => 100644 extras/tests/esp32_hw_based/seq_01b.sh mode change 100755 => 100644 extras/tests/esp32_hw_based/seq_01c.sh mode change 100755 => 100644 extras/tests/esp32_hw_based/seq_02.sh mode change 100755 => 100644 extras/tests/esp32_hw_based/seq_03.sh mode change 100755 => 100644 extras/tests/esp32_hw_based/seq_04.sh mode change 100755 => 100644 extras/tests/esp32_hw_based/seq_05.sh mode change 100755 => 100644 extras/tests/esp32_hw_based/seq_06.sh mode change 100755 => 100644 extras/tests/esp32_hw_based/seq_07a.sh mode change 100755 => 100644 extras/tests/esp32_hw_based/seq_07b.sh mode change 100755 => 100644 extras/tests/esp32_hw_based/seq_07c.sh mode change 100755 => 100644 extras/tests/esp32_hw_based/seq_xxx.sh.off rename src/{PoorManFloat.cpp => Log2Representation.cpp} (94%) create mode 100644 src/Log2Representation.h create mode 100644 src/Log2RepresentationConst.h delete mode 100644 src/PoorManFloat.h delete mode 100644 src/PoorManFloatConst.h rename src/{RampConstAcceleration.cpp => RampControl.cpp} (96%) rename src/{RampConstAcceleration.h => RampControl.h} (97%) create mode 100644 src/StepperISR_esp32xx_rmt.cpp create mode 100644 src/StepperISR_rp_pico.cpp create mode 100644 src/fas_arch/arduino_rp_pico.h create mode 100644 src/fas_arch/common_esp32_idf6.h create mode 100644 src/fas_arch/common_rp_pico.h create mode 100644 src/fas_arch/result_codes.h create mode 100644 src/pico_pio.cpp create mode 100644 src/pico_pio.h diff --git a/CMakeLists.txt b/CMakeLists.txt index 093736f..42cb7e6 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,6 +1,24 @@ idf_component_register( - SRC_DIRS "src" + SRCS + "src/FastAccelStepper.cpp" + "src/Log2Representation.cpp" + "src/RampCalculator.cpp" + "src/RampControl.cpp" + "src/RampGenerator.cpp" + "src/StepperISR.cpp" + "src/StepperISR_esp32.cpp" + "src/StepperISR_esp32xx_rmt.cpp" + "src/StepperISR_idf5_esp32_rmt.cpp" INCLUDE_DIRS "src" - REQUIRES arduino-esp32 - PRIV_REQUIRES driver soc + REQUIRES + esp_driver_gpio + esp_driver_rmt + esp_hal_gpio + esp_hal_rmt + esp_rom + esp_system + esp_timer + freertos + hal + soc ) diff --git a/extras/scripts/build-idf-platformio.sh b/extras/scripts/build-idf-platformio.sh old mode 100755 new mode 100644 diff --git a/extras/scripts/build-pio-dirs.sh b/extras/scripts/build-pio-dirs.sh old mode 100755 new mode 100644 diff --git a/extras/scripts/build-platformio.sh b/extras/scripts/build-platformio.sh old mode 100755 new mode 100644 diff --git a/extras/scripts/format_code.sh b/extras/scripts/format_code.sh old mode 100755 new mode 100644 diff --git a/extras/scripts/header2markdown.sh b/extras/scripts/header2markdown.sh old mode 100755 new mode 100644 diff --git a/extras/tests/esp32_hw_based/seq_01a.sh b/extras/tests/esp32_hw_based/seq_01a.sh old mode 100755 new mode 100644 diff --git a/extras/tests/esp32_hw_based/seq_01b.sh b/extras/tests/esp32_hw_based/seq_01b.sh old mode 100755 new mode 100644 diff --git a/extras/tests/esp32_hw_based/seq_01c.sh b/extras/tests/esp32_hw_based/seq_01c.sh old mode 100755 new mode 100644 diff --git a/extras/tests/esp32_hw_based/seq_02.sh b/extras/tests/esp32_hw_based/seq_02.sh old mode 100755 new mode 100644 diff --git a/extras/tests/esp32_hw_based/seq_03.sh b/extras/tests/esp32_hw_based/seq_03.sh old mode 100755 new mode 100644 diff --git a/extras/tests/esp32_hw_based/seq_04.sh b/extras/tests/esp32_hw_based/seq_04.sh old mode 100755 new mode 100644 diff --git a/extras/tests/esp32_hw_based/seq_05.sh b/extras/tests/esp32_hw_based/seq_05.sh old mode 100755 new mode 100644 diff --git a/extras/tests/esp32_hw_based/seq_06.sh b/extras/tests/esp32_hw_based/seq_06.sh old mode 100755 new mode 100644 diff --git a/extras/tests/esp32_hw_based/seq_07a.sh b/extras/tests/esp32_hw_based/seq_07a.sh old mode 100755 new mode 100644 diff --git a/extras/tests/esp32_hw_based/seq_07b.sh b/extras/tests/esp32_hw_based/seq_07b.sh old mode 100755 new mode 100644 diff --git a/extras/tests/esp32_hw_based/seq_07c.sh b/extras/tests/esp32_hw_based/seq_07c.sh old mode 100755 new mode 100644 diff --git a/extras/tests/esp32_hw_based/seq_xxx.sh.off b/extras/tests/esp32_hw_based/seq_xxx.sh.off old mode 100755 new mode 100644 diff --git a/src/FastAccelStepper.cpp b/src/FastAccelStepper.cpp index 341b907..6495649 100644 --- a/src/FastAccelStepper.cpp +++ b/src/FastAccelStepper.cpp @@ -14,26 +14,35 @@ static uint8_t fas_ledPin = PIN_UNDEFINED; static uint16_t fas_debug_led_cnt = 0; // dynamic allocation seems to not work so well on avr +#if !defined(SUPPORT_STEPPER_CONNECT_BY_DRIVER) FastAccelStepper fas_stepper[MAX_STEPPER]; +#endif //************************************************************************************************* //************************************************************************************************* -void FastAccelStepperEngine::init() { - _externalCallForPin = nullptr; - _stepper_cnt = 0; - fas_init_engine(this, 255); - for (auto & i : _stepper) { - i = nullptr; - } -} - #if defined(SUPPORT_CPU_AFFINITY) void FastAccelStepperEngine::init(uint8_t cpu_core) { - _externalCallForPin = nullptr; + _externalCallForPin = NULL; _stepper_cnt = 0; + for (uint8_t i = 0; i < MAX_STEPPER; i++) { + _stepper[i] = NULL; + } fas_init_engine(this, cpu_core); } +#else +void FastAccelStepperEngine::init() { + _externalCallForPin = NULL; + _stepper_cnt = 0; + for (uint8_t i = 0; i < MAX_STEPPER; i++) { + _stepper[i] = NULL; + } +#if defined(SUPPORT_RP_PICO) + claimed_pios = 0; #endif + fas_init_engine(this); +} +#endif + void FastAccelStepperEngine::setExternalCallForPin( bool (*func)(uint8_t pin, uint8_t value)) { _externalCallForPin = func; @@ -64,25 +73,26 @@ bool FastAccelStepperEngine::isDirPinBusy(uint8_t dir_pin, FastAccelStepper* FastAccelStepperEngine::stepperConnectToPin(uint8_t step_pin) #else FastAccelStepper* FastAccelStepperEngine::stepperConnectToPin( - uint8_t step_pin, uint8_t driver_type) + uint8_t step_pin, FasDriver driver_type) #endif { // Check if already connected - for (auto s : _stepper) { + for (uint8_t i = 0; i < MAX_STEPPER; i++) { + FastAccelStepper* s = _stepper[i]; if (s) { if (s->getStepPin() == step_pin) { - return nullptr; + return NULL; } } } if (!_isValidStepPin(step_pin)) { - return nullptr; + return NULL; } #if !defined(SUPPORT_SELECT_DRIVER_TYPE) int8_t fas_stepper_num = StepperQueue::queueNumForStepPin(step_pin); if (fas_stepper_num < 0) { // flexible, so just choose next if (_stepper_cnt >= MAX_STEPPER) { - return nullptr; + return NULL; } fas_stepper_num = _stepper_cnt; } @@ -109,9 +119,13 @@ 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 (auto sx : _stepper) { + for (uint8_t i = 0; i < MAX_STEPPER; i++) { + const FastAccelStepper* sx = _stepper[i]; if (sx) { fas_queue[sx->_queue_num].adjustSpeedToStepperCount(_stepper_cnt); } @@ -137,7 +151,8 @@ void FastAccelStepperEngine::manageSteppers() { } } #endif - for (auto s : _stepper) { + for (uint8_t i = 0; i < MAX_STEPPER; i++) { + FastAccelStepper* s = _stepper[i]; if (s) { #ifdef SUPPORT_EXTERNAL_DIRECTION_PIN if (s->externalDirPinChangeCompletedIfNeeded()) { @@ -174,7 +189,8 @@ void FastAccelStepperEngine::manageSteppers() { } } if (agree) { - for (auto current : _stepper) { + for (uint8_t j = 0; j < MAX_STEPPER; j++) { + FastAccelStepper* current = _stepper[j]; if (current) { if (current->usesAutoEnablePin(high_active_pin) || current->usesAutoEnablePin(low_active_pin)) { @@ -191,7 +207,8 @@ void FastAccelStepperEngine::manageSteppers() { } // Update the auto disable counters - for (auto s : _stepper) { + for (uint8_t i = 0; i < MAX_STEPPER; i++) { + FastAccelStepper* s = _stepper[i]; if (s) { fasDisableInterrupts(); // update the counters down to 1 @@ -216,11 +233,11 @@ void FastAccelStepperEngine::manageSteppers() { //************************************************************************************************* //************************************************************************************************* -int8_t FastAccelStepper::addQueueEntry(const struct stepper_command_s* cmd, - bool start) { +AqeResultCode FastAccelStepper::addQueueEntry( + const struct stepper_command_s* cmd, bool start) { StepperQueue* q = &fas_queue[_queue_num]; - if (cmd == nullptr) { - return q->addQueueEntry(nullptr, start); + if (cmd == NULL) { + return q->addQueueEntry(NULL, start); } if (cmd->ticks < q->max_speed_in_ticks) { return AQE_ERROR_TICKS_TOO_LOW; @@ -228,7 +245,7 @@ int8_t FastAccelStepper::addQueueEntry(const struct stepper_command_s* cmd, if (_dirPin != PIN_UNDEFINED) { if (!isQueueRunning()) { - if (_engine != nullptr) { + if (_engine != NULL) { if (_engine->isDirPinBusy(_dirPin, _queue_num)) { return AQE_DIR_PIN_IS_BUSY; } @@ -240,7 +257,7 @@ int8_t FastAccelStepper::addQueueEntry(const struct stepper_command_s* cmd, } } - int res = AQE_OK; + AqeResultCode res = AQE_OK; if (_autoEnable) { fasDisableInterrupts(); uint16_t delay_counter = _auto_disable_delay_counter; @@ -273,7 +290,7 @@ int8_t FastAccelStepper::addQueueEntry(const struct stepper_command_s* cmd, q->addQueueEntry(&start_cmd, false); delay -= ticks_u16; } - res = q->addQueueEntry(nullptr, start); + res = q->addQueueEntry(NULL, start); if (res != AQE_OK) { return res; } @@ -384,7 +401,7 @@ void FastAccelStepper::fill_queue() { // For run time measurement uint32_t runtime_us = micros(); #endif - int8_t res = AQE_OK; + AqeResultCode res = AQE_OK; _rg.getNextCommand(&q->queue_end, &cmd); if (cmd.command.ticks != 0) { res = addQueueEntry(&cmd.command, !delayed_start); @@ -410,13 +427,13 @@ void FastAccelStepper::fill_queue() { break; } if (res != AQE_OK) { - if (res > 0) { + if (aqeRetry(res)) { // try later again break; } else { #ifdef SIM_TEST_INPUT Serial.println("Abort ramp due to queue error res="); - Serial.print(res); + Serial.print(static_cast(res)); Serial.print(" Steps="); Serial.print(cmd.command.steps); Serial.print(" ticks="); @@ -425,7 +442,7 @@ void FastAccelStepper::fill_queue() { Serial.println(MIN_CMD_TICKS); #endif #ifdef TEST - printf("ERROR: Abort ramp due to queue error (%d)\n", res); + printf("ERROR: Abort ramp due to queue error: %s\n", toString(res)); printf("steps=%d ticks=%d limit=%ld state=%d\n", cmd.command.steps, cmd.command.ticks, MIN_CMD_TICKS, cmd.rw.ramp_state); assert(false); @@ -436,7 +453,7 @@ void FastAccelStepper::fill_queue() { } } if (need_delayed_start) { - addQueueEntry(nullptr, true); + addQueueEntry(NULL, true); } } @@ -478,7 +495,7 @@ bool FastAccelStepper::needAutoDisable() { return need_disable; } -bool FastAccelStepper::usesAutoEnablePin(uint8_t pin) const { +bool FastAccelStepper::usesAutoEnablePin(uint8_t pin) { if (pin != PIN_UNDEFINED) { if ((pin == _enablePinHighActive) || (pin == _enablePinLowActive)) { return true; @@ -487,7 +504,7 @@ bool FastAccelStepper::usesAutoEnablePin(uint8_t pin) const { 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 @@ -508,15 +525,16 @@ void FastAccelStepper::init(FastAccelStepperEngine* engine, uint8_t num, _rg.init(); _queue_num = num; - fas_queue[_queue_num].init(_queue_num, step_pin); + 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 = nullptr; + _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() const { return _stepPin; } +uint8_t FastAccelStepper::getStepPin() { return _stepPin; } void FastAccelStepper::setDirectionPin(uint8_t dirPin, bool dirHighCountsUp, uint16_t dir_change_delay_us) { _dirPin = dirPin; @@ -581,7 +599,7 @@ void FastAccelStepper::setAutoEnable(bool auto_enable) { _off_delay_count = 1; } } -int8_t FastAccelStepper::setDelayToEnable(uint32_t delay_us) { +DelayResultCode FastAccelStepper::setDelayToEnable(uint32_t delay_us) { uint32_t delay_ticks = US_TO_TICKS(delay_us); if (delay_ticks > 0) { if (delay_ticks < MIN_CMD_TICKS) { @@ -602,10 +620,10 @@ void FastAccelStepper::setDelayToDisable(uint16_t delay_ms) { } _off_delay_count = fas_max(delay_count, (uint16_t)1); } -int8_t FastAccelStepper::runForward() { return _rg.startRun(true); } -int8_t FastAccelStepper::runBackward() { return _rg.startRun(false); } -int8_t FastAccelStepper::moveTo(int32_t position, bool blocking) { - int8_t res = _rg.moveTo(position, &fas_queue[_queue_num].queue_end); +MoveResultCode FastAccelStepper::runForward() { return _rg.startRun(true); } +MoveResultCode FastAccelStepper::runBackward() { return _rg.startRun(false); } +MoveResultCode FastAccelStepper::moveTo(int32_t position, bool blocking) { + MoveResultCode res = _rg.moveTo(position, &fas_queue[_queue_num].queue_end); if ((res == MOVE_OK) && blocking) { while (isRunning()) { noop_or_wait; @@ -613,11 +631,11 @@ int8_t FastAccelStepper::moveTo(int32_t position, bool blocking) { } return res; } -int8_t FastAccelStepper::move(int32_t move, bool blocking) { +MoveResultCode FastAccelStepper::move(int32_t move, bool blocking) { if ((move < 0) && (_dirPin == PIN_UNDEFINED)) { return MOVE_ERR_NO_DIRECTION_PIN; } - int8_t res = _rg.move(move, &fas_queue[_queue_num].queue_end); + MoveResultCode res = _rg.move(move, &fas_queue[_queue_num].queue_end); if ((res == MOVE_OK) && blocking) { while (isRunning()) { noop_or_wait; @@ -630,9 +648,9 @@ void FastAccelStepper::stopMove() { _rg.initiateStop(); } void FastAccelStepper::applySpeedAcceleration() { _rg.applySpeedAcceleration(); } -int8_t FastAccelStepper::moveByAcceleration(int32_t acceleration, - bool allow_reverse) { - int8_t res = MOVE_OK; +MoveResultCode FastAccelStepper::moveByAcceleration(int32_t acceleration, + bool allow_reverse) { + MoveResultCode res = MOVE_OK; if (acceleration > 0) { setAcceleration(acceleration); res = runForward(); @@ -685,7 +703,7 @@ bool FastAccelStepper::disableOutputs() { bool disabled = true; if (_enablePinLowActive != PIN_UNDEFINED) { if (_enablePinLowActive & PIN_EXTERNAL_FLAG) { - if (_engine->_externalCallForPin != nullptr) { + if (_engine->_externalCallForPin != NULL) { disabled &= (_engine->_externalCallForPin(_enablePinLowActive, HIGH) == HIGH); } @@ -695,7 +713,7 @@ bool FastAccelStepper::disableOutputs() { } if (_enablePinHighActive != PIN_UNDEFINED) { if (_enablePinHighActive & PIN_EXTERNAL_FLAG) { - if (_engine->_externalCallForPin != nullptr) { + if (_engine->_externalCallForPin != NULL) { disabled &= (_engine->_externalCallForPin(_enablePinHighActive, LOW) == LOW); } @@ -712,7 +730,7 @@ bool FastAccelStepper::enableOutputs() { bool enabled = true; if (_enablePinLowActive != PIN_UNDEFINED) { if (_enablePinLowActive & PIN_EXTERNAL_FLAG) { - if (_engine->_externalCallForPin != nullptr) { + if (_engine->_externalCallForPin != NULL) { enabled &= (_engine->_externalCallForPin(_enablePinLowActive, LOW) == LOW); } @@ -722,7 +740,7 @@ bool FastAccelStepper::enableOutputs() { } if (_enablePinHighActive != PIN_UNDEFINED) { if (_enablePinHighActive & PIN_EXTERNAL_FLAG) { - if (_engine->_externalCallForPin != nullptr) { + if (_engine->_externalCallForPin != NULL) { enabled &= (_engine->_externalCallForPin(_enablePinHighActive, HIGH) == HIGH); } @@ -732,7 +750,7 @@ bool FastAccelStepper::enableOutputs() { } return enabled; } -int32_t FastAccelStepper::getPositionAfterCommandsCompleted() const { +int32_t FastAccelStepper::getPositionAfterCommandsCompleted() { return fas_queue[_queue_num].queue_end.pos; } uint32_t FastAccelStepper::getPeriodInTicksAfterCommandsCompleted() { @@ -784,26 +802,26 @@ int32_t FastAccelStepper::getCurrentSpeedInMilliHz(bool realtime) { } return 0; } -uint16_t FastAccelStepper::getMaxSpeedInTicks() const { +uint16_t FastAccelStepper::getMaxSpeedInTicks() { return fas_queue[_queue_num].getMaxSpeedInTicks(); } -uint16_t FastAccelStepper::getMaxSpeedInUs() const { +uint16_t FastAccelStepper::getMaxSpeedInUs() { uint16_t ticks = getMaxSpeedInTicks(); uint16_t speed_in_us = ticks / (TICKS_PER_S / 1000000); return speed_in_us; } -uint32_t FastAccelStepper::getMaxSpeedInHz() const { +uint32_t FastAccelStepper::getMaxSpeedInHz() { uint16_t ticks = getMaxSpeedInTicks(); uint32_t speed_in_hz = TICKS_PER_S / ticks; return speed_in_hz; } -uint32_t FastAccelStepper::getMaxSpeedInMilliHz() const { +uint32_t FastAccelStepper::getMaxSpeedInMilliHz() { uint16_t ticks = getMaxSpeedInTicks(); uint32_t speed_in_milli_hz = ((uint32_t)250 * TICKS_PER_S) / ticks * 4; return speed_in_milli_hz; } #if SUPPORT_UNSAFE_ABS_SPEED_LIMIT_SETTING == 1 -void FastAccelStepper::setAbsoluteSpeedLimit(uint16_t max_speed_in_ticks) const { +void FastAccelStepper::setAbsoluteSpeedLimit(uint16_t max_speed_in_ticks) { fas_queue[_queue_num].setAbsoluteSpeedLimit(max_speed_in_ticks); } #endif @@ -858,22 +876,22 @@ void FastAccelStepper::setPositionAfterCommandsCompleted(int32_t new_pos) { } fasEnableInterrupts(); } -uint8_t FastAccelStepper::queueEntries() const { +uint8_t FastAccelStepper::queueEntries() { return fas_queue[_queue_num].queueEntries(); } uint32_t FastAccelStepper::ticksInQueue() { return fas_queue[_queue_num].ticksInQueue(); } -bool FastAccelStepper::hasTicksInQueue(uint32_t min_ticks) const { +bool FastAccelStepper::hasTicksInQueue(uint32_t min_ticks) { return fas_queue[_queue_num].hasTicksInQueue(min_ticks); } -bool FastAccelStepper::isQueueFull() const { +bool FastAccelStepper::isQueueFull() { return fas_queue[_queue_num].isQueueFull(); } -bool FastAccelStepper::isQueueEmpty() const { +bool FastAccelStepper::isQueueEmpty() { return fas_queue[_queue_num].isQueueEmpty(); } -bool FastAccelStepper::isQueueRunning() const { +bool FastAccelStepper::isQueueRunning() { return fas_queue[_queue_num].isRunning(); } bool FastAccelStepper::isRunning() { @@ -899,8 +917,161 @@ void FastAccelStepper::forwardStep(bool blocking) { void FastAccelStepper::backwardStep(bool blocking) { performOneStep(false, blocking); } -int32_t FastAccelStepper::getCurrentPosition() const { +int32_t FastAccelStepper::getCurrentPosition() { return fas_queue[_queue_num].getCurrentPosition(); } -void FastAccelStepper::detachFromPin() const { fas_queue[_queue_num].disconnect(); } -void FastAccelStepper::reAttachToPin() const { fas_queue[_queue_num].connect(); } +MoveTimedResultCode FastAccelStepper::moveTimed(int16_t steps, + uint32_t duration, + uint32_t* actual_duration, + bool start) { + MoveTimedResultCode ret_ok = + isQueueEmpty() ? MOVE_TIMED_EMPTY : MOVE_TIMED_OK; + if ((steps == 0) && (duration == 0)) { + if (start) { + addQueueEntry(NULL, true); // start the queue + } + return ret_ok; + } + uint8_t freeEntries = QUEUE_LEN - queueEntries(); + if (actual_duration) { + *actual_duration = 0; + } + struct stepper_command_s cmd = {.ticks = 0, .steps = 0, .count_up = true}; + if (steps == 0) { + if ((duration >> 16) >= QUEUE_LEN) { + return MOVE_TIMED_TOO_LARGE_ERROR; + } + if ((duration >> 16) >= freeEntries) { + return MOVE_TIMED_BUSY; + } + // Should fit + while (duration > 0) { + if (duration <= 65535) { + // done using one command + cmd.ticks = duration; + } else if (duration >= 131072) { + // need more than one command + cmd.ticks = 65535; + } else { + // just use half of the duration now, and the other half in the next + // cmd. + cmd.ticks = duration >> 1; + } + AqeResultCode ret = addQueueEntry(&cmd, start); + if (ret != AQE_OK) { + // unexpected + return tmrFrom(ret); + } + if (actual_duration) { + *actual_duration += cmd.ticks; + } + duration -= cmd.ticks; + } + return ret_ok; + } + + // let's evaluate the direction + if (steps < 0) { + cmd.count_up = false; + steps = -steps; + } + + // There are steps to execute + // Let's first calculate the step rate + uint32_t rate = duration; + rate /= steps; + if (rate > 65535) { + // we need pauses, so only few steps can be executed + uint16_t cmds_per_step = (rate >> 16) + 1; // bit too small + if (cmds_per_step >= QUEUE_LEN) { + return MOVE_TIMED_TOO_LARGE_ERROR; + } + if (steps >= QUEUE_LEN) { + return MOVE_TIMED_TOO_LARGE_ERROR; + } + uint8_t cmds = steps * cmds_per_step; + if (cmds >= QUEUE_LEN) { + return MOVE_TIMED_TOO_LARGE_ERROR; + } + if (cmds > freeEntries) { + return MOVE_TIMED_BUSY; + } + // Should fit into the queue. + for (uint8_t s = 0; s < steps; s++) { + uint32_t this_duration = rate; + cmd.steps = 1; + while (this_duration) { + if (this_duration > 131072) { + cmd.ticks = 65535; + } else if (this_duration > 65535) { + cmd.ticks = this_duration / 2; + } else { + cmd.ticks = this_duration; + } + this_duration -= cmd.ticks; + + AqeResultCode ret = addQueueEntry(&cmd, start); + if (ret != AQE_OK) { + // unexpected + return tmrFrom(ret); + } + if (actual_duration) { + *actual_duration += cmd.ticks; + } + // remaining are pauses + cmd.steps = 0; + } + } + return ret_ok; + } + // Now we need to run steps at "high" speed. + if (steps > QUEUE_LEN * 255) { + return MOVE_TIMED_TOO_LARGE_ERROR; + } + if (steps > freeEntries * 255) { + return MOVE_TIMED_BUSY; + } + // The steps should fit in + cmd.ticks = rate; + uint32_t expected_duration = rate; + expected_duration *= steps; + // duration must be larger than expected_duration + int16_t missing = duration - expected_duration; +#ifdef TEST + assert(duration >= expected_duration); +#endif + while (steps > 0) { + if (steps > 510) { + cmd.steps = 255; + } else if (steps > 255) { + cmd.steps = steps / 2; + } else { + cmd.steps = steps; + } + if (steps <= missing) { + // run the remaining steps bit slower to adjust for missing ticks + cmd.ticks++; + missing = 0; // only increase once +#ifdef TEST + printf("increase ticks for %d steps\n", steps); +#endif + } + AqeResultCode ret = addQueueEntry(&cmd, start); + if (ret != AQE_OK) { + // unexpected + return tmrFrom(ret); + } + // Why has this been calculated before and actual_duration is used ? + // uint32_t cmd_duration = cmd.ticks; + // cmd_duration *= cmd.steps; + if (actual_duration) { + uint32_t d = cmd.ticks; + d *= steps; + *actual_duration += d; + } + steps -= cmd.steps; + } + return ret_ok; +} +void FastAccelStepper::detachFromPin() { fas_queue[_queue_num].disconnect(); } +void FastAccelStepper::reAttachToPin() { fas_queue[_queue_num].connect(); } diff --git a/src/FastAccelStepper.h b/src/FastAccelStepper.h index cef3110..b92a8c2 100644 --- a/src/FastAccelStepper.h +++ b/src/FastAccelStepper.h @@ -1,7 +1,7 @@ #ifndef FASTACCELSTEPPER_H #define FASTACCELSTEPPER_H #include -#include "PoorManFloat.h" +#include "Log2Representation.h" #include "fas_arch/common.h" // # FastAccelStepper @@ -63,14 +63,14 @@ class FastAccelStepperEngine { // } // ``` - void init(); - #if defined(SUPPORT_CPU_AFFINITY) // In a multitasking and multicore system like ESP32, the steppers are // controlled by a continuously running task. This task can be fixed to one // CPU core with this modified init()-call. ESP32 implementation detail: For // values 0 and 1, xTaskCreatePinnedToCore() is used, or else xTaskCreate() - void init(uint8_t cpu_core); + void init(uint8_t cpu_core = 255); +#else + void init(); #endif // ### Creation of FastAccelStepper @@ -87,11 +87,40 @@ class FastAccelStepperEngine { // One using mcpwm and pcnt module. And another using rmt module. // This call allows to select the respective driver #if defined(SUPPORT_SELECT_DRIVER_TYPE) -#define DRIVER_MCPWM_PCNT 0 -#define DRIVER_RMT 1 -#define DRIVER_DONT_CARE 2 - FastAccelStepper* stepperConnectToPin(uint8_t step_pin, - uint8_t driver_type = DRIVER_DONT_CARE); +#define DRIVER_MCPWM_PCNT FasDriver::MCPWM_PCNT +#define DRIVER_RMT FasDriver::RMT +#define DRIVER_DONT_CARE FasDriver::DONT_CARE + FastAccelStepper* stepperConnectToPin( + uint8_t step_pin, FasDriver driver_type = DRIVER_DONT_CARE); +#endif + +#if defined(SUPPORT_TASK_RATE_CHANGE) + // For e.g. esp32 the repetition rate of the stepper task can be changed. + // The default delay is 4ms. + // + // The steppertask is looping with: + // manageSteppers() + // wdt_reset() + // delay() + // + // The actual repetition rate of the stepper task is delay + execution time of + // manageSteppers() + // + // This function is primary of interest in conjunction with + // setForwardPlanningTimeInMs(). If the delay is larger then forward planning + // time, then the stepper queue will always run out of commands, which lead to + // a sudden stop of the motor. If the delay is 0, then the stepper task will + // constantly looping, which may lead to the task blocking other tasks. + // Consequently, this function is intended for advanced users. + // + // There is not planned to test this functionality, because automatic testing + // is only available for avr devices and those continue to use fixed 4ms rate. + // + // Please be aware, that the configured tick rate aka portTICK_PERIOD_MS is + // relevant. Apparently, arduino-esp32 has FreeRTOS configured to have a + // tick-rate of 1000Hz + inline void task_rate(uint8_t delay_ms) { _delay_ms = delay_ms; }; + uint8_t _delay_ms; #endif // Comments to valid pins: @@ -136,7 +165,7 @@ class FastAccelStepperEngine { // If blinking of a LED is required to indicate, the stepper controller is // still running, then the port. to which the LED is connected, can be told to // the engine. The periodic task will let the associated LED blink with 1 Hz - static void setDebugLed(uint8_t ledPin); + void setDebugLed(uint8_t ledPin); /* This should be only called from ISR or stepper task. So do not call it */ void manageSteppers(); @@ -147,10 +176,19 @@ class FastAccelStepperEngine { uint8_t _stepper_cnt; FastAccelStepper* _stepper[MAX_STEPPER]; - static bool _isValidStepPin(uint8_t step_pin); + bool _isValidStepPin(uint8_t step_pin); bool (*_externalCallForPin)(uint8_t pin, uint8_t value); +#if defined(SUPPORT_RP_PICO) + uint8_t claimed_pios; + PIO pio[NUM_PIOS]; + + public: + void pushCommands(); +#endif + friend class FastAccelStepper; + friend class StepperQueue; }; // ### Return codes of calls to `move()` and `moveTo()` @@ -233,12 +271,12 @@ 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 // step pin is defined at creation. Here can retrieve the pin - uint8_t getStepPin() const; + uint8_t getStepPin(); // ## Direction Pin // if direction pin is connected, call this function. @@ -292,11 +330,8 @@ class FastAccelStepper { // interrupt/task with 4 or 10 ms repetition rate and as such is with several // ms jitter. void setAutoEnable(bool auto_enable); - int8_t setDelayToEnable(uint32_t delay_us); + DelayResultCode setDelayToEnable(uint32_t delay_us); void setDelayToDisable(uint16_t delay_ms); -#define DELAY_OK 0 -#define DELAY_TOO_LOW -1 -#define DELAY_TOO_HIGH -2 // ## Stepper Position // Retrieve the current position of the stepper @@ -305,7 +340,7 @@ class FastAccelStepper { // The actual position may be off by the number of steps in the ongoing // command. If precise real time position is needed, attaching a pulse counter // may be of help. - int32_t getCurrentPosition() const; + int32_t getCurrentPosition(); // Set the current position of the stepper - either in standstill or while // moving. @@ -327,17 +362,17 @@ class FastAccelStepper { // - In us: This means in us/step // // For the device's maximum allowed speed, the following calls can be used. - uint16_t getMaxSpeedInUs() const; - uint16_t getMaxSpeedInTicks() const; - uint32_t getMaxSpeedInHz() const; - uint32_t getMaxSpeedInMilliHz() const; + uint16_t getMaxSpeedInUs(); + uint16_t getMaxSpeedInTicks(); + uint32_t getMaxSpeedInHz(); + uint32_t getMaxSpeedInMilliHz(); // For esp32 and avr, the device's maximum allowed speed can be overridden. // Allocating a new stepper will override any absolute speed limit. // This is absolutely untested, no error checking implemented. // Use at your own risk ! #if SUPPORT_UNSAFE_ABS_SPEED_LIMIT_SETTING == 1 - void setAbsoluteSpeedLimit(uint16_t max_speed_in_ticks) const; + void setAbsoluteSpeedLimit(uint16_t max_speed_in_ticks); #endif // Setting the speed can be done with the four `setSpeed...()` calls. @@ -469,8 +504,8 @@ class FastAccelStepper { // move/moveTo for an ongoing command would reverse the direction, then the // command is silently ignored. // return values are the MOVE_... constants - int8_t move(int32_t move, bool blocking = false); - int8_t moveTo(int32_t position, bool blocking = false); + MoveResultCode move(int32_t move, bool blocking = false); + MoveResultCode moveTo(int32_t position, bool blocking = false); // ### keepRunning() // This command flags the stepper to keep run continuously into current @@ -485,8 +520,8 @@ class FastAccelStepper { // These commands just let the motor run continuously in one direction. // If the motor is running in the opposite direction, it will reverse // return value as with move/moveTo - int8_t runForward(); - int8_t runBackward(); + MoveResultCode runForward(); + MoveResultCode runBackward(); // ### forwardStep() and backwardStep() // forwardStep()/backwardstep() can be called, while stepper is not moving @@ -508,7 +543,8 @@ class FastAccelStepper { // => accelerate towards negative maximum speed if allow_reverse // => decelerate towards motor stop if allow_reverse = false // return value as with move/moveTo - int8_t moveByAcceleration(int32_t acceleration, bool allow_reverse = true); + MoveResultCode moveByAcceleration(int32_t acceleration, + bool allow_reverse = true); // ### stopMove() // Stop the running stepper with normal deceleration. @@ -588,6 +624,49 @@ class FastAccelStepper { _forward_planning_in_ticks *= TICKS_PER_S / 1000; // ticks per ms } + // ## Intermediate Level Stepper Control for Advanced Users + // + // The main purpose is to bypass the ramp generator as mentioned in + // [#299](https://github.com/gin66/FastAccelStepper/issues/299). + // This shall allow to run consecutive small moves with fixed speed. + // The parameters are steps (which can be 0) and duration in ticks. + // steps=0 makes sense in order to keep the time running and not + // getting out of sync. + // Due to integer arithmetics the actual duration may be off by a small value. + // That's why the actual_duration in TICKS is returned. + // The application should consider this for the next runTimed move. + // + // The optional parameter is a boolean called start. This allows for the first + // invocation to not start the queue yet. This is for managing steppers in + // parallel. It allows to fill all steppers' queues and then kick it off by a + // call to `moveTimed(0,0,NULL,true)`. Successive invocations can keep true. + // + // In order to not have another lightweight ramp generator running in + // background interrupt, the expecation to the application is, that this + // function is frequently enough called without the queue being emptied. + // + // The current implementation immediately starts with a step, if there should + // be one. Perhaps performing the step in the middle of the duration is more + // appropriate ? + // + // Meaning of the return values - which are in addtion to AQE from below + // - OK: Move has been successfully appended to the queue + // - BUSY: Queue does not have sufficient entries to append this timed + // move. + // - EMPTY: The queue has run out of commands, but the move has been + // appended. + // - TOO_LARGE: The move request does not fit into the queue. + // Reasons: The queue depth is (32/16) for SAM+ESP32/AVR. + // Each queue entry can emit 255 steps => (8160/4080) + // steps If the time between steps is >65535 ticks, then + // pauses have to be generated. In this case only (16/8) + // steps can be generated...but the queue shall not be + // empty + // => so even less steps can be done. + // Recommendation: keep the duration in the range of ms. + MoveTimedResultCode moveTimed(int16_t steps, uint32_t duration, + uint32_t* actual_duration, bool start = true); + // ## Low Level Stepper Queue Management (low level access) // // If the queue is already running, then the start parameter is obsolote. @@ -604,23 +683,15 @@ class FastAccelStepper { // should be called with interrupts disabled and return very fast. // Actually this is necessary, too, in case the queue is full and not // started. - int8_t addQueueEntry(const struct stepper_command_s* cmd, bool start = true); - // Return codes for addQueueEntry // positive values mean, that caller should retry later -#define AQE_OK 0 -#define AQE_QUEUE_FULL 1 -#define AQE_DIR_PIN_IS_BUSY 2 -#define AQE_WAIT_FOR_ENABLE_PIN_ACTIVE 3 -#define AQE_DEVICE_NOT_READY 4 -#define AQE_ERROR_TICKS_TOO_LOW -1 -#define AQE_ERROR_EMPTY_QUEUE_TO_START -2 -#define AQE_ERROR_NO_DIR_PIN_TO_TOGGLE -3 + AqeResultCode addQueueEntry(const struct stepper_command_s* cmd, + bool start = true); // ### check functions for command queue being empty, full or running. - bool isQueueEmpty() const; - bool isQueueFull() const; - bool isQueueRunning() const; + bool isQueueEmpty(); + bool isQueueFull(); + bool isQueueRunning(); // ### functions to get the fill level of the queue // @@ -633,15 +704,15 @@ class FastAccelStepper { // This function can be used to check, if the commands in the queue // will last for ticks. This is again without the // currently processed command. - bool hasTicksInQueue(uint32_t min_ticks) const; + bool hasTicksInQueue(uint32_t min_ticks); // This function allows to check the number of commands in the queue. // This is including the currently processed command. - uint8_t queueEntries() const; + uint8_t queueEntries(); // Get the future position of the stepper after all commands in queue are // completed - int32_t getPositionAfterCommandsCompleted() const; + int32_t getPositionAfterCommandsCompleted(); // Get the future speed of the stepper after all commands in queue are // completed. This is in µs. Returns 0 for stopped motor @@ -666,8 +737,8 @@ class FastAccelStepper { // These functions allow to detach and reAttach a step pin for other use. // Pretty low level, use with care or not at all - void detachFromPin() const; - void reAttachToPin() const; + void detachFromPin(); + void reAttachToPin(); // ## ESP32 only: Free pulse counter // These four functions are only available on esp32. @@ -703,6 +774,10 @@ class FastAccelStepper { // stepper (at exact this moment) can be retrieved just by reading the pulse // counter. If the value is negative, then just add 3200. // + // In case external direction pin is used and the dir pin is available on one + // of the GPIOs, then the additional dir_pin_readback parameter informs + // about this pin. + // // Update for idf5 version: // The pcnt_unit value is not used, because the available units are managed // by the system. The parameter is kept for compatibility. @@ -710,14 +785,16 @@ class FastAccelStepper { #if defined(SUPPORT_ESP32_PULSE_COUNTER) && (ESP_IDF_VERSION_MAJOR == 5) bool attachToPulseCounter(uint8_t unused_pcnt_unit = 0, int16_t low_value = -16384, - int16_t high_value = 16384); + int16_t high_value = 16384, + uint8_t dir_pin_readback = PIN_UNDEFINED); int16_t readPulseCounter(); void clearPulseCounter(); inline bool pulseCounterAttached() { return _attached_pulse_unit != NULL; } #endif #if defined(SUPPORT_ESP32_PULSE_COUNTER) && (ESP_IDF_VERSION_MAJOR == 4) bool attachToPulseCounter(uint8_t pcnt_unit, int16_t low_value = -16384, - int16_t high_value = 16384); + int16_t high_value = 16384, + uint8_t dir_pin_readback = PIN_UNDEFINED); int16_t readPulseCounter(); void clearPulseCounter(); inline bool pulseCounterAttached() { return _attached_pulse_cnt_unit >= 0; } @@ -733,7 +810,7 @@ class FastAccelStepper { void blockingWaitForForceStopComplete(); bool needAutoDisable(); bool agreeWithAutoDisable(); - bool usesAutoEnablePin(uint8_t pin) const; + bool usesAutoEnablePin(uint8_t pin); void getCurrentSpeedInTicks(struct actual_ticks_s* speed, bool realtime); FastAccelStepperEngine* _engine; diff --git a/src/FastAccelStepper_idf4_esp32_pcnt.cpp b/src/FastAccelStepper_idf4_esp32_pcnt.cpp index f66f3a7..822b330 100644 --- a/src/FastAccelStepper_idf4_esp32_pcnt.cpp +++ b/src/FastAccelStepper_idf4_esp32_pcnt.cpp @@ -20,16 +20,19 @@ uint32_t ctrl_idx[SUPPORT_ESP32_PULSE_COUNTER] = { bool FastAccelStepper::attachToPulseCounter(uint8_t pcnt_unit, int16_t low_value, - int16_t high_value) { + int16_t high_value, + uint8_t dir_pin) { if (pcnt_unit >= SUPPORT_ESP32_PULSE_COUNTER) { return false; } pcnt_config_t cfg; - uint8_t dir_pin = getDirectionPin(); uint8_t step_pin = getStepPin(); - cfg.pulse_gpio_num = PCNT_PIN_NOT_USED; if (dir_pin == PIN_UNDEFINED) { + dir_pin = getDirectionPin(); + } + cfg.pulse_gpio_num = PCNT_PIN_NOT_USED; + if (dir_pin == PIN_UNDEFINED || (dir_pin & PIN_EXTERNAL_FLAG) != 0) { cfg.ctrl_gpio_num = PCNT_PIN_NOT_USED; cfg.hctrl_mode = PCNT_MODE_KEEP; cfg.lctrl_mode = PCNT_MODE_KEEP; @@ -64,7 +67,7 @@ bool FastAccelStepper::attachToPulseCounter(uint8_t pcnt_unit, gpio_matrix_in(step_pin, sig_idx[pcnt_unit], 0); gpio_iomux_in(step_pin, sig_idx[pcnt_unit]); // test failure without this call - if (dir_pin != PIN_UNDEFINED) { + if (dir_pin != PIN_UNDEFINED && (dir_pin & PIN_EXTERNAL_FLAG) == 0) { pinMode(dir_pin, OUTPUT); gpio_matrix_out(dir_pin, 0x100, false, false); gpio_matrix_in(dir_pin, ctrl_idx[pcnt_unit], 0); diff --git a/src/FastAccelStepper_idf5_esp32_pcnt.cpp b/src/FastAccelStepper_idf5_esp32_pcnt.cpp index 1912c3c..dc53202 100644 --- a/src/FastAccelStepper_idf5_esp32_pcnt.cpp +++ b/src/FastAccelStepper_idf5_esp32_pcnt.cpp @@ -20,7 +20,8 @@ struct pcnt_chan_t { bool FastAccelStepper::attachToPulseCounter(uint8_t unused_pcnt_unit, int16_t low_value, - int16_t high_value) { + int16_t high_value, + uint8_t dir_pin) { pcnt_unit_config_t config = {.low_limit = low_value, .high_limit = high_value, .intr_priority = 0, @@ -44,8 +45,12 @@ bool FastAccelStepper::attachToPulseCounter(uint8_t unused_pcnt_unit, pcnt_channel_level_action_t level_high = PCNT_CHANNEL_LEVEL_ACTION_KEEP; pcnt_channel_level_action_t level_low = PCNT_CHANNEL_LEVEL_ACTION_KEEP; - uint8_t dir_pin = getDirectionPin(); - if (dir_pin != PIN_UNDEFINED) { + + if (dir_pin == PIN_UNDEFINED) { + dir_pin = getDirectionPin(); + } + + if (dir_pin != PIN_UNDEFINED && (dir_pin & PIN_EXTERNAL_FLAG) == 0) { chan_config.level_gpio_num = dir_pin; if (directionPinHighCountsUp()) { level_low = PCNT_CHANNEL_LEVEL_ACTION_INVERSE; @@ -105,7 +110,7 @@ bool FastAccelStepper::attachToPulseCounter(uint8_t unused_pcnt_unit, .pulse_sig; gpio_matrix_in(step_pin, signal, 0); gpio_iomux_in(step_pin, signal); - if (dir_pin != PIN_UNDEFINED) { + if (dir_pin != PIN_UNDEFINED && (dir_pin & PIN_EXTERNAL_FLAG) == 0) { pinMode(dir_pin, OUTPUT); int control = pcnt_periph_signals.groups[0] .units[unit_id] diff --git a/src/PoorManFloat.cpp b/src/Log2Representation.cpp similarity index 94% rename from src/PoorManFloat.cpp rename to src/Log2Representation.cpp index 7822c2a..31a49f0 100644 --- a/src/PoorManFloat.cpp +++ b/src/Log2Representation.cpp @@ -5,7 +5,7 @@ #define PROGMEM #define pgm_read_byte_near(x) (*(x)) #endif -#include "PoorManFloat.h" +#include "Log2Representation.h" #ifdef TEST #include #endif @@ -165,7 +165,7 @@ uint8_t leading_zeros(uint8_t x) { return res; } -pmf_logarithmic pmfl_from(uint8_t x) { +pmf_logarithmic log2_from(uint8_t x) { // calling with x == 0 is considered an error. // // In a first step convert to @@ -184,7 +184,7 @@ pmf_logarithmic pmfl_from(uint8_t x) { // 3. add the value from the log2_minus_x_plus_one_shifted_by_1 table uint8_t leading = leading_zeros(x); if (leading == 8) { - return PMF_CONST_INVALID; + return LOG2_CONST_INVALID; } x <<= leading + 1; uint8_t e = 7 - leading; @@ -196,10 +196,10 @@ pmf_logarithmic pmfl_from(uint8_t x) { return res; } -pmf_logarithmic pmfl_from(uint16_t x) { +pmf_logarithmic log2_from(uint16_t x) { uint8_t leading = leading_zeros(x >> 8); if (leading == 8) { - return pmfl_from((uint8_t)x); + return log2_from((uint8_t)x); } // shift msb out x <<= leading + 1; @@ -222,7 +222,7 @@ pmf_logarithmic pmfl_from(uint16_t x) { x += ((uint16_t)exponent) << 9; return x; } -pmf_logarithmic pmfl_from(uint32_t x) { +pmf_logarithmic log2_from(uint32_t x) { int16_t exp_offset; uint16_t w; if ((x & 0xff000000) == 0) { @@ -243,13 +243,13 @@ pmf_logarithmic pmfl_from(uint32_t x) { w = x >> 16; exp_offset = 0x2000; } - return pmfl_from(w) + exp_offset; + return log2_from(w) + exp_offset; } -uint16_t pmfl_to_u16(pmf_logarithmic x) { +uint16_t log2_to_u16(pmf_logarithmic x) { if (x < 0) { return 0; } - if (x >= PMF_CONST_UINT16_MAX) { + if (x >= LOG2_CONST_UINT16_MAX) { return __UINT16_MAX__; } uint8_t exponent = ((uint16_t)x) >> 9; @@ -274,29 +274,29 @@ uint16_t pmfl_to_u16(pmf_logarithmic x) { } return x; } -uint32_t pmfl_to_u32(pmf_logarithmic x) { +uint32_t log2_to_u32(pmf_logarithmic x) { if (x < 0) { return 0; } - if (x >= PMF_CONST_UINT32_MAX) { + if (x >= LOG2_CONST_UINT32_MAX) { return __UINT32_MAX__; } uint8_t exponent = ((uint16_t)x) >> 9; if (exponent < 0x10) { - return pmfl_to_u16(x); + return log2_to_u16(x); } uint8_t shift = exponent - 0x0f; - x = pmfl_shr(x, shift); - uint32_t res = pmfl_to_u16(x); + x = log2_shr(x, shift); + uint32_t res = log2_to_u16(x); res <<= shift; return res; } -pmf_logarithmic pmfl_square(pmf_logarithmic x) { +pmf_logarithmic log2_square(pmf_logarithmic x) { if (x > 0x4000) { - return PMF_CONST_MAX; + return LOG2_CONST_MAX; } if (x <= -0x4000) { - return PMF_CONST_MIN; + return LOG2_CONST_MIN; } return x + x; } diff --git a/src/Log2Representation.h b/src/Log2Representation.h new file mode 100644 index 0000000..9c92c7d --- /dev/null +++ b/src/Log2Representation.h @@ -0,0 +1,45 @@ +#ifndef LOG2REPRESENTATION_H +#define LOG2REPRESENTATION_H +#include + +#include + +typedef int16_t pmf_logarithmic; + +#define LOG2_CONST_INVALID ((pmf_logarithmic)0x8000) +#define LOG2_CONST_MAX ((pmf_logarithmic)0x7fff) +#define LOG2_CONST_MIN ((pmf_logarithmic)0x8001) + +pmf_logarithmic log2_from(uint8_t x); +pmf_logarithmic log2_from(uint16_t x); +pmf_logarithmic log2_from(uint32_t x); + +uint16_t log2_to_u16(pmf_logarithmic x); +uint32_t log2_to_u32(pmf_logarithmic x); + +#define log2_shl(x, n) ((x) + (((int16_t)(n)) << 9)) +#define log2_shr(x, n) ((x) - (((int16_t)(n)) << 9)) + +#define log2_multiply(x, y) ((x) + (y)) +#define log2_divide(x, y) ((x) - (y)) +#define log2_reciprocal(x) (-(x)) +#define log2_sqrt(x) ((x) / 2) +#define log2_rsqrt(x) (-(x) / 2) +#define log2_rsquare(x) log2_reciprocal(log2_square(x)) + +inline pmf_logarithmic log2_pow_div_3(pmf_logarithmic x) { + // 1/3 ~ (1/4+1/16+1/64+1/256+1/1024+1/4096+1/16384) + x /= 2; // x/2 + x += x / 4; // x/2 + x/8 + x += x / 16; // x/2 + x/8 + x/32 + x/128 + x += x / 256; // x/2 + x/8 + x/32 + x/128 + x/512 + x/2048 + x/8192 + x/32768 + x += 1; + return x / 2; +} +#define log2_pow_2_div_3(x) ((x) - log2_pow_div_3(x)) +#define log2_pow_3_div_2(x) ((x) + (x) / 2) + +pmf_logarithmic log2_square(pmf_logarithmic x); + +uint8_t leading_zeros(uint8_t x); +#endif diff --git a/src/Log2RepresentationConst.h b/src/Log2RepresentationConst.h new file mode 100644 index 0000000..9ed6432 --- /dev/null +++ b/src/Log2RepresentationConst.h @@ -0,0 +1,73 @@ +// Autogenerated by extras/gen_pmf_const/main +// DO NOT EDIT +#ifndef LOG2REPRESENTATIONCONST_H +#define LOG2REPRESENTATIONCONST_H + +#include + +// LOG2_CONST_1 = 0.000000 0 = 0x0000 +#define LOG2_CONST_1 ((pmf_logarithmic)0x0000) +// => converted back => 1.00 + +// LOG2_CONST_3_DIV_2 = 299.500793 300 = 0x012c +#define LOG2_CONST_3_DIV_2 ((pmf_logarithmic)0x012c) +// => converted back => 1.50 + +// LOG2_CONST_128E12 = 23993.925781 23994 = 0x5dba +#define LOG2_CONST_128E12 ((pmf_logarithmic)0x5dba) +// => converted back => 128012783714304.00 + +// LOG2_CONST_16E6 = 12252.962891 12253 = 0x2fdd +#define LOG2_CONST_16E6 ((pmf_logarithmic)0x2fdd) +// => converted back => 16000799.00 + +// LOG2_CONST_500 = 4590.481445 4590 = 0x11ee +#define LOG2_CONST_500 ((pmf_logarithmic)0x11ee) +// => converted back => 499.67 + +// LOG2_CONST_1000 = 5102.481445 5102 = 0x13ee +#define LOG2_CONST_1000 ((pmf_logarithmic)0x13ee) +// => converted back => 999.35 + +// LOG2_CONST_2000 = 5614.481445 5614 = 0x15ee +#define LOG2_CONST_2000 ((pmf_logarithmic)0x15ee) +// => converted back => 1998.70 + +// LOG2_CONST_32000 = 7662.481445 7662 = 0x1dee +#define LOG2_CONST_32000 ((pmf_logarithmic)0x1dee) +// => converted back => 31979.14 + +// LOG2_CONST_16E6_DIV_SQRT_OF_2 = 11996.962891 11997 = 0x2edd +#define LOG2_CONST_16E6_DIV_SQRT_OF_2 ((pmf_logarithmic)0x2edd) +// => converted back => 11314274.00 + +// LOG2_CONST_21E6 = 12453.830078 12454 = 0x30a6 +#define LOG2_CONST_21E6 ((pmf_logarithmic)0x30a6) +// => converted back => 21004844.00 + +// LOG2_CONST_42000 = 7863.348145 7863 = 0x1eb7 +#define LOG2_CONST_42000 ((pmf_logarithmic)0x1eb7) +// => converted back => 41980.21 + +// LOG2_CONST_21E6_DIV_SQRT_OF_2 = 12197.830078 12198 = 0x2fa6 +#define LOG2_CONST_21E6_DIV_SQRT_OF_2 ((pmf_logarithmic)0x2fa6) +// => converted back => 14852668.00 + +// LOG2_CONST_2205E11 = 24395.660156 24396 = 0x5f4c +#define LOG2_CONST_2205E11 ((pmf_logarithmic)0x5f4c) +// => converted back => 220601734135808.00 + +// LOG2_CONST_UINT32_MAX = 16384.000000 16384 = 0x4000 +#define LOG2_CONST_UINT32_MAX ((pmf_logarithmic)0x4000) +// => converted back => 4294967296.00 + +// LOG2_CONST_UINT16_MAX = 8191.988770 8192 = 0x2000 +#define LOG2_CONST_UINT16_MAX ((pmf_logarithmic)0x2000) +// => converted back => 65536.00 + +// used in Log2Representation.cpp as example +// +// LOG2_CONST_15373 = 7120.953125 7121 = 0x1bd1 +#define LOG2_CONST_15373 ((pmf_logarithmic)0x1bd1) +// => converted back => 15373.98 +#endif diff --git a/src/PoorManFloat.h b/src/PoorManFloat.h deleted file mode 100644 index 66f33d3..0000000 --- a/src/PoorManFloat.h +++ /dev/null @@ -1,45 +0,0 @@ -#ifndef POORMANFLOAT_H -#define POORMANFLOAT_H -#include - -#include - -typedef int16_t pmf_logarithmic; - -#define PMF_CONST_INVALID ((pmf_logarithmic)0x8000) -#define PMF_CONST_MAX ((pmf_logarithmic)0x7fff) -#define PMF_CONST_MIN ((pmf_logarithmic)0x8001) - -pmf_logarithmic pmfl_from(uint8_t x); -pmf_logarithmic pmfl_from(uint16_t x); -pmf_logarithmic pmfl_from(uint32_t x); - -uint16_t pmfl_to_u16(pmf_logarithmic x); -uint32_t pmfl_to_u32(pmf_logarithmic x); - -#define pmfl_shl(x, n) ((x) + (((int16_t)(n)) << 9)) -#define pmfl_shr(x, n) ((x) - (((int16_t)(n)) << 9)) - -#define pmfl_multiply(x, y) ((x) + (y)) -#define pmfl_divide(x, y) ((x) - (y)) -#define pmfl_reciprocal(x) (-(x)) -#define pmfl_sqrt(x) ((x) / 2) -#define pmfl_rsqrt(x) (-(x) / 2) -#define pmfl_rsquare(x) pmfl_reciprocal(pmfl_square(x)) - -inline pmf_logarithmic pmfl_pow_div_3(pmf_logarithmic x) { - // 1/3 ~ (1/4+1/16+1/64+1/256+1/1024+1/4096+1/16384) - x /= 2; // x/2 - x += x / 4; // x/2 + x/8 - x += x / 16; // x/2 + x/8 + x/32 + x/128 - x += x / 256; // x/2 + x/8 + x/32 + x/128 + x/512 + x/2048 + x/8192 + x/32768 - x += 1; - return x / 2; -} -#define pmfl_pow_2_div_3(x) ((x) - pmfl_pow_div_3(x)) -#define pmfl_pow_3_div_2(x) ((x) + (x) / 2) - -pmf_logarithmic pmfl_square(pmf_logarithmic x); - -uint8_t leading_zeros(uint8_t x); -#endif diff --git a/src/PoorManFloatConst.h b/src/PoorManFloatConst.h deleted file mode 100644 index 637851e..0000000 --- a/src/PoorManFloatConst.h +++ /dev/null @@ -1,73 +0,0 @@ -// Autogenerated by extras/gen_pmf_const/main -// DO NOT EDIT -#ifndef POORMANFLOATCONST_H -#define POORMANFLOATCONST_H - -#include - -// PMF_CONST_1 = 0.000000 0 = 0x0000 -#define PMF_CONST_1 ((pmf_logarithmic)0x0000) -// => converted back => 1.00 - -// PMF_CONST_3_DIV_2 = 299.500793 300 = 0x012c -#define PMF_CONST_3_DIV_2 ((pmf_logarithmic)0x012c) -// => converted back => 1.50 - -// PMF_CONST_128E12 = 23993.925781 23994 = 0x5dba -#define PMF_CONST_128E12 ((pmf_logarithmic)0x5dba) -// => converted back => 128012783714304.00 - -// PMF_CONST_16E6 = 12252.962891 12253 = 0x2fdd -#define PMF_CONST_16E6 ((pmf_logarithmic)0x2fdd) -// => converted back => 16000799.00 - -// PMF_CONST_500 = 4590.481445 4590 = 0x11ee -#define PMF_CONST_500 ((pmf_logarithmic)0x11ee) -// => converted back => 499.67 - -// PMF_CONST_1000 = 5102.481445 5102 = 0x13ee -#define PMF_CONST_1000 ((pmf_logarithmic)0x13ee) -// => converted back => 999.35 - -// PMF_CONST_2000 = 5614.481445 5614 = 0x15ee -#define PMF_CONST_2000 ((pmf_logarithmic)0x15ee) -// => converted back => 1998.70 - -// PMF_CONST_32000 = 7662.481445 7662 = 0x1dee -#define PMF_CONST_32000 ((pmf_logarithmic)0x1dee) -// => converted back => 31979.14 - -// PMF_CONST_16E6_DIV_SQRT_OF_2 = 11996.962891 11997 = 0x2edd -#define PMF_CONST_16E6_DIV_SQRT_OF_2 ((pmf_logarithmic)0x2edd) -// => converted back => 11314274.00 - -// PMF_CONST_21E6 = 12453.830078 12454 = 0x30a6 -#define PMF_CONST_21E6 ((pmf_logarithmic)0x30a6) -// => converted back => 21004844.00 - -// PMF_CONST_42000 = 7863.348145 7863 = 0x1eb7 -#define PMF_CONST_42000 ((pmf_logarithmic)0x1eb7) -// => converted back => 41980.21 - -// PMF_CONST_21E6_DIV_SQRT_OF_2 = 12197.830078 12198 = 0x2fa6 -#define PMF_CONST_21E6_DIV_SQRT_OF_2 ((pmf_logarithmic)0x2fa6) -// => converted back => 14852668.00 - -// PMF_CONST_2205E11 = 24395.660156 24396 = 0x5f4c -#define PMF_CONST_2205E11 ((pmf_logarithmic)0x5f4c) -// => converted back => 220601734135808.00 - -// PMF_CONST_UINT32_MAX = 16384.000000 16384 = 0x4000 -#define PMF_CONST_UINT32_MAX ((pmf_logarithmic)0x4000) -// => converted back => 4294967296.00 - -// PMF_CONST_UINT16_MAX = 8191.988770 8192 = 0x2000 -#define PMF_CONST_UINT16_MAX ((pmf_logarithmic)0x2000) -// => converted back => 65536.00 - -// used in PoorManFloat.cpp as example -// -// PMF_CONST_15373 = 7120.953125 7121 = 0x1bd1 -#define PMF_CONST_15373 ((pmf_logarithmic)0x1bd1) -// => converted back => 15373.98 -#endif diff --git a/src/RampCalculator.cpp b/src/RampCalculator.cpp index d2c9b22..170cdaf 100644 --- a/src/RampCalculator.cpp +++ b/src/RampCalculator.cpp @@ -49,11 +49,11 @@ uint32_t calculate_ticks_v3(uint32_t steps, float pre_calc) { // // Using pmf_logarithmic improves to 22-4 = 18us, but less precision uint32_t calculate_ticks_v4(uint32_t steps, uint32_t acceleration) { - pmf_logarithmic pmfl_a = pmfl_from(acceleration); - pmf_logarithmic pmfl_s = pmfl_from(steps); - pmf_logarithmic pmfl_res = - pmfl_divide(PMF_TICKS_PER_S, pmfl_sqrt(pmfl_multiply(pmfl_s, pmfl_a))); - uint32_t res = pmfl_to_u32(pmfl_res); + pmf_logarithmic log2_a = log2_from(acceleration); + pmf_logarithmic log2_s = log2_from(steps); + pmf_logarithmic log2_res = + log2_divide(LOG2_TICKS_PER_S, log2_sqrt(log2_multiply(log2_s, log2_a))); + uint32_t res = log2_to_u32(log2_res); return res; } // @@ -62,17 +62,17 @@ uint32_t calculate_ticks_v4(uint32_t steps, uint32_t acceleration) { // So this routine does not fit to that measurement anymore // uint32_t calculate_ticks_v5(uint32_t steps, pmf_logarithmic pre_calc) { - pmf_logarithmic pmfl_s = pmfl_from(steps); - pmf_logarithmic pmfl_res = pmfl_divide(pre_calc, pmfl_sqrt(pmfl_s)); - uint32_t res = pmfl_to_u32(pmfl_res); + pmf_logarithmic log2_s = log2_from(steps); + pmf_logarithmic log2_res = log2_divide(pre_calc, log2_sqrt(log2_s)); + uint32_t res = log2_to_u32(log2_res); return res; } // // using the combined function yields actually no measureable improvement uint32_t calculate_ticks_v6(uint32_t steps, pmf_logarithmic pre_calc) { - // pmf_logarithmic pmfl_s = pmfl_from(steps); - // pmf_logarithmic pmfl_res = pmfl_sqrt_after_divide(pre_calc, pmfl_s); - // uint32_t res = pmfl_to_u32(pmfl_res); + // pmf_logarithmic log2_s = log2_from(steps); + // pmf_logarithmic log2_res = log2_sqrt_after_divide(pre_calc, log2_s); + // uint32_t res = log2_to_u32(log2_res); // return res; return calculate_ticks_v5(steps, pre_calc); } @@ -172,10 +172,10 @@ uint32_t calculate_ticks_v7(uint32_t steps, pmf_logarithmic pre_calc) { // make -C off_test_timing // From the results subtract port B from port A measurement uint32_t calculate_ticks_v8(uint32_t steps, pmf_logarithmic pre_calc) { - pmf_logarithmic pmfl_steps = pmfl_from(steps); - pmf_logarithmic pmfl_sqrt_steps = pmfl_sqrt(pmfl_steps); - pmf_logarithmic pmfl_res = pmfl_divide(pre_calc, pmfl_sqrt_steps); - uint32_t res = pmfl_to_u32(pmfl_res); + pmf_logarithmic log2_steps = log2_from(steps); + pmf_logarithmic log2_sqrt_steps = log2_sqrt(log2_steps); + pmf_logarithmic log2_res = log2_divide(pre_calc, log2_sqrt_steps); + uint32_t res = log2_to_u32(log2_res); return res; } #endif diff --git a/src/RampCalculator.h b/src/RampCalculator.h index b173456..0bbd2f5 100644 --- a/src/RampCalculator.h +++ b/src/RampCalculator.h @@ -3,26 +3,26 @@ #include -#include "PoorManFloat.h" +#include "Log2Representation.h" #include "fas_arch/common.h" #if (TICKS_PER_S == 16000000L) -#define PMF_TICKS_PER_S PMF_CONST_16E6 -#define PMF_TICKS_PER_S_DIV_SQRT_OF_2 PMF_CONST_16E6_DIV_SQRT_OF_2 -#define PMF_ACCEL_FACTOR PMF_CONST_128E12 +#define LOG2_TICKS_PER_S LOG2_CONST_16E6 +#define LOG2_TICKS_PER_S_DIV_SQRT_OF_2 LOG2_CONST_16E6_DIV_SQRT_OF_2 +#define LOG2_ACCEL_FACTOR LOG2_CONST_128E12 #define US_TO_TICKS(u32) ((u32) * 16) #define TICKS_TO_US(u32) ((u32) / 16) #elif (TICKS_PER_S == 21000000L) -#define PMF_TICKS_PER_S PMF_CONST_21E6 -#define PMF_TICKS_PER_S_DIV_SQRT_OF_2 PMF_CONST_21E6_DIV_SQRT_OF_2 -#define PMF_ACCEL_FACTOR PMF_CONST_2205E11 +#define LOG2_TICKS_PER_S LOG2_CONST_21E6 +#define LOG2_TICKS_PER_S_DIV_SQRT_OF_2 LOG2_CONST_21E6_DIV_SQRT_OF_2 +#define LOG2_ACCEL_FACTOR LOG2_CONST_2205E11 #define US_TO_TICKS(u32) ((u32) * 21) #define TICKS_TO_US(u32) ((u32) / 21) #else -#define SUPPORT_PMF_TIMER_FREQ_VARIABLES -#define PMF_TICKS_PER_S pmfl_timer_freq -#define PMF_TICKS_PER_S_DIV_SQRT_OF_2 pmfl_timer_freq_div_sqrt_of_2 -#define PMF_ACCEL_FACTOR pmfl_timer_freq_square_div_2 +#define SUPPORT_LOG2_TIMER_FREQ_VARIABLES +#define LOG2_TICKS_PER_S log2_timer_freq +#define LOG2_TICKS_PER_S_DIV_SQRT_OF_2 log2_timer_freq_div_sqrt_of_2 +#define LOG2_ACCEL_FACTOR log2_timer_freq_square_div_2 // This overflows for approx. 1s at 40 MHz, only #define US_TO_TICKS(u32) \ ((uint32_t)((((uint32_t)((u32) * (TICKS_PER_S / 10000L))) / 100L))) @@ -49,7 +49,7 @@ struct ramp_parameters_s { uint32_t min_travel_ticks; uint32_t s_h; uint32_t s_jump; - pmf_logarithmic pmfl_accel; + pmf_logarithmic log2_accel; bool apply : 1; // clear on read by stepper task. Triggers read ! bool any_change : 1; // clear on read by stepper task bool recalc_ramp_steps : 1; // clear on read by stepper task @@ -131,18 +131,18 @@ struct ramp_parameters_s { } } inline void setAcceleration(int32_t accel) { - pmf_logarithmic new_pmfl_accel = pmfl_from((uint32_t)accel); - if (!valid_acceleration || (pmfl_accel != new_pmfl_accel)) { + pmf_logarithmic new_log2_accel = log2_from((uint32_t)accel); + if (!valid_acceleration || (log2_accel != new_log2_accel)) { fasDisableInterrupts(); valid_acceleration = true; any_change = true; recalc_ramp_steps = true; - pmfl_accel = new_pmfl_accel; + log2_accel = new_log2_accel; fasEnableInterrupts(); } } inline void setJumpStart(uint32_t jump_step) { s_jump = jump_step; } - inline int8_t checkValidConfig() const { + inline MoveResultCode checkValidConfig() { if (!valid_speed) { return MOVE_ERR_SPEED_IS_UNDEFINED; } @@ -158,24 +158,24 @@ struct ramp_config_s { // These three variables are derived uint32_t max_ramp_up_steps; - pmf_logarithmic pmfl_ticks_h; + pmf_logarithmic log2_ticks_h; pmf_logarithmic cubic; void init() { parameters.init(); } inline void update() { if (parameters.s_h > 0) { - pmf_logarithmic pmfl_s_h = pmfl_from(parameters.s_h); + pmf_logarithmic log2_s_h = log2_from(parameters.s_h); // 1/cubic = sqrt(3/2 * a) / s_h^(1/6) / TICKS_PER_S // = sqrt(3/2 * a / s_h^(1/3)) / TICKS_PER_S // cubic = TICKS_PER_S / sqrt(s_h^(1/3) / (3/2 * a)) - cubic = pmfl_multiply(PMF_CONST_3_DIV_2, parameters.pmfl_accel); - cubic = pmfl_sqrt(pmfl_divide(pmfl_pow_div_3(pmfl_s_h), cubic)); - cubic = pmfl_multiply(PMF_TICKS_PER_S, cubic); + cubic = log2_multiply(LOG2_CONST_3_DIV_2, parameters.log2_accel); + cubic = log2_sqrt(log2_divide(log2_pow_div_3(log2_s_h), cubic)); + cubic = log2_multiply(LOG2_TICKS_PER_S, cubic); // calculate_ticks(s_h) - pmfl_ticks_h = pmfl_divide(cubic, pmfl_pow_2_div_3(pmfl_s_h)); + log2_ticks_h = log2_divide(cubic, log2_pow_2_div_3(log2_s_h)); } else { - pmfl_ticks_h = PMF_CONST_MAX; + log2_ticks_h = LOG2_CONST_MAX; } max_ramp_up_steps = calculate_ramp_steps(parameters.min_travel_ticks); if (max_ramp_up_steps == 0) { @@ -194,41 +194,41 @@ struct ramp_config_s { // ticks = TICKS_PER_S/sqrt(2) / sqrt(a*s) if (steps >= parameters.s_h) { steps -= (parameters.s_h + 2) >> 2; - pmf_logarithmic pmfl_steps = pmfl_from(steps); - pmf_logarithmic pmfl_steps_mul_accel = - pmfl_multiply(pmfl_steps, parameters.pmfl_accel); - pmf_logarithmic pmfl_sqrt_steps_mul_accel = - pmfl_sqrt(pmfl_steps_mul_accel); - pmf_logarithmic pmfl_res = - pmfl_divide(PMF_TICKS_PER_S_DIV_SQRT_OF_2, pmfl_sqrt_steps_mul_accel); - uint32_t res = pmfl_to_u32(pmfl_res); + pmf_logarithmic log2_steps = log2_from(steps); + pmf_logarithmic log2_steps_mul_accel = + log2_multiply(log2_steps, parameters.log2_accel); + pmf_logarithmic log2_sqrt_steps_mul_accel = + log2_sqrt(log2_steps_mul_accel); + pmf_logarithmic log2_res = + log2_divide(LOG2_TICKS_PER_S_DIV_SQRT_OF_2, log2_sqrt_steps_mul_accel); + uint32_t res = log2_to_u32(log2_res); return res; } // ticks = cubic / s^(2/3) - pmf_logarithmic pmfl_steps = pmfl_from(steps); - pmf_logarithmic pmfl_res = pmfl_divide(cubic, pmfl_pow_2_div_3(pmfl_steps)); - uint32_t res = pmfl_to_u32(pmfl_res); + pmf_logarithmic log2_steps = log2_from(steps); + pmf_logarithmic log2_res = log2_divide(cubic, log2_pow_2_div_3(log2_steps)); + uint32_t res = log2_to_u32(log2_res); return res; } uint32_t calculate_ramp_steps(uint32_t ticks) const { - // pmfl is in range -64..<64 due to shift by 1 - // pmfl_ticks is in range 0..<32 - // pmfl_accel is in range 0..<32 - // PMF_ACCEL_FACTOR is approx. 47 for 16 Mticks/s - // pmfl_ticks squared is in range 0..<64 - pmf_logarithmic pmfl_ticks = pmfl_from(ticks); - if (pmfl_ticks <= pmfl_ticks_h) { - pmf_logarithmic pmfl_inv_accel2 = - pmfl_divide(PMF_ACCEL_FACTOR, parameters.pmfl_accel); + // log2 is in range -64..<64 due to shift by 1 + // log2_ticks is in range 0..<32 + // log2_accel is in range 0..<32 + // LOG2_ACCEL_FACTOR is approx. 47 for 16 Mticks/s + // log2_ticks squared is in range 0..<64 + pmf_logarithmic log2_ticks = log2_from(ticks); + if (log2_ticks <= log2_ticks_h) { + pmf_logarithmic log2_inv_accel2 = + log2_divide(LOG2_ACCEL_FACTOR, parameters.log2_accel); uint32_t steps = - pmfl_to_u32(pmfl_divide(pmfl_inv_accel2, pmfl_square(pmfl_ticks))); + log2_to_u32(log2_divide(log2_inv_accel2, log2_square(log2_ticks))); steps += (parameters.s_h + 2) >> 2; return steps; } // s = (cubic/ticks)^(3/2) - pmf_logarithmic pmfl_res = pmfl_divide(cubic, pmfl_ticks); - pmfl_res = pmfl_pow_3_div_2(pmfl_res); - uint32_t steps = pmfl_to_u32(pmfl_res); + pmf_logarithmic log2_res = log2_divide(cubic, log2_ticks); + log2_res = log2_pow_3_div_2(log2_res); + uint32_t steps = log2_to_u32(log2_res); return steps; } }; diff --git a/src/RampConstAcceleration.cpp b/src/RampControl.cpp similarity index 96% rename from src/RampConstAcceleration.cpp rename to src/RampControl.cpp index 7bc7266..85e6cec 100644 --- a/src/RampConstAcceleration.cpp +++ b/src/RampControl.cpp @@ -1,23 +1,23 @@ -#include +#include #include "FastAccelStepper.h" #include "StepperISR.h" -#include "RampConstAcceleration.h" +#include "RampControl.h" #include "fas_arch/common.h" -#ifdef SUPPORT_PMF_TIMER_FREQ_VARIABLES -static pmf_logarithmic pmfl_timer_freq; -static pmf_logarithmic pmfl_timer_freq_div_sqrt_of_2; -static pmf_logarithmic pmfl_timer_freq_square_div_2; +#ifdef SUPPORT_LOG2_TIMER_FREQ_VARIABLES +static pmf_logarithmic log2_timer_freq; +static pmf_logarithmic log2_timer_freq_div_sqrt_of_2; +static pmf_logarithmic log2_timer_freq_square_div_2; #endif void init_ramp_module() { -#ifdef SUPPORT_PMF_TIMER_FREQ_VARIABLES - pmfl_timer_freq = pmfl_from((uint32_t)TICKS_PER_S); - pmfl_timer_freq_div_sqrt_of_2 = - pmfl_shr(pmfl_multiply(pmfl_timer_freq, pmfl_timer_freq)); - pmfl_timer_freq_square_div_2 = pmfl_shr(pmfl_square(pmfl_timer_freq)); +#ifdef SUPPORT_LOG2_TIMER_FREQ_VARIABLES + log2_timer_freq = log2_from((uint32_t)TICKS_PER_S); + log2_timer_freq_div_sqrt_of_2 = + log2_shr(log2_multiply(log2_timer_freq, log2_timer_freq), 1); + log2_timer_freq_square_div_2 = log2_shr(log2_square(log2_timer_freq), 1); #endif } @@ -265,7 +265,7 @@ void _getNextCommand(const struct ramp_ro_s *ramp, const struct ramp_rw_s *rw, uint32_t dec_steps = remaining_steps - performed_ramp_up_steps; if (dec_steps < 512) { // Only allow half, cause the steps accelerating need to decelerate, too - auto dec_steps_u16 = (uint16_t)dec_steps; + uint16_t dec_steps_u16 = (uint16_t)dec_steps; dec_steps_u16 /= 2; // Perhaps it would be better to coast instead // consideration has been done above already diff --git a/src/RampConstAcceleration.h b/src/RampControl.h similarity index 97% rename from src/RampConstAcceleration.h rename to src/RampControl.h index 4870d79..82dcff6 100644 --- a/src/RampConstAcceleration.h +++ b/src/RampControl.h @@ -1,5 +1,5 @@ -#ifndef RAMP_CONST_ACCELERATION_H -#define RAMP_CONST_ACCELERATION_H +#ifndef RAMP_CONTROL_H +#define RAMP_CONTROL_H #include "fas_arch/common.h" diff --git a/src/RampGenerator.cpp b/src/RampGenerator.cpp index fe51a2b..93dce41 100644 --- a/src/RampGenerator.cpp +++ b/src/RampGenerator.cpp @@ -1,4 +1,4 @@ -#include +#include #include "FastAccelStepper.h" #include "RampGenerator.h" @@ -29,8 +29,8 @@ void RampGenerator::applySpeedAcceleration() { _parameters.applyParameters(); } } -int8_t RampGenerator::startRun(bool countUp) { - uint8_t res = _parameters.checkValidConfig(); +MoveResultCode RampGenerator::startRun(bool countUp) { + MoveResultCode res = _parameters.checkValidConfig(); if (res != MOVE_OK) { return res; } @@ -72,9 +72,9 @@ void RampGenerator::_startMove(bool position_changed) { #endif } -int8_t RampGenerator::moveTo(int32_t position, - const struct queue_end_s *queue_end) { - uint8_t res = _parameters.checkValidConfig(); +MoveResultCode RampGenerator::moveTo(int32_t position, + const struct queue_end_s *queue_end) { + MoveResultCode res = _parameters.checkValidConfig(); if (res != MOVE_OK) { return res; } @@ -90,8 +90,9 @@ int8_t RampGenerator::moveTo(int32_t position, inject_fill_interrupt(2); return MOVE_OK; } -int8_t RampGenerator::move(int32_t move, const struct queue_end_s *queue_end) { - uint8_t res = _parameters.checkValidConfig(); +MoveResultCode RampGenerator::move(int32_t move, + const struct queue_end_s *queue_end) { + MoveResultCode res = _parameters.checkValidConfig(); if (res != MOVE_OK) { return res; } @@ -105,7 +106,7 @@ void RampGenerator::advanceTargetPosition(int32_t delta, _ro.target_pos += delta; } -void RampGenerator::afterCommandEnqueued(NextCommand *command) { +void RampGenerator::afterCommandEnqueued(const NextCommand *command) { #ifdef TEST printf( "after Command Enqueued: performed ramp up steps = %u, pause left = %u, " @@ -154,7 +155,7 @@ void RampGenerator::getNextCommand(const struct queue_end_s *queue_end, } if (_ro.config.parameters.valid_acceleration) { Serial.print(" a="); - Serial.print(pmfl_to_u32(_ro.config.parameters.pmfl_accel)); + Serial.print(log2_to_u32(_ro.config.parameters.log2_accel)); } if (_ro.config.parameters.recalc_ramp_steps) { Serial.print(" recalc"); diff --git a/src/RampGenerator.h b/src/RampGenerator.h index 5ebaf3a..2355aaa 100644 --- a/src/RampGenerator.h +++ b/src/RampGenerator.h @@ -3,15 +3,15 @@ #include "FastAccelStepper.h" #include "RampCalculator.h" -#include "RampConstAcceleration.h" +#include "RampControl.h" #include "fas_arch/common.h" class FastAccelStepper; -#ifdef SUPPORT_PMF_TIMER_FREQ_VARIABLES -extern pmf_logarithmic pmfl_timer_freq; -extern pmf_logarithmic pmfl_timer_freq_div_sqrt_of_2; -extern pmf_logarithmic pmfl_timer_freq_square_div_2; +#ifdef SUPPORT_LOG2_TIMER_FREQ_VARIABLES +extern pmf_logarithmic log2_timer_freq; +extern pmf_logarithmic log2_timer_freq_div_sqrt_of_2; +extern pmf_logarithmic log2_timer_freq_square_div_2; #endif class RampGenerator { @@ -73,9 +73,9 @@ class RampGenerator { return _parameters.checkValidConfig() == MOVE_OK; } void applySpeedAcceleration(); - int8_t move(int32_t move, const struct queue_end_s *queue); - int8_t moveTo(int32_t position, const struct queue_end_s *queue); - int8_t startRun(bool countUp); + MoveResultCode move(int32_t move, const struct queue_end_s *queue); + MoveResultCode moveTo(int32_t position, const struct queue_end_s *queue); + MoveResultCode startRun(bool countUp); inline void forceStop() { _ro.immediateStop(); } inline void initiateStop() { _ro.initiateStop(); } inline bool isStopping() { @@ -93,7 +93,7 @@ class RampGenerator { inline bool isRunningContinuously() { return _ro.isRunningContinuously(); } void getNextCommand(const struct queue_end_s *queue_end, NextCommand *cmd_out); - void afterCommandEnqueued(NextCommand *cmd_in); + void afterCommandEnqueued(const NextCommand *cmd_in); void getCurrentSpeedInTicks(struct actual_ticks_s *speed) { fasDisableInterrupts(); speed->ticks = _rw.curr_ticks; diff --git a/src/StepperISR.cpp b/src/StepperISR.cpp index c7baba1..202c07b 100644 --- a/src/StepperISR.cpp +++ b/src/StepperISR.cpp @@ -2,15 +2,17 @@ #include "StepperISR.h" -int8_t StepperQueue::addQueueEntry(const struct stepper_command_s* cmd, - bool start) { +AqeResultCode StepperQueue::addQueueEntry(const struct stepper_command_s* cmd, + bool start) { // Just to check if, if the struct has the correct size // if (sizeof(entry) != 6 * QUEUE_LEN) { // return -1; //} +#ifndef TEST if (!isReadyForCommands()) { return AQE_DEVICE_NOT_READY; } +#endif if (cmd == NULL) { if (start && !isRunning()) { if (next_write_idx == read_idx) { @@ -25,30 +27,6 @@ int8_t StepperQueue::addQueueEntry(const struct stepper_command_s* cmd, } uint16_t period = cmd->ticks; uint8_t steps = cmd->steps; - // generation discrepancy: pc vs target - // after Command Enqueued: performed ramp up steps = 1, pause left = 0, - // curr_ticks = 11320 after Command Enqueued: performed ramp up steps = 3, - // pause left = 0, curr_ticks = 6536 after Command Enqueued: performed ramp up - // steps = 7, pause left = 0, curr_ticks = 4276 after Command Enqueued: - // performed ramp up steps = 14, pause left = 0, curr_ticks = 3024 after - // Command Enqueued: performed ramp up steps = 24, pause left = 0, curr_ticks - // = 2312 after Command Enqueued: performed ramp up steps = 17, pause left = - // 0, curr_ticks = 3412 after Command Enqueued: performed ramp up steps = 8, - // pause left = 0, curr_ticks = 4004 after Command Enqueued: performed ramp up - // steps = 1, pause left = 0, curr_ticks = 11320 - // - // esp32 mcpwm and rmt: - // 0/268435455us/4294967295:1:11320X:2:6536X:4:4276X:7:3024X:10:2312X:13:3412X:9:4004X:7:11320X:1:11320X - // - // - // Failed 10steps: - // :0:40000X:0:40000X:1:56608X:0:56608X:1:40032X:0:40032X:1:65344X:1:56608X:1:50624X:1:56608X:1:65344X:1:40032X:0:40032X:1:56608X:0:56608X:1:56608X:0:56608X - // - // Failed seq_07 - // :0:40000X:0:40000X:1:56608X:0:56608X:1:40032X:0:40032X:1:65344X:1:56608X:1:50624X - // :1:46208X:1:42784X:1:40032X:1:37728X:1:35808X:1:34112X:1:32672X:1:31376X:1:30256X:1:29216X:1:28304X:1:27440X:1:26672X:1:25968X:1:25312X:1:24688X:1:24128X:1:23616X:1:23104X:1:22640X:1:22192X:1:21776X:1:21392X:1:21024X:1:20656X:1:20320X:1:20016X:1:19696X:1:19408X:1:19152X:1:18864X:1:18608X:1:18352X:1:18144X:1:17904X:1:17680X:1:17472X:1:17280X:1:17056X:1:16880X:1:16704X:1:16512X:1:16336X:1:16160X:1:16016X:1:15840X:2:15544X:2:15272X:2:14984X:2:14744X:2:14488X:2:14256X:2:14040X:2:13840X:2:13632X - // :2:13432X:2:13248X:2:13072X:2:12896X:2:12736X:2:12584X:2:12432X:2:12280X:2:12128X:2:12000X:2:11872X:2:11744X:2:11616X:2:11488X:2:11384X:2:11264X:2:11152X:2:11048X:2:10944X:2:10840X:2:10736X:2:10656X:3:10512X:3:10384X:3:10248X:3:10120X:3:10008X:3:9888X:3:9784X:3:9680X:3:9576X:3:9472X:3:9368X:3:9280X:3:9176X:3:9096X:3:9008X:3:8928X:3:8840X:3:8760X:3:8688X:3:8600X:3:8528X:3:8464X:3:8392X:3:8328X:3:8256X:3:8192X:3:8124X:3:8060X:3:8008X:3:7944X:4:7864X:4:7792X:4:7720X:4:7648X:4:7584X:4:7512X:4:7452X:4:7384X:4:7324X - // :4:7264X:4:7204X:4:7148X:4:7088X:4:7040X:4:6984X:4:6928X // #define TRACE #ifdef TRACE @@ -150,6 +128,8 @@ int8_t StepperQueue::addQueueEntry(const struct stepper_command_s* cmd, return AQE_OK; } +#if defined(SUPPORT_QUEUE_ENTRY_END_POS_U16) || \ + defined(SUPPORT_QUEUE_ENTRY_START_POS_U16) int32_t StepperQueue::getCurrentPosition() { fasDisableInterrupts(); uint32_t pos = (uint32_t)queue_end.pos; @@ -234,6 +214,7 @@ int32_t StepperQueue::getCurrentPosition() { } return pos; } +#endif uint32_t StepperQueue::ticksInQueue() { fasDisableInterrupts(); @@ -246,7 +227,7 @@ uint32_t StepperQueue::ticksInQueue() { uint32_t ticks = 0; rp++; // ignore currently processed entry while (wp != rp) { - struct queue_entry* e = &entry[rp++ & QUEUE_LEN_MASK]; + const struct queue_entry* e = &entry[rp++ & QUEUE_LEN_MASK]; ticks += e->ticks; uint8_t steps = e->steps; if (steps > 1) { @@ -293,7 +274,7 @@ bool StepperQueue::getActualTicksWithDirection(struct actual_ticks_s* speed) { speed->ticks = 0; return true; } - struct queue_entry* e = &entry[rp & QUEUE_LEN_MASK]; + const struct queue_entry* e = &entry[rp & QUEUE_LEN_MASK]; if (e->hasSteps) { speed->count_up = e->countUp; speed->ticks = e->ticks; diff --git a/src/StepperISR.h b/src/StepperISR.h index 087e40f..d14911c 100644 --- a/src/StepperISR.h +++ b/src/StepperISR.h @@ -54,15 +54,24 @@ class StepperQueue { // provide information, that device is not yet ready for new commands. // This has been called isReadyForCommands(). // - +#if defined(SUPPORT_RP_PICO) + bool _isStarting; + bool isRunning(); + bool isReadyForCommands(); + uint8_t _step_pin; + PIO pio; + uint sm; + bool claim_pio_sm(FastAccelStepperEngine* engine); + void setupSM(); +#endif #if defined(SUPPORT_ESP32) volatile bool _isRunning; bool _nextCommandIsPrepared; inline bool isRunning() { return _isRunning; } - bool isReadyForCommands() const; + bool isReadyForCommands(); bool use_rmt; uint8_t _step_pin; - uint16_t _getPerformedPulses() const; + uint16_t _getPerformedPulses(); #endif #ifdef SUPPORT_ESP32_MCPWM_PCNT const void* driver_data; @@ -70,10 +79,8 @@ class StepperQueue { #ifdef SUPPORT_ESP32_RMT RMT_CHANNEL_T channel; bool _rmtStopped; -#if ESP_IDF_VERSION_MAJOR == 4 - bool bufferContainsSteps[2]; -#else bool lastChunkContainsSteps; +#if defined(SUPPORT_ESP32_RMT_V2) rmt_encoder_handle_t _tx_encoder; #endif #endif @@ -112,7 +119,8 @@ 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; @@ -135,7 +143,7 @@ class StepperQueue { } #endif - int8_t addQueueEntry(const struct stepper_command_s* cmd, bool start); + AqeResultCode addQueueEntry(const struct stepper_command_s* cmd, bool start); int32_t getCurrentPosition(); uint32_t ticksInQueue(); bool hasTicksInQueue(uint32_t min_ticks); @@ -152,7 +160,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(); @@ -160,8 +168,8 @@ class StepperQueue { void disconnect_mcpwm_pcnt(); #endif #ifdef SUPPORT_ESP32_RMT - bool isReadyForCommands_rmt() const; - void init_rmt(uint8_t channel_num, uint8_t step_pin); + bool isReadyForCommands_rmt(); + 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); @@ -169,7 +177,7 @@ class StepperQueue { bool _channel_enabled; #endif void forceStop_rmt(); - static uint16_t _getPerformedPulses_rmt(); + uint16_t _getPerformedPulses_rmt(); void connect_rmt(); void disconnect_rmt(); #endif @@ -199,4 +207,13 @@ class StepperQueue { extern StepperQueue fas_queue[NUM_QUEUES]; +#if defined(SUPPORT_ESP32_RMT) +void rmt_fill_buffer(StepperQueue* q, bool fill_part_one, uint32_t* data); +void rmt_apply_command(StepperQueue* q, bool fill_part_one, uint32_t* data); +#endif + +#if defined(SUPPORT_CPU_AFFINITY) void fas_init_engine(FastAccelStepperEngine* engine, uint8_t cpu_core); +#else +void fas_init_engine(FastAccelStepperEngine* engine); +#endif diff --git a/src/StepperISR_avr.cpp b/src/StepperISR_avr.cpp index c8c8d50..b0e9da0 100644 --- a/src/StepperISR_avr.cpp +++ b/src/StepperISR_avr.cpp @@ -45,22 +45,29 @@ #elif defined(SIMAVR_TIME_MEASUREMENT_QUEUE) #define prepareISRtimeMeasurement() DDRB |= 0x10 #define enterStepperISR() \ - {} + { \ + } #define exitStepperISR() \ - {} + { \ + } #define enterFillQueueISR() PORTB |= 0x10 #define exitFillQueueISR() PORTB ^= 0x10 #else #define prepareISRtimeMeasurement() \ - {} + { \ + } #define enterStepperISR() \ - {} + { \ + } #define exitStepperISR() \ - {} + { \ + } #define enterFillQueueISR() \ - {} + { \ + } #define exitFillQueueISR() \ - {} + { \ + } #endif #ifdef SUPPORT_EXTERNAL_DIRECTION_PIN @@ -108,7 +115,8 @@ 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); @@ -116,17 +124,20 @@ void StepperQueue::init(uint8_t queue_num, uint8_t step_pin) { if (step_pin == stepPinStepperA) { 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 @@ -253,7 +264,7 @@ AVR_CYCLIC_ISR_GEN(FAS_TIMER_MODULE) void StepperQueue::startQueue() { uint8_t rp; - struct queue_entry* e; + const struct queue_entry* e; _isRunning = true; switch (channel) { @@ -353,7 +364,5 @@ void StepperQueue::adjustSpeedToStepperCount(uint8_t steppers) { } } -void fas_init_engine(FastAccelStepperEngine* engine, uint8_t cpu_core) { - fas_engine = engine; -} +void fas_init_engine(FastAccelStepperEngine* engine) { fas_engine = engine; } #endif diff --git a/src/StepperISR_due.cpp b/src/StepperISR_due.cpp index 5fc6943..b3d06d6 100644 --- a/src/StepperISR_due.cpp +++ b/src/StepperISR_due.cpp @@ -452,7 +452,8 @@ 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 +522,7 @@ void StepperQueue::init(uint8_t queue_num, uint8_t step_pin) { PWM_INTERFACE->PWM_IDR1 = 0x00FFFF0F; connect(); + return true; } void StepperQueue::connect() { @@ -713,7 +715,5 @@ void StepperQueue::adjustSpeedToStepperCount(uint8_t steppers) { max_speed_in_ticks = 420; // This equals 50kHz @ 21MHz } -void fas_init_engine(FastAccelStepperEngine* engine, uint8_t cpu_core) { - fas_engine = engine; -} +void fas_init_engine(FastAccelStepperEngine* engine) { fas_engine = engine; } #endif diff --git a/src/StepperISR_esp32.cpp b/src/StepperISR_esp32.cpp index 98c5007..0c6e5de 100644 --- a/src/StepperISR_esp32.cpp +++ b/src/StepperISR_esp32.cpp @@ -5,20 +5,21 @@ // 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() { @@ -45,7 +46,7 @@ void StepperQueue::disconnect() { #endif } -bool StepperQueue::isReadyForCommands() const { +bool StepperQueue::isReadyForCommands() { #if defined(SUPPORT_ESP32_RMT) && defined(SUPPORT_ESP32_MCPWM_PCNT) if (use_rmt) { return isReadyForCommands_rmt(); @@ -82,7 +83,7 @@ void StepperQueue::forceStop() { forceStop_mcpwm_pcnt(); #endif } -uint16_t StepperQueue::_getPerformedPulses() const { +uint16_t StepperQueue::_getPerformedPulses() { #ifdef SUPPORT_ESP32_RMT if (use_rmt) { return _getPerformedPulses_rmt(); @@ -104,10 +105,8 @@ bool StepperQueue::isValidStepPin(uint8_t step_pin) { int8_t StepperQueue::queueNumForStepPin(uint8_t step_pin) { return -1; } //************************************************************************************************* -[[noreturn]] void StepperTask(void *parameter) { - auto *engine = (FastAccelStepperEngine *)parameter; - const TickType_t delay_4ms = - (DELAY_MS_BASE + portTICK_PERIOD_MS - 1) / portTICK_PERIOD_MS; +void StepperTask(void *parameter) { + FastAccelStepperEngine *engine = (FastAccelStepperEngine *)parameter; while (true) { engine->manageSteppers(); #if ESP_IDF_VERSION_MAJOR == 4 @@ -115,7 +114,9 @@ int8_t StepperQueue::queueNumForStepPin(uint8_t step_pin) { return -1; } // causes an issue. esp_task_wdt_reset(); #endif - vTaskDelay(delay_4ms); + const TickType_t delay_time = + (engine->_delay_ms + portTICK_PERIOD_MS - 1) / portTICK_PERIOD_MS; + vTaskDelay(delay_time); } } @@ -131,11 +132,12 @@ void fas_init_engine(FastAccelStepperEngine *engine, uint8_t cpu_core) { #define STACK_SIZE 3000 #define PRIORITY (configMAX_PRIORITIES - 1) #endif + engine->_delay_ms = DELAY_MS_BASE; if (cpu_core > 1) { - xTaskCreate(StepperTask, "StepperTask", STACK_SIZE, engine, PRIORITY, nullptr); + xTaskCreate(StepperTask, "StepperTask", STACK_SIZE, engine, PRIORITY, NULL); } else { xTaskCreatePinnedToCore(StepperTask, "StepperTask", STACK_SIZE, engine, - PRIORITY, nullptr, cpu_core); + PRIORITY, NULL, cpu_core); } } diff --git a/src/StepperISR_esp32xx_rmt.cpp b/src/StepperISR_esp32xx_rmt.cpp new file mode 100644 index 0000000..c6b8c5e --- /dev/null +++ b/src/StepperISR_esp32xx_rmt.cpp @@ -0,0 +1,254 @@ +#include "StepperISR.h" +#if defined(SUPPORT_ESP32_RMT) + +// #define TEST_MODE + +#include "fas_arch/test_probe.h" + +// The following concept is in use: +// +// The rmt buffer is split into two parts. +// Each part will hold one command (or part of). +// +// This way, the threshold interrupt occurs after the first part +// and the end interrupt after the second part. +// After the two parts an end marker is placed. +// +// Of these 32 bits, the low 16-bit entry is sent first and the high entry +// second. +// Every 16 bit entry defines with MSB the output level and the lower 15 bits +// the ticks. +// +// Important difference of esp32c3/esp32s3 (compared to esp32): +// Esp32c3 and Esp32s3 technical reference manuals state for relation 1: +// 3*T_APB + 5*T_RMT_CLK < period*T_CLK_DIV +// and relation 2, if period[14:0] == 0: +// 6*T_APB + 12*T_RMT_CLK < period*T_CLK_DIV +// +// Relation 2 is not applicable, because our end marker is 0x00000000 +// +// Relation 1 means: +// T_APB = 1/80MHz = 12.5ns +// T_CLK_DIV = 1/16MHz = 62.5ns +// T_RMT_CLK = 1/80MHz = 12.5ns +// => period > (3*12.5ns + 5*12.5ns)/(62.5ns) = 1.6 +// +void IRAM_ATTR rmt_fill_buffer(StepperQueue *q, bool fill_part_one, + uint32_t *data) { + // Process command + uint8_t rp = q->read_idx; + struct queue_entry *e_curr = &q->entry[rp & QUEUE_LEN_MASK]; + if (e_curr->toggle_dir) { + // the command requests dir pin toggle + // This is ok only, if the ongoing command does not contain steps + if (q->lastChunkContainsSteps) { + // So we need a pause. change the finished read entry into a pause + q->lastChunkContainsSteps = false; + for (uint8_t i = 0; i < PART_SIZE; i++) { + // two pauses à n ticks to achieve MIN_CMD_TICKS + *data++ = 0x00010001 * + ((MIN_CMD_TICKS + 2 * PART_SIZE - 1) / (2 * PART_SIZE)); + } + return; + } + // The ongoing command does not contain steps, so change dir here should be + // ok. But we need the gpio_ll functions, which are always + // inlined...hopefully. + LL_TOGGLE_PIN(q->dirPin); + // and delete the request + e_curr->toggle_dir = 0; + } + + uint8_t steps = e_curr->steps; + uint16_t ticks = e_curr->ticks; + // if (steps != 0) { + // PROBE_1_TOGGLE; + //} + if (steps == 0) { + uint32_t last_entry; + q->lastChunkContainsSteps = false; + for (uint8_t i = 0; i < PART_SIZE - 1; i++) { + // two pauses à 4 ticks + *data++ = 0x00020002; + ticks -= 4; + } + uint16_t ticks_l = ticks >> 1; + uint16_t ticks_r = ticks - ticks_l; + last_entry = ticks_l; + last_entry <<= 16; + last_entry |= ticks_r; + *data++ = last_entry; + } else { + q->lastChunkContainsSteps = true; + if (ticks == 0xffff) { + // special treatment for this case, because an rmt entry can only cover up + // to 0xfffe ticks every step must be minimum split into two rmt entries, + // so at max PART/2 steps can be done. + if (steps < PART_SIZE / 2) { + for (uint8_t i = 1; i < steps; i++) { + // steps-1 iterations + *data++ = 0x40007fff | 0x8000; + *data++ = 0x20002000; + } + *data++ = 0x40007fff | 0x8000; + uint16_t delta = PART_SIZE - 2 * steps; + delta <<= 5; + *data++ = 0x20002000 - delta; + // 2*(steps - 1) + 1 already stored => 2*steps - 1 + // and after this for loop one entry added => 2*steps + for (uint8_t i = 2 * steps; i < PART_SIZE; i++) { + *data++ = 0x00100010; + } + steps = 0; + } else { + steps -= PART_SIZE / 2; + for (uint8_t i = 0; i < PART_SIZE / 2; i++) { + *data++ = 0x40007fff | 0x8000; + *data++ = 0x20002000; + } + } + } else { + uint8_t steps_to_do = steps; + if (steps > PART_SIZE) { + steps_to_do = PART_SIZE; + if (steps_to_do > PART_SIZE) { + steps_to_do >>= 1; + } + } + // deduct this buffer's step from total + steps -= steps_to_do; + if (steps_to_do < PART_SIZE) { + // We need to fill up the partition. + // Worst case would be one step in the buffer. + // In order to get threshold interrupt early enough, + // the first step should be stretched to maximum. + // The minimum period is 80ticks @200kHz. + // Minimum command time is 3200 ticks. + // Consequently, 80 ticks will come with minimum 40 steps. + // Split into two rmt parts, so 20 steps each. + // For PART_SIZE 23 or 31, minimum period is 2*2*PART_SIZE = 92 or + // 124ticks. A single step with 80ticks cannot be stretched to + // PART_SIZE. + // => we need to stretch eventually two steps. + uint8_t i = 0; + while (true) { + uint8_t extend_to_i = PART_SIZE - steps_to_do; // extend_to_i >= 1 + // if steps_to_do = PART_SIZE-1, then extend_to_i = 1 + if (i >= extend_to_i) { + // we have already extended enough + break; + } + steps_to_do--; // one step less to do + uint16_t ticks_high = ticks >> 1; + uint16_t ticks_low = ticks - ticks_high; + while (true) { + // need i+1, because low entry is still needed + if ((ticks_high >= 8) && (i + 1 < extend_to_i)) { + *data++ = 0x80028002; + i++; + ticks_high -= 4; + } else { + uint32_t entry = ticks_high>>1; + entry <<= 16; + entry |= ticks_high - (ticks_high>>1); + *data++ = 0x80008000 | entry; + i++; + break; + } + } + while (true) { + if ((ticks_low >= 8) && (i < extend_to_i)) { + *data++ = 0x00020002; + i++; + ticks_low -= 4; + } else { + // #325: esp32c3 has frozen with one step and ticks ~38600 + // now the last entry is balanced instead of long pause and then 2 ticks + uint32_t entry = ticks_low>>1; + entry <<= 16; + entry |= ticks_low - (ticks_low>>1); + *data++ = entry; + i++; + break; + } + } + } + // Now add remaining steps, if any + if (steps_to_do > 0) { + uint16_t ticks_high = ticks >> 1; + uint16_t ticks_low = ticks - ticks_high; + // ticks_l <= ticks_r + uint32_t rmt_entry = ticks_low; + rmt_entry <<= 16; + rmt_entry |= ticks_high | 0x8000; // with step + for (uint8_t i = 1; i <= steps_to_do; i++) { + *data++ = rmt_entry; + } + } + } else { + // either >= 2*PART_SIZE or = PART_SIZE + // every entry one step + uint16_t ticks_high = ticks >> 1; + uint16_t ticks_low = ticks - ticks_high; + uint32_t rmt_entry = ticks_low; + rmt_entry <<= 16; + rmt_entry |= ticks_high | 0x8000; // with step + for (uint8_t i = 0; i < PART_SIZE; i++) { + *data++ = rmt_entry; + } + } + } + } +#if defined(SUPPORT_ESP32_RMT_TICK_LOST) + // No tick lost mentioned for esp32s3 and esp32c3 + if (!fill_part_one) { + // Note: When enabling the continuous transmission mode by setting + // RMT_REG_TX_CONTI_MODE, the transmitter will transmit the data on the + // channel continuously, that is, from the first byte to the last one, + // then from the first to the last again, and so on. In this mode, there + // will be an idle level lasting one clk_div cycle between N and N+1 + // transmissions. + data--; + *data -= 1; + } +#endif + + // Data is complete + if (steps == 0) { + // The command has been completed + if (e_curr->repeat_entry == 0) { + q->read_idx = rp + 1; + PROBE_4_TOGGLE; + } + } else { + e_curr->steps = steps; + } + return; +} +#if (ESP_IDF_VERSION_MAJOR == 4) +void IRAM_ATTR rmt_apply_command(StepperQueue *q, bool fill_part_one, + uint32_t *data) { + if (!fill_part_one) { + data += PART_SIZE; + } + uint8_t rp = q->read_idx; + if (rp == q->next_write_idx) { + // no command in queue + if (fill_part_one) { + q->lastChunkContainsSteps = false; + for (uint8_t i = 0; i < PART_SIZE; i++) { + // make a pause with approx. 1ms + // 258 ticks * 2 * 31 = 15996 @ 16MHz + // 347 ticks * 2 * 23 = 15962 @ 16MHz + *data++ = 0x00010001 * (16000 / 2 / PART_SIZE); + } + } else { + q->stop_rmt(false); + } + return; + } + rmt_fill_buffer(q, fill_part_one, data); +} +#endif + +#endif diff --git a/src/StepperISR_idf4_esp32_mcpwm_pcnt.cpp b/src/StepperISR_idf4_esp32_mcpwm_pcnt.cpp index 73c2128..aa66553 100644 --- 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 index 9f9e2a6..8c3879e 100644 --- a/src/StepperISR_idf4_esp32_rmt.cpp +++ b/src/StepperISR_idf4_esp32_rmt.cpp @@ -19,7 +19,6 @@ // second. // Every 16 bit entry defines with MSB the output level and the lower 15 bits // the ticks. -#define PART_SIZE 31 void IRAM_ATTR StepperQueue::stop_rmt(bool both) { // We are stopping the rmt by letting it run into the end at high speed. @@ -45,158 +44,6 @@ void IRAM_ATTR StepperQueue::stop_rmt(bool both) { _rmtStopped = true; } -static void IRAM_ATTR apply_command(StepperQueue *q, bool fill_part_one, - uint32_t *data) { - if (!fill_part_one) { - data += PART_SIZE; - } - uint8_t rp = q->read_idx; - if (rp == q->next_write_idx) { - // no command in queue - if (fill_part_one) { - q->bufferContainsSteps[0] = false; - for (uint8_t i = 0; i < PART_SIZE; i++) { - // make a pause with approx. 1ms - // 258 ticks * 2 * 31 = 15996 @ 16MHz - *data++ = 0x01020102; - } - } else { - q->stop_rmt(false); - } - return; - } - // Process command - struct queue_entry *e_curr = &q->entry[rp & QUEUE_LEN_MASK]; - - if (e_curr->toggle_dir) { - // the command requests dir pin toggle - // This is ok only, if the ongoing command does not contain steps - if (q->bufferContainsSteps[fill_part_one ? 1 : 0]) { - // So we need a pause. change the finished read entry into a pause - q->bufferContainsSteps[fill_part_one ? 0 : 1] = false; - for (uint8_t i = 0; i < PART_SIZE; i++) { - // two pauses à n ticks to achieve MIN_CMD_TICKS - *data++ = 0x00010001 * ((MIN_CMD_TICKS + 61) / 62); - } - return; - } - // The ongoing command does not contain steps, so change dir here should be - // ok. But we need the gpio_ll functions, which are always - // inlined...hopefully. - LL_TOGGLE_PIN(q->dirPin); - // and delete the request - e_curr->toggle_dir = 0; - } - - uint8_t steps = e_curr->steps; - uint16_t ticks = e_curr->ticks; - // if (steps != 0) { - // PROBE_2_TOGGLE; - //} - uint32_t last_entry; - if (steps == 0) { - q->bufferContainsSteps[fill_part_one ? 0 : 1] = false; - for (uint8_t i = 0; i < PART_SIZE - 1; i++) { - // two pauses à 3 ticks. the 2 for debugging - *data++ = 0x00010002; - ticks -= 3; - } - uint16_t ticks_l = ticks >> 1; - uint16_t ticks_r = ticks - ticks_l; - last_entry = ticks_l; - last_entry <<= 16; - last_entry |= ticks_r; - } else { - q->bufferContainsSteps[fill_part_one ? 0 : 1] = true; - if (ticks == 0xffff) { - // special treatment for this case, because an rmt entry can only cover up - // to 0xfffe ticks every step must be minimum split into two rmt entries, - // so at max PART/2 steps can be done. - if (steps < PART_SIZE / 2) { - for (uint8_t i = 1; i < steps; i++) { - // steps-1 iterations - *data++ = 0x40007fff | 0x8000; - *data++ = 0x20002000; - } - *data++ = 0x40007fff | 0x8000; - uint16_t delta = PART_SIZE - 2 * steps; - delta <<= 5; - *data++ = 0x20002000 - delta; - // 2*(steps - 1) + 1 already stored => 2*steps - 1 - // and after this for loop one entry added => 2*steps - for (uint8_t i = 2 * steps; i < PART_SIZE - 1; i++) { - *data++ = 0x00100010; - } - last_entry = 0x00100010; - steps = 0; - } else { - steps -= PART_SIZE / 2; - for (uint8_t i = 0; i < PART_SIZE / 2 - 1; i++) { - *data++ = 0x40007fff | 0x8000; - *data++ = 0x20002000; - } - *data++ = 0x40007fff | 0x8000; - last_entry = 0x20002000; - } - } else if ((steps < 2 * PART_SIZE) && (steps != PART_SIZE)) { - uint8_t steps_to_do = steps; - if (steps > PART_SIZE) { - steps_to_do /= 2; - } - - uint16_t ticks_l = ticks >> 1; - uint16_t ticks_r = ticks - ticks_l; - uint32_t rmt_entry = ticks_l; - rmt_entry <<= 16; - rmt_entry |= ticks_r | 0x8000; // with step - for (uint8_t i = 1; i < steps_to_do; i++) { - *data++ = rmt_entry; - } - uint32_t delta = PART_SIZE - steps_to_do; - delta <<= 18; // shift in upper 16bit and multiply with 4 - *data++ = rmt_entry - delta; - for (uint8_t i = steps_to_do; i < PART_SIZE - 1; i++) { - *data++ = 0x00020002; - } - last_entry = 0x00020002; - steps -= steps_to_do; - } else { - // either >= 2*PART_SIZE or = PART_SIZE - // every entry one step - uint16_t ticks_l = ticks >> 1; - uint16_t ticks_r = ticks - ticks_l; - uint32_t rmt_entry = ticks_l; - rmt_entry <<= 16; - rmt_entry |= ticks_r | 0x8000; // with step - for (uint8_t i = 0; i < PART_SIZE - 1; i++) { - *data++ = rmt_entry; - } - last_entry = rmt_entry; - steps -= PART_SIZE; - } - } - if (!fill_part_one) { - // Note: When enabling the continuous transmission mode by setting - // RMT_REG_TX_CONTI_MODE, the transmitter will transmit the data on the - // channel continuously, that is, from the first byte to the last one, - // then from the first to the last again, and so on. In this mode, there - // will be an idle level lasting one clk_div cycle between N and N+1 - // transmissions. - last_entry -= 1; - } - *data = last_entry; - - // Data is complete - if (steps == 0) { - // The command has been completed - if (e_curr->repeat_entry == 0) { - q->read_idx = rp + 1; - } - } else { - e_curr->steps = steps; - } -} - #ifndef RMT_CHANNEL_MEM #define RMT_LIMIT tx_lim #define RMT_FIFO apb_fifo_mask @@ -226,14 +73,14 @@ static void IRAM_ATTR apply_command(StepperQueue *q, bool fill_part_one, q->_isRunning = false; \ PROBE_1_TOGGLE; \ } else { \ - apply_command(q, false, FAS_RMT_MEM(ch)); \ + rmt_apply_command(q, false, FAS_RMT_MEM(ch)); \ } \ } \ if (mask & RMT_CH##ch##_TX_THR_EVENT_INT_ST) { \ PROBE_3_TOGGLE; \ StepperQueue *q = &fas_queue[QUEUES_MCPWM_PCNT + ch]; \ if (!q->_rmtStopped) { \ - apply_command(q, true, FAS_RMT_MEM(ch)); \ + rmt_apply_command(q, true, FAS_RMT_MEM(ch)); \ /* now repeat the interrupt at buffer size + end marker */ \ RMT.tx_lim_ch[ch].RMT_LIMIT = PART_SIZE * 2 + 1; \ } \ @@ -257,7 +104,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 +206,7 @@ void StepperQueue::init_rmt(uint8_t channel_num, uint8_t step_pin) { } } #endif + return true; } void StepperQueue::connect_rmt() { @@ -449,9 +297,8 @@ void StepperQueue::startQueue_rmt() { entry[rp & QUEUE_LEN_MASK].toggle_dir = false; } - bufferContainsSteps[0] = true; - bufferContainsSteps[1] = true; - apply_command(this, true, mem); + lastChunkContainsSteps = true; + rmt_apply_command(this, true, mem); #ifdef TRACE Serial.print(RMT.conf_ch[channel].conf0.val, BIN); @@ -470,7 +317,7 @@ void StepperQueue::startQueue_rmt() { } #endif - apply_command(this, false, mem); + rmt_apply_command(this, false, mem); #ifdef TRACE Serial.print(RMT.conf_ch[channel].conf0.val, BIN); diff --git a/src/StepperISR_idf4_esp32c3_rmt.cpp b/src/StepperISR_idf4_esp32c3_rmt.cpp index 4c2e10e..39a0e6f 100644 --- a/src/StepperISR_idf4_esp32c3_rmt.cpp +++ b/src/StepperISR_idf4_esp32c3_rmt.cpp @@ -26,15 +26,6 @@ // - minimum periods as per relation 1 and 2 to be adhered to // // -#ifdef HAVE_ESP32C3_RMT -#define PART_SIZE 23 -#define RMT_MEM_SIZE 48 -#else -#error -#define PART_SIZE 31 -#define RMT_MEM_SIZE 64 -#endif - // In order to avoid threshold/end interrupt on end, add one #define enable_rmt_interrupts(channel) \ { \ @@ -44,8 +35,10 @@ RMT.int_clr.val |= 0x101 << channel; \ RMT.int_ena.val |= 0x101 << channel; \ } -#define disable_rmt_interrupts(channel) \ - { RMT.int_ena.val &= ~(0x101 << channel); } +#define disable_rmt_interrupts(channel) \ + { \ + RMT.int_ena.val &= ~(0x101 << channel); \ + } void IRAM_ATTR StepperQueue::stop_rmt(bool both) { // We are stopping the rmt by letting it run into the end at high speed. @@ -73,162 +66,6 @@ void IRAM_ATTR StepperQueue::stop_rmt(bool both) { _rmtStopped = true; } -static void IRAM_ATTR apply_command(StepperQueue *q, bool fill_part_one, - uint32_t *data) { - if (!fill_part_one) { - data += PART_SIZE; - } - uint8_t rp = q->read_idx; - if (rp == q->next_write_idx) { - // no command in queue - if (fill_part_one) { - q->bufferContainsSteps[0] = false; - for (uint8_t i = 0; i < PART_SIZE; i++) { - // make a pause with approx. 1ms - // 258 ticks * 2 * 31 = 15996 @ 16MHz - // 347 ticks * 2 * 23 = 15962 @ 16MHz - *data++ = 0x010001 * (16000 / 2 / PART_SIZE); - } - } else { - q->stop_rmt(false); - } - return; - } - // Process command - struct queue_entry *e_curr = &q->entry[rp & QUEUE_LEN_MASK]; - - if (e_curr->toggle_dir) { - // the command requests dir pin toggle - // This is ok only, if the ongoing command does not contain steps - if (q->bufferContainsSteps[fill_part_one ? 1 : 0]) { - // So we need a pause. change the finished read entry into a pause - q->bufferContainsSteps[fill_part_one ? 0 : 1] = false; - for (uint8_t i = 0; i < PART_SIZE; i++) { - // two pauses à n ticks to achieve MIN_CMD_TICKS - *data++ = 0x00010001 * - ((MIN_CMD_TICKS + 2 * PART_SIZE - 1) / (2 * PART_SIZE)); - } - return; - } - // The ongoing command does not contain steps, so change dir here should be - // ok - LL_TOGGLE_PIN(q->dirPin); - // and delete the request - e_curr->toggle_dir = 0; - } - - uint8_t steps = e_curr->steps; - uint16_t ticks = e_curr->ticks; - // if (steps != 0) { - // PROBE_1_TOGGLE; - //} - uint32_t last_entry; - if (steps == 0) { - q->bufferContainsSteps[fill_part_one ? 0 : 1] = false; - // Perhaps the rmt performs look ahead - ticks -= (PART_SIZE - 2) * 4 + 8; - uint16_t ticks_l = ticks >> 1; - uint16_t ticks_r = ticks - ticks_l; - last_entry = ticks_l; - last_entry <<= 16; - last_entry |= ticks_r; - *data++ = last_entry; - for (uint8_t i = 1; i < PART_SIZE - 1; i++) { - *data++ = 0x00020002; - } - last_entry = 0x00040004; - } else { - q->bufferContainsSteps[fill_part_one ? 0 : 1] = true; - if (ticks == 0xffff) { - // special treatment for this case, because an rmt entry can only cover up - // to 0xfffe ticks every step must be minimum split into two rmt entries, - // so at max PART/2 steps can be done. - if (steps < PART_SIZE / 2) { - for (uint8_t i = 1; i < steps; i++) { - // steps-1 iterations - *data++ = 0x40007fff | 0x8000; - *data++ = 0x20002000; - } - *data++ = 0x40007fff | 0x8000; - uint16_t delta = PART_SIZE - 2 * steps; - delta <<= 5; - *data++ = 0x20002000 - delta; - // 2*(steps - 1) + 1 already stored => 2*steps - 1 - // and after this for loop one entry added => 2*steps - for (uint8_t i = 2 * steps; i < PART_SIZE - 1; i++) { - *data++ = 0x00100010; - } - last_entry = 0x00100010; - steps = 0; - } else { - steps -= PART_SIZE / 2; - for (uint8_t i = 0; i < PART_SIZE / 2 - 1; i++) { - *data++ = 0x40007fff | 0x8000; - *data++ = 0x20002000; - } - *data++ = 0x40007fff | 0x8000; - last_entry = 0x20002000; - } - } else if ((steps < 2 * PART_SIZE) && (steps != PART_SIZE)) { - uint8_t steps_to_do = steps; - if (steps > PART_SIZE) { - steps_to_do /= 2; - } - - uint16_t ticks_l = ticks >> 1; - uint16_t ticks_r = ticks - ticks_l; - uint32_t rmt_entry = ticks_l; - rmt_entry <<= 16; - rmt_entry |= ticks_r | 0x8000; // with step - for (uint8_t i = 1; i < steps_to_do; i++) { - *data++ = rmt_entry; - } - uint32_t delta = (PART_SIZE - steps_to_do) * 4 + 8; - delta <<= 16; // shift in upper 16bit - *data++ = rmt_entry - delta; - for (uint8_t i = steps_to_do; i < PART_SIZE - 1; i++) { - *data++ = 0x00020002; - } - last_entry = 0x00040004; - steps -= steps_to_do; - } else { - // either >= 2*PART_SIZE or = PART_SIZE - // every entry one step - uint16_t ticks_l = ticks >> 1; - uint16_t ticks_r = ticks - ticks_l; - uint32_t rmt_entry = ticks_l; - rmt_entry <<= 16; - rmt_entry |= ticks_r | 0x8000; // with step - for (uint8_t i = 0; i < PART_SIZE - 1; i++) { - *data++ = rmt_entry; - } - last_entry = rmt_entry; - steps -= PART_SIZE; - } - } - // No tick lost mentioned for esp32c3 - // if (!fill_part_one) { - // Note: When enabling the continuous transmission mode by setting - // RMT_REG_TX_CONTI_MODE, the transmitter will transmit the data on the - // channel continuously, that is, from the first byte to the last one, - // then from the first to the last again, and so on. In this mode, there - // will be an idle level lasting one clk_div cycle between N and N+1 - // transmissions. - // last_entry -= 1; - // } - *data = last_entry; - - // Data is complete - if (steps == 0) { - // The command has been completed - if (e_curr->repeat_entry == 0) { - q->read_idx = rp + 1; - } - } else { - e_curr->steps = steps; - } -} - #if !defined(RMT_CHANNEL_MEM) && !defined(HAVE_ESP32C3_RMT) #define RMT_LIMIT tx_lim #define RMT_FIFO apb_fifo_mask @@ -284,14 +121,14 @@ static void IRAM_ATTR apply_command(StepperQueue *q, bool fill_part_one, if (old_limit == PART_SIZE + 1) { \ /* second half of buffer sent */ \ PROBE_2_TOGGLE; \ - apply_command(q, false, mem); \ + rmt_apply_command(q, false, mem); \ /* demonstrate modification of RAM */ \ /*mem[PART_SIZE] = 0x33fff3ff; */ \ RMT.tx_lim[ch].RMT_LIMIT = PART_SIZE; \ } else { \ /* first half of buffer sent */ \ PROBE_3_TOGGLE; \ - apply_command(q, true, mem); \ + rmt_apply_command(q, true, mem); \ RMT.tx_lim[ch].RMT_LIMIT = PART_SIZE + 1; \ } \ RMT.tx_conf[ch].conf_update = 1; \ @@ -316,7 +153,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); @@ -442,6 +279,7 @@ void StepperQueue::init_rmt(uint8_t channel_num, uint8_t step_pin) { } } #endif + return true; } void StepperQueue::connect_rmt() { @@ -581,9 +419,8 @@ void StepperQueue::startQueue_rmt() { entry[rp & QUEUE_LEN_MASK].toggle_dir = false; } - bufferContainsSteps[0] = true; - bufferContainsSteps[1] = true; - apply_command(this, true, mem); + lastChunkContainsSteps = true; + rmt_apply_command(this, true, mem); #ifdef TRACE USBSerial.print(RMT.tx_conf[channel].val, BIN); @@ -600,7 +437,7 @@ void StepperQueue::startQueue_rmt() { } #endif - apply_command(this, false, mem); + rmt_apply_command(this, false, mem); #ifdef TRACE USBSerial.print(RMT.tx_conf[channel].val, BIN); diff --git a/src/StepperISR_idf4_esp32s3_rmt.cpp b/src/StepperISR_idf4_esp32s3_rmt.cpp index f37a273..1363f06 100644 --- a/src/StepperISR_idf4_esp32s3_rmt.cpp +++ b/src/StepperISR_idf4_esp32s3_rmt.cpp @@ -26,14 +26,6 @@ // - minimum periods as per relation 1 and 2 to be adhered to // // -#ifdef HAVE_ESP32S3_RMT -#define PART_SIZE 23 -#define RMT_MEM_SIZE 48 -#else -#error -#define PART_SIZE 31 -#define RMT_MEM_SIZE 64 -#endif // In order to avoid threshold/end interrupt on end, add one #define enable_rmt_interrupts(channel) \ @@ -44,8 +36,10 @@ RMT.int_clr.val |= 0x101 << channel; \ RMT.int_ena.val |= 0x101 << channel; \ } -#define disable_rmt_interrupts(channel) \ - { RMT.int_ena.val &= ~(0x101 << channel); } +#define disable_rmt_interrupts(channel) \ + { \ + RMT.int_ena.val &= ~(0x101 << channel); \ + } void IRAM_ATTR StepperQueue::stop_rmt(bool both) { // We are stopping the rmt by letting it run into the end at high speed. @@ -73,162 +67,6 @@ void IRAM_ATTR StepperQueue::stop_rmt(bool both) { _rmtStopped = true; } -static void IRAM_ATTR apply_command(StepperQueue *q, bool fill_part_one, - uint32_t *data) { - if (!fill_part_one) { - data += PART_SIZE; - } - uint8_t rp = q->read_idx; - if (rp == q->next_write_idx) { - // no command in queue - if (fill_part_one) { - q->bufferContainsSteps[0] = false; - for (uint8_t i = 0; i < PART_SIZE; i++) { - // make a pause with approx. 1ms - // 258 ticks * 2 * 31 = 15996 @ 16MHz - // 347 ticks * 2 * 23 = 15962 @ 16MHz - *data++ = 0x010001 * (16000 / 2 / PART_SIZE); - } - } else { - q->stop_rmt(false); - } - return; - } - // Process command - struct queue_entry *e_curr = &q->entry[rp & QUEUE_LEN_MASK]; - - if (e_curr->toggle_dir) { - // the command requests dir pin toggle - // This is ok only, if the ongoing command does not contain steps - if (q->bufferContainsSteps[fill_part_one ? 1 : 0]) { - // So we need a pause. change the finished read entry into a pause - q->bufferContainsSteps[fill_part_one ? 0 : 1] = false; - for (uint8_t i = 0; i < PART_SIZE; i++) { - // two pauses à n ticks to achieve MIN_CMD_TICKS - *data++ = 0x00010001 * - ((MIN_CMD_TICKS + 2 * PART_SIZE - 1) / (2 * PART_SIZE)); - } - return; - } - // The ongoing command does not contain steps, so change dir here should be - // ok - LL_TOGGLE_PIN(q->dirPin); - // and delete the request - e_curr->toggle_dir = 0; - } - - uint8_t steps = e_curr->steps; - uint16_t ticks = e_curr->ticks; - // if (steps != 0) { - // PROBE_1_TOGGLE; - //} - uint32_t last_entry; - if (steps == 0) { - q->bufferContainsSteps[fill_part_one ? 0 : 1] = false; - // Perhaps the rmt performs look ahead - ticks -= (PART_SIZE - 2) * 4 + 8; - uint16_t ticks_l = ticks >> 1; - uint16_t ticks_r = ticks - ticks_l; - last_entry = ticks_l; - last_entry <<= 16; - last_entry |= ticks_r; - *data++ = last_entry; - for (uint8_t i = 1; i < PART_SIZE - 1; i++) { - *data++ = 0x00020002; - } - last_entry = 0x00040004; - } else { - q->bufferContainsSteps[fill_part_one ? 0 : 1] = true; - if (ticks == 0xffff) { - // special treatment for this case, because an rmt entry can only cover up - // to 0xfffe ticks every step must be minimum split into two rmt entries, - // so at max PART/2 steps can be done. - if (steps < PART_SIZE / 2) { - for (uint8_t i = 1; i < steps; i++) { - // steps-1 iterations - *data++ = 0x40007fff | 0x8000; - *data++ = 0x20002000; - } - *data++ = 0x40007fff | 0x8000; - uint16_t delta = PART_SIZE - 2 * steps; - delta <<= 5; - *data++ = 0x20002000 - delta; - // 2*(steps - 1) + 1 already stored => 2*steps - 1 - // and after this for loop one entry added => 2*steps - for (uint8_t i = 2 * steps; i < PART_SIZE - 1; i++) { - *data++ = 0x00100010; - } - last_entry = 0x00100010; - steps = 0; - } else { - steps -= PART_SIZE / 2; - for (uint8_t i = 0; i < PART_SIZE / 2 - 1; i++) { - *data++ = 0x40007fff | 0x8000; - *data++ = 0x20002000; - } - *data++ = 0x40007fff | 0x8000; - last_entry = 0x20002000; - } - } else if ((steps < 2 * PART_SIZE) && (steps != PART_SIZE)) { - uint8_t steps_to_do = steps; - if (steps > PART_SIZE) { - steps_to_do /= 2; - } - - uint16_t ticks_l = ticks >> 1; - uint16_t ticks_r = ticks - ticks_l; - uint32_t rmt_entry = ticks_l; - rmt_entry <<= 16; - rmt_entry |= ticks_r | 0x8000; // with step - for (uint8_t i = 1; i < steps_to_do; i++) { - *data++ = rmt_entry; - } - uint32_t delta = (PART_SIZE - steps_to_do) * 4 + 8; - delta <<= 16; // shift in upper 16bit - *data++ = rmt_entry - delta; - for (uint8_t i = steps_to_do; i < PART_SIZE - 1; i++) { - *data++ = 0x00020002; - } - last_entry = 0x00040004; - steps -= steps_to_do; - } else { - // either >= 2*PART_SIZE or = PART_SIZE - // every entry one step - uint16_t ticks_l = ticks >> 1; - uint16_t ticks_r = ticks - ticks_l; - uint32_t rmt_entry = ticks_l; - rmt_entry <<= 16; - rmt_entry |= ticks_r | 0x8000; // with step - for (uint8_t i = 0; i < PART_SIZE - 1; i++) { - *data++ = rmt_entry; - } - last_entry = rmt_entry; - steps -= PART_SIZE; - } - } - // No tick lost mentioned for esp32s3 - // if (!fill_part_one) { - // Note: When enabling the continuous transmission mode by setting - // RMT_REG_TX_CONTI_MODE, the transmitter will transmit the data on the - // channel continuously, that is, from the first byte to the last one, - // then from the first to the last again, and so on. In this mode, there - // will be an idle level lasting one clk_div cycle between N and N+1 - // transmissions. - // last_entry -= 1; - // } - *data = last_entry; - - // Data is complete - if (steps == 0) { - // The command has been completed - if (e_curr->repeat_entry == 0) { - q->read_idx = rp + 1; - } - } else { - e_curr->steps = steps; - } -} - #if !defined(RMT_CHANNEL_MEM) && !defined(HAVE_ESP32S3_RMT) #define RMT_LIMIT tx_lim #define RMT_FIFO apb_fifo_mask @@ -284,14 +122,14 @@ static void IRAM_ATTR apply_command(StepperQueue *q, bool fill_part_one, if (old_limit == PART_SIZE + 1) { \ /* second half of buffer sent */ \ PROBE_2_TOGGLE; \ - apply_command(q, false, mem); \ + rmt_apply_command(q, false, mem); \ /* demonstrate modification of RAM */ \ /*mem[PART_SIZE] = 0x33fff3ff; */ \ RMT.chn_tx_lim[ch].RMT_LIMIT = PART_SIZE; \ } else { \ /* first half of buffer sent */ \ PROBE_3_TOGGLE; \ - apply_command(q, true, mem); \ + rmt_apply_command(q, true, mem); \ RMT.chn_tx_lim[ch].RMT_LIMIT = PART_SIZE + 1; \ } \ RMT.chnconf0[ch].conf_update_n = 1; \ @@ -316,7 +154,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); @@ -442,6 +280,7 @@ void StepperQueue::init_rmt(uint8_t channel_num, uint8_t step_pin) { } } #endif + return true; } void StepperQueue::connect_rmt() { @@ -581,9 +420,8 @@ void StepperQueue::startQueue_rmt() { entry[rp & QUEUE_LEN_MASK].toggle_dir = false; } - bufferContainsSteps[0] = true; - bufferContainsSteps[1] = true; - apply_command(this, true, mem); + lastChunkContainsSteps = true; + rmt_apply_command(this, true, mem); #ifdef TRACE USBSerial.print(RMT.chnconf0[channel].val, BIN); @@ -600,7 +438,7 @@ void StepperQueue::startQueue_rmt() { } #endif - apply_command(this, false, mem); + rmt_apply_command(this, false, mem); #ifdef TRACE USBSerial.print(RMT.chnconf0[channel].val, BIN); diff --git a/src/StepperISR_idf5_esp32_rmt.cpp b/src/StepperISR_idf5_esp32_rmt.cpp index e080ad7..d3f5291 100644 --- a/src/StepperISR_idf5_esp32_rmt.cpp +++ b/src/StepperISR_idf5_esp32_rmt.cpp @@ -1,16 +1,14 @@ #include "StepperISR.h" -#if defined(HAVE_ESP32_RMT) && (ESP_IDF_VERSION_MAJOR == 5) +#if defined(SUPPORT_ESP32_RMT_V2) // #define TEST_MODE #include "fas_arch/test_probe.h" -#define PART_SIZE (RMT_SIZE / 2) - static bool IRAM_ATTR queue_done(rmt_channel_handle_t tx_chan, const rmt_tx_done_event_data_t *edata, void *user_ctx) { - auto *q = (StepperQueue *)user_ctx; + StepperQueue *q = (StepperQueue *)user_ctx; q->_isRunning = false; return false; } @@ -18,7 +16,7 @@ static bool IRAM_ATTR queue_done(rmt_channel_handle_t tx_chan, #define ENTER_PAUSE(ticks) \ { \ uint16_t remaining_ticks = ticks; \ - uint16_t half_ticks_per_symbol = (ticks) / (2 * PART_SIZE); \ + uint16_t half_ticks_per_symbol = ticks / (2 * PART_SIZE); \ uint32_t main_symbol = 0x00010001 * half_ticks_per_symbol; \ for (uint8_t i = 0; i < PART_SIZE - 1; i++) { \ (*symbols++).val = main_symbol; \ @@ -26,7 +24,7 @@ static bool IRAM_ATTR queue_done(rmt_channel_handle_t tx_chan, remaining_ticks -= 2 * (PART_SIZE - 1) * half_ticks_per_symbol; \ uint16_t first_ticks = remaining_ticks / 2; \ remaining_ticks -= first_ticks; \ - last_entry = 0x00010000 * first_ticks + remaining_ticks; \ + symbols->val = 0x00010000 * first_ticks + remaining_ticks; \ } static size_t IRAM_ATTR encode_commands(const void *data, size_t data_size, @@ -37,156 +35,30 @@ static size_t IRAM_ATTR encode_commands(const void *data, size_t data_size, // this printf causes Guru Meditation // printf("encode commands\n"); - auto *q = (StepperQueue *)arg; + StepperQueue *q = (StepperQueue *)arg; *done = false; if (symbols_free < PART_SIZE) { // not sufficient space for the symbols return 0; } - - uint8_t rp = q->read_idx; if (q->_rmtStopped) { *done = true; return 0; } + uint8_t rp = q->read_idx; if ((rp == q->next_write_idx) || q->_rmtStopped) { // if we return done already here, then single stepping fails q->_rmtStopped = true; // Not sure if this pause is really needed - uint16_t last_entry; ENTER_PAUSE(MIN_CMD_TICKS); - symbols->val = last_entry; return PART_SIZE; } - - // Process command - struct queue_entry *e_curr = &q->entry[rp & QUEUE_LEN_MASK]; - - if (e_curr->toggle_dir) { - // the command requests dir pin toggle - // This is ok only, if the ongoing command does not contain steps - if (q->lastChunkContainsSteps) { - // So we need a pause. change the finished read entry into a pause - q->lastChunkContainsSteps = false; - uint16_t last_entry; - ENTER_PAUSE(MIN_CMD_TICKS); - symbols->val = last_entry; - return PART_SIZE; - } - // The ongoing command does not contain steps, so change dir here should be - // ok - LL_TOGGLE_PIN(q->dirPin); - // and delete the request - e_curr->toggle_dir = 0; - } - - uint8_t steps = e_curr->steps; - uint16_t ticks = e_curr->ticks; - // if (steps != 0) { - // PROBE_2_TOGGLE; - //} - uint32_t last_entry; - if (steps == 0) { - q->lastChunkContainsSteps = false; - ENTER_PAUSE(ticks); - } else { - q->lastChunkContainsSteps = true; - if (ticks == 0xffff) { - // special treatment for this case, because an rmt entry can only cover up - // to 0xfffe ticks every step must be minimum split into two rmt entries, - // so at max PART/2 steps can be done. - if (steps < PART_SIZE / 2) { - for (uint8_t i = 1; i < steps; i++) { - // steps-1 iterations - (*symbols++).val = 0x40007fff | 0x8000; - (*symbols++).val = 0x20002000; - } - // the last step needs to be stretched to fill PART_SIZE entries - (*symbols++).val = 0x40007fff | 0x8000; - uint16_t delta = PART_SIZE - 2 * steps; - delta <<= 5; - (*symbols++).val = 0x20002000 - delta; - // 2*(steps - 1) + 1 already stored => 2*steps - 1 - // and after this for loop one entry added => 2*steps - for (uint8_t i = 2 * steps; i < PART_SIZE - 1; i++) { - (*symbols++).val = 0x00100010; - } - last_entry = 0x00100010; - steps = 0; - } else { - steps -= PART_SIZE / 2; - for (uint8_t i = 0; i < PART_SIZE / 2 - 1; i++) { - (*symbols++).val = 0x40007fff | 0x8000; - (*symbols++).val = 0x20002000; - } - (*symbols++).val = 0x40007fff | 0x8000; - last_entry = 0x20002000; - } - } else if ((steps < 2 * PART_SIZE) && (steps != PART_SIZE)) { - uint8_t steps_to_do = steps; - if (steps > PART_SIZE) { - steps_to_do /= 2; - } - - uint16_t ticks_l = ticks >> 1; - uint16_t ticks_r = ticks - ticks_l; - uint32_t rmt_entry = ticks_l; - rmt_entry <<= 16; - rmt_entry |= ticks_r | 0x8000; // with step - for (uint8_t i = 1; i < steps_to_do; i++) { - (*symbols++).val = rmt_entry; - } - // the last step needs to be stretched to fill PART_SIZE entries - uint32_t delta = PART_SIZE - steps_to_do; - delta <<= 18; // shift in upper 16bit and multiply with 4 - (*symbols++).val = rmt_entry - delta; - for (uint8_t i = steps_to_do; i < PART_SIZE - 1; i++) { - (*symbols++).val = 0x00020002; - } - last_entry = 0x00020002; - steps -= steps_to_do; - } else { - // either >= 2*PART_SIZE or = PART_SIZE - // every entry one step - uint16_t ticks_l = ticks >> 1; - uint16_t ticks_r = ticks - ticks_l; - uint32_t rmt_entry = ticks_l; - rmt_entry <<= 16; - rmt_entry |= ticks_r | 0x8000; // with step - for (uint8_t i = 0; i < PART_SIZE - 1; i++) { - (*symbols++).val = rmt_entry; - } - last_entry = rmt_entry; - steps -= PART_SIZE; - } - } - - // if (!fill_part_one) { - // Note: When enabling the continuous transmission mode by setting - // RMT_REG_TX_CONTI_MODE, the transmitter will transmit the data on the - // channel continuously, that is, from the first byte to the last one, - // then from the first to the last again, and so on. In this mode, there - // will be an idle level lasting one clk_div cycle between N and N+1 - // transmissions. - // last_entry -= 1; - //} - symbols->val = last_entry; - - // Data is complete - if (steps == 0) { - // The command has been completed - if (e_curr->repeat_entry == 0) { - q->read_idx = rp + 1; - } - } else { - e_curr->steps = steps; - } - + rmt_fill_buffer(q, true, &symbols[0].val); 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,10 +95,11 @@ void StepperQueue::init_rmt(uint8_t channel_num, uint8_t step_pin) { connect_rmt(); _isRunning = false; _rmtStopped = true; + return true; } void StepperQueue::connect_rmt() { - rmt_tx_channel_config_t config; + rmt_tx_channel_config_t config{}; config.gpio_num = (gpio_num_t)_step_pin; config.clk_src = RMT_CLK_SRC_DEFAULT; config.resolution_hz = TICKS_PER_S; @@ -235,13 +108,13 @@ void StepperQueue::connect_rmt() { config.intr_priority = 0; config.flags.invert_out = 0; config.flags.with_dma = 0; - config.flags.io_loop_back = 0; - config.flags.io_od_mode = 0; + //config.flags.io_loop_back = 0; + //config.flags.io_od_mode = 0; esp_err_t rc = rmt_new_tx_channel(&config, &channel); ESP_ERROR_CHECK_WITHOUT_ABORT(rc); rmt_tx_event_callbacks_t callbacks = {.on_trans_done = queue_done}; - (void)rmt_tx_register_event_callbacks(channel, &callbacks, this); + rmt_tx_register_event_callbacks(channel, &callbacks, this); _channel_enabled = false; } @@ -250,8 +123,8 @@ void StepperQueue::disconnect_rmt() { if (_channel_enabled || _isRunning || !_rmtStopped) { return; } - (void)rmt_del_channel(channel); - channel = nullptr; + rmt_del_channel(channel); + channel = NULL; } void StepperQueue::startQueue_rmt() { @@ -281,7 +154,7 @@ void StepperQueue::startQueue_rmt() { _isRunning ? "Running" : "Stopped"); #endif - if (channel == nullptr) { + if (channel == NULL) { return; } @@ -332,7 +205,7 @@ void StepperQueue::startQueue_rmt() { #endif } void StepperQueue::forceStop_rmt() { - (void)rmt_disable(channel); + rmt_disable(channel); _channel_enabled = false; _isRunning = false; _rmtStopped = true; @@ -340,7 +213,7 @@ void StepperQueue::forceStop_rmt() { // and empty the buffer read_idx = next_write_idx; } -bool StepperQueue::isReadyForCommands_rmt() const { +bool StepperQueue::isReadyForCommands_rmt() { if (_isRunning) { return !_rmtStopped; } diff --git a/src/StepperISR_rp_pico.cpp b/src/StepperISR_rp_pico.cpp new file mode 100644 index 0000000..4ef4bf4 --- /dev/null +++ b/src/StepperISR_rp_pico.cpp @@ -0,0 +1,228 @@ +#include "StepperISR.h" + +#if defined(SUPPORT_RP_PICO) +#include +#include + +#include "pico_pio.h" + +// Here are the global variables to interface with the interrupts +StepperQueue fas_queue[NUM_QUEUES]; +static stepper_pio_program *program; + +bool StepperQueue::init(FastAccelStepperEngine *engine, uint8_t queue_num, + uint8_t step_pin) { + uint8_t channel = queue_num; + _step_pin = step_pin; + _isStarting = false; + bool ok = claim_pio_sm(engine); + if (ok) { + setupSM(); + connect(); + } + return ok; +} + +bool StepperQueue::claim_pio_sm(FastAccelStepperEngine *engine) { + // We have two or three PIO modules. If we need one sm from a pio, + // the whole PIO need to be claimed due to the size of our pio code. + // Let's check first, if there is any PIO claimed. + // If yes, check if we can claim a sm from that PIO. + for (uint8_t i = 0; i < engine->claimed_pios; i++) { + // pio has been claimed, so our program is valid + int claimed_sm = pio_claim_unused_sm(engine->pio[i], false); + if (claimed_sm >= 0) { + // successfully claimed + pio = engine->pio[i]; + sm = claimed_sm; + return true; + } + } + pio_program_t pio_program; + pio_program.instructions = program->code; + pio_program.length = program->pc; + pio_program.origin = 0; + pio_program.pio_version = 0; +#if defined(PICO_RP_2350) + pio_program.used_gpio_ranges = 0; +#endif + uint offset; + bool rc = pio_claim_free_sm_and_add_program_for_gpio_range( + &pio_program, &pio, &sm, &offset, _step_pin, 1, true); + return rc; +} + +void StepperQueue::setupSM() { + pio_sm_config c = pio_get_default_sm_config(); + // Map the state machine's OUT pin group to one pin, namely the `pin` + // parameter to this function. + sm_config_set_jmp_pin(&c, _step_pin); // Step pin read back + if ((dirPin != PIN_UNDEFINED) && ((dirPin & PIN_EXTERNAL_FLAG) == 0)) { + sm_config_set_set_pins(&c, dirPin, 1); // Direction pin via set + } + sm_config_set_out_pins(&c, _step_pin, 1); // Step pin via out + sm_config_set_wrap(&c, program->wrap_target, program->wrap_at); + + // Load our configuration, and jump to the start of the program + uint offset = 0; + pio_sm_init(pio, sm, offset, &c); + // Set the pin direction to output at the PIO + pio_sm_set_consecutive_pindirs(pio, sm, _step_pin, 1, true); + if ((dirPin != PIN_UNDEFINED) && ((dirPin & PIN_EXTERNAL_FLAG) == 0)) { + pio_sm_set_consecutive_pindirs(pio, sm, dirPin, 1, true); + } + pio_sm_set_enabled(pio, sm, true); // sm is running, otherwise loop() stops + pio_sm_clear_fifos(pio, sm); + pio_sm_restart(pio, sm); +} + +void StepperQueue::connect() { + pio_gpio_init(pio, _step_pin); + if ((dirPin != PIN_UNDEFINED) && ((dirPin & PIN_EXTERNAL_FLAG) == 0)) { + pio_gpio_init(pio, dirPin); + } +} + +void StepperQueue::disconnect() { + pio_sm_set_enabled(pio, sm, false); + gpio_init(_step_pin); + if ((dirPin != PIN_UNDEFINED) && ((dirPin & PIN_EXTERNAL_FLAG) == 0)) { + gpio_init(dirPin); + } +} + +bool StepperQueue::isReadyForCommands() { return true; } + +static bool push_command(StepperQueue *q) { + uint8_t rp = q->read_idx; + if (rp == q->next_write_idx) { + // no command in queue + return false; + } + if (pio_sm_is_tx_fifo_full(q->pio, q->sm)) { + // Serial.println("TX FIFO full, cannot push command"); + return false; + } + // Process command + struct queue_entry *e_curr = &q->entry[rp & QUEUE_LEN_MASK]; + uint8_t steps = e_curr->steps; + uint16_t ticks = e_curr->ticks; + bool dirHigh = e_curr->dirPinState == 1; + bool countUp = e_curr->countUp == 1; + //char out[200]; + //sprintf(out, "push_command %d: dirHigh: %d, countUp: %d, steps: %d, ticks: %d", + // rp, dirHigh, countUp, steps, ticks); + //Serial.println(out); + uint32_t entry = stepper_make_fifo_entry(dirHigh, countUp, steps, ticks); + pio_sm_put(q->pio, q->sm, entry); + rp++; + q->read_idx = rp; + return true; +} + +void StepperQueue::startQueue() { + // These commands would clear isr and consequently the sm state's position is + // lost + // pio_sm_set_enabled(pio, sm, true); // sm is running, otherwise loop() + // stops pio_sm_clear_fifos(pio, sm); pio_sm_restart(pio, sm); + _isStarting = true; + while (push_command(this)) { + }; + _isStarting = false; +} +void StepperQueue::forceStop() { + pio_sm_clear_fifos(pio, sm); + pio_sm_restart(pio, sm); + // ensure step is zero + uint32_t entry = stepper_make_fifo_entry(queue_end.dir, 0, 0, 1); // no steps and 1us cycleAA + pio_sm_put(pio, sm, entry); +} +bool StepperQueue::isRunning() { + if (!pio_sm_is_tx_fifo_empty(pio, sm)) { + return true; + } + // Still the sm can process a command + uint8_t pc = pio_sm_get_pc(pio, sm); + // if pc > 0, then sm is not waiting for fifo entry + return (pc != 0); +} +int32_t StepperQueue::getCurrentPosition() { + // Drop old values in fifo + for (uint8_t i = 0; i <= 4; i++) { + pio_sm_get(pio, sm); + } + if (!isRunning()) { + // kick off loop to probe position + uint32_t entry = stepper_make_fifo_entry(queue_end.dir, 0, 0, 1); // no steps and 1us cycleAA + pio_sm_put(pio, sm, entry); + } + // just need to wait a while for next value + for (uint8_t i = 0; i <= 100; i++) { + if (!pio_sm_is_rx_fifo_empty(pio, sm)) { + break; + } + } + uint32_t pos = pio_sm_get(pio, sm); + return (int32_t)pos; +} + +//************************************************************************************************* + +bool StepperQueue::isValidStepPin(uint8_t step_pin) { + // for now we do only support lower 32 gpios + return (step_pin < 32); +} +int8_t StepperQueue::queueNumForStepPin(uint8_t step_pin) { return -1; } + +//************************************************************************************************* +void StepperTask(void *parameter) { + FastAccelStepperEngine *engine = (FastAccelStepperEngine *)parameter; + while (true) { + engine->manageSteppers(); + const TickType_t delay_time = + (engine->_delay_ms + portTICK_PERIOD_MS - 1) / portTICK_PERIOD_MS; + vTaskDelay(delay_time); + } +} +void FastAccelStepperEngine::pushCommands() { + for (uint8_t i = 0; i < MAX_STEPPER; i++) { + FastAccelStepper *s = _stepper[i]; + if (s) { + StepperQueue *q = &fas_queue[s->_queue_num]; + if (q->_isStarting) { + continue; + } + if (q->isRunning()) { + // sm is running, so we can push commands + // without this, any new command in queue would be pushed immediately + while (push_command(q)) { + }; + } + } + } +} +void StepperTaskQueue(void *parameter) { + FastAccelStepperEngine *engine = (FastAccelStepperEngine *)parameter; + while (true) { + engine->pushCommands(); + const TickType_t delay_time = 1; + vTaskDelay(delay_time); + } +} + +void StepperQueue::adjustSpeedToStepperCount(uint8_t steppers) { + max_speed_in_ticks = 80; // This equals 200kHz @ 16MHz +} + +void fas_init_engine(FastAccelStepperEngine *engine) { +#define STACK_SIZE 3000 +#define PRIORITY (configMAX_PRIORITIES - 1) + engine->_delay_ms = DELAY_MS_BASE; + xTaskCreate(StepperTask, "StepperTask", STACK_SIZE, engine, PRIORITY, NULL); + + xTaskCreate(StepperTaskQueue, "StepperTaskQueue", STACK_SIZE, engine, + PRIORITY, NULL); + + program = stepper_make_program(); +} +#endif diff --git a/src/fas_arch/arduino_rp_pico.h b/src/fas_arch/arduino_rp_pico.h new file mode 100644 index 0000000..7b4b1df --- /dev/null +++ b/src/fas_arch/arduino_rp_pico.h @@ -0,0 +1,13 @@ +#ifndef FAS_ARCH_ARDUINO_RP_PICO_H +#define FAS_ARCH_ARDUINO_RP_PICO_H + +// this is an arduino platform, so include the Arduino.h header file +#include + +// For pico using arduino, just use arduino definition +#define fasEnableInterrupts interrupts +#define fasDisableInterrupts noInterrupts + +#include "fas_arch/common_rp_pico.h" + +#endif /* FAS_ARCH_ARDUINO_RP_PICO_H */ diff --git a/src/fas_arch/common.h b/src/fas_arch/common.h index 40f17e3..73f1f8a 100644 --- a/src/fas_arch/common.h +++ b/src/fas_arch/common.h @@ -1,12 +1,9 @@ #ifndef FAS_COMMON_H #define FAS_COMMON_H -#define TICKS_FOR_STOPPED_MOTOR 0xffffffff +#include "fas_arch/result_codes.h" -#define MOVE_OK 0 -#define MOVE_ERR_NO_DIRECTION_PIN -1 -#define MOVE_ERR_SPEED_IS_UNDEFINED -2 -#define MOVE_ERR_ACCELERATION_IS_UNDEFINED -3 +#define TICKS_FOR_STOPPED_MOTOR 0xffffffff // Low level stepper motor command. // @@ -83,6 +80,10 @@ struct queue_end_s { // AVR family #include "fas_arch/arduino_avr.h" +#elif defined(PICO_RP2040) || defined(PICO_RP2350) +// Raspberry Pico and Pico 2 +#include "fas_arch/arduino_rp_pico.h" + #else #error "Unsupported devices" #endif @@ -97,6 +98,10 @@ struct queue_end_s { } #endif +#if defined(SUPPORT_SELECT_DRIVER_TYPE) +enum class FasDriver : uint8_t { MCPWM_PCNT = 0, RMT = 1, DONT_CARE = 255 }; +#endif + // disable inject_fill_interrupt() for all real devices. Only defined in TEST #ifndef TEST #define inject_fill_interrupt(x) diff --git a/src/fas_arch/common_esp32.h b/src/fas_arch/common_esp32.h index 5f5c171..cd5ed04 100644 --- a/src/fas_arch/common_esp32.h +++ b/src/fas_arch/common_esp32.h @@ -12,7 +12,9 @@ #include #include -#if ESP_IDF_VERSION_MAJOR == 5 +#if ESP_IDF_VERSION_MAJOR == 6 +#include "fas_arch/common_esp32_idf6.h" +#elif ESP_IDF_VERSION_MAJOR == 5 #if ESP_IDF_VERSION_MINOR < 3 #error "FastAccelStepper requires esp-idf >= 5.3.0" #endif @@ -45,6 +47,9 @@ // have more than one core #define SUPPORT_CPU_AFFINITY +// have adjustable stepper task rate +#define SUPPORT_TASK_RATE_CHANGE + #define LL_TOGGLE_PIN(dirPin) \ gpio_ll_set_level(&GPIO, (gpio_num_t)dirPin, \ gpio_ll_get_level(&GPIO, (gpio_num_t)dirPin) ^ 1) diff --git a/src/fas_arch/common_esp32_idf4.h b/src/fas_arch/common_esp32_idf4.h index ea9e8b8..52b5d5c 100644 --- a/src/fas_arch/common_esp32_idf4.h +++ b/src/fas_arch/common_esp32_idf4.h @@ -11,7 +11,9 @@ #define SUPPORT_ESP32_MCPWM_PCNT #define SUPPORT_ESP32_RMT #define SUPPORT_ESP32_PULSE_COUNTER 8 +#define SUPPORT_ESP32_RMT_TICK_LOST #define HAVE_ESP32_RMT +#define RMT_SIZE 64 #define QUEUES_MCPWM_PCNT 6 #define QUEUES_RMT 8 #define NEED_RMT_HEADERS @@ -28,6 +30,7 @@ #define SUPPORT_ESP32_PULSE_COUNTER 4 #define HAVE_ESP32S3_PULSE_COUNTER #define HAVE_ESP32_RMT +#define RMT_SIZE 64 #define QUEUES_MCPWM_PCNT 0 #define QUEUES_RMT 4 #define NEED_RMT_HEADERS @@ -44,6 +47,7 @@ #define SUPPORT_ESP32_PULSE_COUNTER 4 #define HAVE_ESP32S3_PULSE_COUNTER #define HAVE_ESP32S3_RMT +#define RMT_SIZE 48 #define QUEUES_MCPWM_PCNT 4 #define QUEUES_RMT 4 @@ -59,6 +63,7 @@ #elif CONFIG_IDF_TARGET_ESP32C3 #define SUPPORT_ESP32_RMT #define HAVE_ESP32C3_RMT +#define RMT_SIZE 48 #define QUEUES_MCPWM_PCNT 0 #define QUEUES_RMT 2 #define NEED_RMT_HEADERS @@ -99,6 +104,10 @@ #define RMT_CHANNEL_T rmt_channel_t #define FAS_RMT_MEM(channel) ((uint32_t *)RMTMEM.chan[channel].data32) + +// PART_SIZE shall be even. +#define PART_SIZE (((RMT_SIZE - 1) / 4) << 1) + #endif #endif /* FAS_ARCH_COMMON_ESP32_IDF4_H */ diff --git a/src/fas_arch/common_esp32_idf5.h b/src/fas_arch/common_esp32_idf5.h index 08e2da8..eeaf678 100644 --- a/src/fas_arch/common_esp32_idf5.h +++ b/src/fas_arch/common_esp32_idf5.h @@ -10,6 +10,7 @@ #if CONFIG_IDF_TARGET_ESP32 // #define SUPPORT_ESP32_MCPWM_PCNT #define SUPPORT_ESP32_RMT +#define SUPPORT_ESP32_RMT_V2 #define SUPPORT_ESP32_PULSE_COUNTER 8 #define HAVE_ESP32_RMT #define RMT_SIZE 64 @@ -29,6 +30,7 @@ //========================================================================== #elif CONFIG_IDF_TARGET_ESP32S2 #define SUPPORT_ESP32_RMT +#define SUPPORT_ESP32_RMT_V2 #define SUPPORT_ESP32_PULSE_COUNTER 4 #define HAVE_ESP32S3_PULSE_COUNTER #define HAVE_ESP32_RMT @@ -46,6 +48,7 @@ #elif CONFIG_IDF_TARGET_ESP32S3 // #define SUPPORT_ESP32_MCPWM_PCNT #define SUPPORT_ESP32_RMT +#define SUPPORT_ESP32_RMT_V2 #define SUPPORT_ESP32_PULSE_COUNTER 8 #define HAVE_ESP32S3_PULSE_COUNTER #define HAVE_ESP32_RMT @@ -65,6 +68,7 @@ //========================================================================== #elif CONFIG_IDF_TARGET_ESP32C3 #define SUPPORT_ESP32_RMT +#define SUPPORT_ESP32_RMT_V2 #define HAVE_ESP32_RMT #define RMT_SIZE 48 #define QUEUES_MCPWM_PCNT 0 @@ -78,6 +82,7 @@ //========================================================================== #elif CONFIG_IDF_TARGET_ESP32C6 #define SUPPORT_ESP32_RMT +#define SUPPORT_ESP32_RMT_V2 #define SUPPORT_ESP32_PULSE_COUNTER 4 #define HAVE_ESP32_RMT #define RMT_SIZE 48 @@ -93,6 +98,7 @@ //========================================================================== #elif CONFIG_IDF_TARGET_ESP32H2 #define SUPPORT_ESP32_RMT +#define SUPPORT_ESP32_RMT_V2 #define SUPPORT_ESP32_PULSE_COUNTER 4 #define HAVE_ESP32_RMT #define RMT_SIZE 48 @@ -101,6 +107,22 @@ #define NEED_RMT_HEADERS #define NEED_PCNT_HEADERS +//========================================================================== +// +// ESP32 derivate - ESP32P4 +// +//========================================================================== +#elif CONFIG_IDF_TARGET_ESP32P4 +#define SUPPORT_ESP32_RMT +#define SUPPORT_ESP32_RMT_V2 +#define SUPPORT_ESP32_PULSE_COUNTER CONFIG_SOC_PCNT_UNITS_PER_GROUP +#define HAVE_ESP32_RMT +#define RMT_SIZE CONFIG_SOC_RMT_MEM_WORDS_PER_CHANNEL +#define QUEUES_MCPWM_PCNT 0 +#define QUEUES_RMT CONFIG_SOC_RMT_TX_CANDIDATES_PER_GROUP +#define NEED_RMT_HEADERS +#define NEED_PCNT_HEADERS + //========================================================================== // // For all unsupported ESP32 derivates @@ -138,7 +160,13 @@ #include #define RMT_CHANNEL_T rmt_channel_handle_t -#define FAS_RMT_MEM(channel) ((uint32_t *)RMTMEM.chan[channel].data32) +// #define FAS_RMT_MEM(channel) ((uint32_t *)RMTMEM.chan[channel].data32) +// PART_SIZE shall be even. +#define PART_SIZE (RMT_SIZE >> 1) +#endif + +#if (ESP_IDF_VERSION_MINOR >= 5) && defined(SUPPORT_ESP32_PULSE_COUNTER) +#undef SUPPORT_ESP32_PULSE_COUNTER #endif // in order to avoid spikes, first set the value and then make an output diff --git a/src/fas_arch/common_esp32_idf6.h b/src/fas_arch/common_esp32_idf6.h new file mode 100644 index 0000000..799b91b --- /dev/null +++ b/src/fas_arch/common_esp32_idf6.h @@ -0,0 +1,174 @@ +#ifndef FAS_ARCH_COMMON_ESP32_IDF6_H +#define FAS_ARCH_COMMON_ESP32_IDF6_H + +//========================================================================== +// +// ESP32 derivate - the first one +// +//========================================================================== + +#if CONFIG_IDF_TARGET_ESP32 +// #define SUPPORT_ESP32_MCPWM_PCNT +#define SUPPORT_ESP32_RMT +#define SUPPORT_ESP32_RMT_V2 +//#define SUPPORT_ESP32_PULSE_COUNTER 8 +#define HAVE_ESP32_RMT +#define RMT_SIZE 64 + +// #define QUEUES_MCPWM_PCNT 6 +#define QUEUES_MCPWM_PCNT 0 +#define QUEUES_RMT 8 + +#define NEED_RMT_HEADERS +// #define NEED_MCPWM_HEADERS +// #define NEED_PCNT_HEADERS + +//========================================================================== +// +// ESP32 derivate - ESP32S2 +// +//========================================================================== +#elif CONFIG_IDF_TARGET_ESP32S2 +#define SUPPORT_ESP32_RMT +#define SUPPORT_ESP32_RMT_V2 +//#define SUPPORT_ESP32_PULSE_COUNTER 4 +//#define HAVE_ESP32S3_PULSE_COUNTER +#define HAVE_ESP32_RMT +#define RMT_SIZE 64 +#define QUEUES_MCPWM_PCNT 0 +#define QUEUES_RMT 4 +#define NEED_RMT_HEADERS +// #define NEED_PCNT_HEADERS + +//========================================================================== +// +// ESP32 derivate - ESP32S3 +// +//========================================================================== +#elif CONFIG_IDF_TARGET_ESP32S3 +// #define SUPPORT_ESP32_MCPWM_PCNT +#define SUPPORT_ESP32_RMT +#define SUPPORT_ESP32_RMT_V2 +//#define SUPPORT_ESP32_PULSE_COUNTER 8 +//#define HAVE_ESP32S3_PULSE_COUNTER +#define HAVE_ESP32_RMT +#define RMT_SIZE 48 + +//#define QUEUES_MCPWM_PCNT 4 +#define QUEUES_MCPWM_PCNT 0 +#define QUEUES_RMT 4 +#define NEED_RMT_HEADERS +// #define NEED_MCPWM_HEADERS +// #define NEED_PCNT_HEADERS + +//========================================================================== +// +// ESP32 derivate - ESP32C3 +// +//========================================================================== +#elif CONFIG_IDF_TARGET_ESP32C3 +#define SUPPORT_ESP32_RMT +#define SUPPORT_ESP32_RMT_V2 +#define HAVE_ESP32_RMT +#define RMT_SIZE 48 +#define QUEUES_MCPWM_PCNT 0 +#define QUEUES_RMT 2 +#define NEED_RMT_HEADERS + +//========================================================================== +// +// ESP32 derivate - ESP32C6 +// +//========================================================================== +#elif CONFIG_IDF_TARGET_ESP32C6 +#define SUPPORT_ESP32_RMT +#define SUPPORT_ESP32_RMT_V2 +// #define SUPPORT_ESP32_PULSE_COUNTER 4 +#define HAVE_ESP32_RMT +#define RMT_SIZE 48 +#define QUEUES_MCPWM_PCNT 0 +#define QUEUES_RMT 2 +#define NEED_RMT_HEADERS +// #define NEED_PCNT_HEADERS + +//========================================================================== +// +// ESP32 derivate - ESP32H2 +// +//========================================================================== +#elif CONFIG_IDF_TARGET_ESP32H2 +#define SUPPORT_ESP32_RMT +#define SUPPORT_ESP32_RMT_V2 +// #define SUPPORT_ESP32_PULSE_COUNTER 4 +#define HAVE_ESP32_RMT +#define RMT_SIZE 48 +#define QUEUES_MCPWM_PCNT 0 +#define QUEUES_RMT 2 +#define NEED_RMT_HEADERS +// #define NEED_PCNT_HEADERS + +//========================================================================== +// +// ESP32 derivate - ESP32P4 +// +//========================================================================== +#elif CONFIG_IDF_TARGET_ESP32P4 +#define SUPPORT_ESP32_RMT +#define SUPPORT_ESP32_RMT_V2 +// #define SUPPORT_ESP32_PULSE_COUNTER CONFIG_SOC_PCNT_UNITS_PER_GROUP +#define HAVE_ESP32_RMT +#define RMT_SIZE CONFIG_SOC_RMT_MEM_WORDS_PER_CHANNEL +#define QUEUES_MCPWM_PCNT 0 +#define QUEUES_RMT CONFIG_SOC_RMT_TX_CANDIDATES_PER_GROUP +#define NEED_RMT_HEADERS +// #define NEED_PCNT_HEADERS + +//========================================================================== +// +// For all unsupported ESP32 derivates +// +//========================================================================== +#else +#error "Unsupported derivate" +#endif + +// #include +#include +#include + +#ifdef NEED_MCPWM_HEADERS +#include +#include +#include +#endif + +#ifdef NEED_PCNT_HEADERS +#include +// #include +// #include +#include +#include +#include +#include +#include +#endif + +#ifdef NEED_RMT_HEADERS +#include +#include + +#define RMT_CHANNEL_T rmt_channel_handle_t +// #define FAS_RMT_MEM(channel) ((uint32_t *)RMTMEM.chan[channel].data32) +// PART_SIZE shall be even. +#define PART_SIZE (RMT_SIZE >> 1) +#endif + +// in order to avoid spikes, first set the value and then make an output +// esp32 idf5 does not like this approach => output first, then value +#define PIN_OUTPUT(pin, value) \ + { \ + pinMode(pin, OUTPUT); \ + digitalWrite(pin, (value)); \ + } + +#endif /* FAS_ARCH_COMMON_ESP32_IDF6_H */ diff --git a/src/fas_arch/common_rp_pico.h b/src/fas_arch/common_rp_pico.h new file mode 100644 index 0000000..de8776c --- /dev/null +++ b/src/fas_arch/common_rp_pico.h @@ -0,0 +1,40 @@ +#ifndef FAS_ARCH_COMMON_RP_PICO_H +#define FAS_ARCH_COMMON_RP_PICO_H + +#include +#include + +#define SUPPORT_RP_PICO +#define SUPPORT_EXTERNAL_DIRECTION_PIN +#define SUPPORT_UNSAFE_ABS_SPEED_LIMIT_SETTING 1 +// #define SUPPORT_QUEUE_ENTRY_START_POS_U16 + +// pico queue definitions +#define NUM_QUEUES 4 * NUM_PIOS +#define MAX_STEPPER (NUM_QUEUES) +#define QUEUE_LEN 32 + +// Pico timing definition. We use FastAccelStepper standard and recalculate to +// pico system clock. +#define TICKS_PER_S 16000000L +#define MIN_CMD_TICKS (TICKS_PER_S / 5000) +#define MIN_DIR_DELAY_US (MIN_CMD_TICKS / (TICKS_PER_S / 1000000)) +#define MAX_DIR_DELAY_US (65535 / (TICKS_PER_S / 1000000)) +#define DELAY_MS_BASE 4 + +// debug led timing +#define DEBUG_LED_HALF_PERIOD 50 + +#define noop_or_wait vTaskDelay(1) + +// have more than one core +// #define SUPPORT_CPU_AFFINITY + +// have adjustable stepper task rate +#define SUPPORT_TASK_RATE_CHANGE + +//#define LL_TOGGLE_PIN(dirPin) \ +// gpio_ll_set_level(&GPIO, (gpio_num_t)dirPin, \ +// gpio_ll_get_level(&GPIO, (gpio_num_t)dirPin) ^ 1) + +#endif /* FAS_ARCH_COMMON_RP_PICO_H */ diff --git a/src/fas_arch/espidf_esp32.h b/src/fas_arch/espidf_esp32.h index 3458086..e34e822 100644 --- a/src/fas_arch/espidf_esp32.h +++ b/src/fas_arch/espidf_esp32.h @@ -14,9 +14,10 @@ #define LOW 0 #define HIGH 1 #define OUTPUT GPIO_MODE_OUTPUT -#define pinMode(pin, mode) \ - gpio_set_direction((gpio_num_t)pin, \ - (mode) == OUTPUT ? GPIO_MODE_OUTPUT : GPIO_MODE_INPUT) +#define pinMode(pin, mode) \ + gpio_set_direction((gpio_num_t)pin, (mode) == OUTPUT \ + ? GPIO_MODE_INPUT_OUTPUT \ + : GPIO_MODE_INPUT) #define digitalWrite(pin, level) gpio_set_level((gpio_num_t)pin, level) #define digitalRead(pin) gpio_get_level((gpio_num_t)pin) diff --git a/src/fas_arch/result_codes.h b/src/fas_arch/result_codes.h new file mode 100644 index 0000000..baa63a4 --- /dev/null +++ b/src/fas_arch/result_codes.h @@ -0,0 +1,171 @@ +#ifndef FAS_RESULT_CODES_H +#define FAS_RESULT_CODES_H + +// ### Result codes for addQueueEntry() function of FastAccelStepper +enum class AqeResultCode : int8_t { + OK = 0, + QueueFull = 1, + DirPinIsBusy = 2, + WaitForEnablePinActive = 3, + DeviceNotReady = 4, + ErrorTicksTooLow = -1, + ErrorEmptyQueueToStart = -2, + ErrorNoDirPinToToggle = -3 +}; + +static inline bool aqeRetry(AqeResultCode code) { + return (static_cast(code)) > 0; +} +static inline bool aqeIsOk(AqeResultCode status) { + return status == AqeResultCode::OK; +} + +static inline const char* toString(AqeResultCode code) { + switch (code) { + case AqeResultCode::OK: + return "OK"; + case AqeResultCode::QueueFull: + return "Queue Full"; + case AqeResultCode::DirPinIsBusy: + return "Direction Pin is Busy"; + case AqeResultCode::WaitForEnablePinActive: + return "Waiting for Enable Pin Active"; + case AqeResultCode::DeviceNotReady: + return "Device Not Ready"; + case AqeResultCode::ErrorTicksTooLow: + return "Error: Ticks Too Low"; + case AqeResultCode::ErrorEmptyQueueToStart: + return "Error: Empty Queue to Start"; + case AqeResultCode::ErrorNoDirPinToToggle: + return "Error: No Direction Pin to Toggle"; + default: + return "Unknown Error"; + } +} +#define AQE_OK AqeResultCode::OK +#define AQE_QUEUE_FULL AqeResultCode::QueueFull +#define AQE_DIR_PIN_IS_BUSY AqeResultCode::DirPinIsBusy +#define AQE_WAIT_FOR_ENABLE_PIN_ACTIVE AqeResultCode::WaitForEnablePinActive +#define AQE_DEVICE_NOT_READY AqeResultCode::DeviceNotReady +#define AQE_ERROR_TICKS_TOO_LOW AqeResultCode::ErrorTicksTooLow +#define AQE_ERROR_EMPTY_QUEUE_TO_START AqeResultCode::ErrorEmptyQueueToStart +#define AQE_ERROR_NO_DIR_PIN_TO_TOGGLE AqeResultCode::ErrorNoDirPinToToggle + +// Define the MoveResultCode enum with equivalent values +enum class MoveResultCode : int8_t { + OK = 0, + ErrorNoDirectionPin = -1, // Equivalent to MOVE_ERR_NO_DIRECTION_PIN + ErrorSpeedIsUndefined = -2, // Equivalent to MOVE_ERR_SPEED_IS_UNDEFINED + ErrorAccelerationIsUndefined = + -3 // Equivalent to MOVE_ERR_ACCELERATION_IS_UNDEFINED +}; + +static inline bool moveIsOk(MoveResultCode status) { + return status == MoveResultCode::OK; +} + +// Function to convert MoveResultCode to string for debugging/errors +static inline const char* toString(MoveResultCode code) { + switch (code) { + case MoveResultCode::OK: + return "OK"; + case MoveResultCode::ErrorNoDirectionPin: + return "Error: No Direction Pin"; + case MoveResultCode::ErrorSpeedIsUndefined: + return "Error: Speed is Undefined"; + case MoveResultCode::ErrorAccelerationIsUndefined: + return "Error: Acceleration is Undefined"; + default: + return "Unknown Error"; + } +} + +// Macros for easier access to enum values +#define MOVE_OK MoveResultCode::OK +#define MOVE_ERR_NO_DIRECTION_PIN MoveResultCode::ErrorNoDirectionPin +#define MOVE_ERR_SPEED_IS_UNDEFINED MoveResultCode::ErrorSpeedIsUndefined +#define MOVE_ERR_ACCELERATION_IS_UNDEFINED \ + MoveResultCode::ErrorAccelerationIsUndefined + +// Define the MoveResultCode enum with equivalent values +enum class MoveTimedResultCode : int8_t { + OK = 0, + QueueFull = 1, + DirPinIsBusy = 2, + WaitForEnablePinActive = 3, + DeviceNotReady = 4, + ErrorTicksTooLow = -1, + ErrorEmptyQueueToStart = -2, + ErrorNoDirPinToToggle = -3, + MoveBusy = 5, + MoveEmpty = 6, + ErrorMoveTooLarge = -4, +}; + +static inline bool moveTimedIsOk(MoveTimedResultCode status) { + return status == MoveTimedResultCode::OK; +} + +static inline MoveTimedResultCode tmrFrom(AqeResultCode res) { + return static_cast(res); +} + +// Function to convert MoveResultCode to string for debugging/errors +static inline const char* toString(MoveTimedResultCode code) { + switch (code) { + case MoveTimedResultCode::OK: + return "OK"; + case MoveTimedResultCode::QueueFull: + return "Queue Full"; + case MoveTimedResultCode::DirPinIsBusy: + return "Direction Pin is Busy"; + case MoveTimedResultCode::WaitForEnablePinActive: + return "Waiting for Enable Pin Active"; + case MoveTimedResultCode::DeviceNotReady: + return "Device Not Ready"; + case MoveTimedResultCode::ErrorTicksTooLow: + return "Error: Ticks Too Low"; + case MoveTimedResultCode::ErrorEmptyQueueToStart: + return "Error: Empty Queue to Start"; + case MoveTimedResultCode::ErrorNoDirPinToToggle: + return "Error: No Direction Pin to Toggle"; + case MoveTimedResultCode::MoveBusy: + return "Move still ongoing"; + case MoveTimedResultCode::MoveEmpty: + return "Queue has been empty"; + case MoveTimedResultCode::ErrorMoveTooLarge: + return "Error: Move too large"; + default: + return "Unknown Error"; + } +} +#define MOVE_TIMED_OK MoveTimedResultCode::OK +#define MOVE_TIMED_BUSY MoveTimedResultCode::MoveBusy +#define MOVE_TIMED_EMPTY MoveTimedResultCode::MoveEmpty +#define MOVE_TIMED_TOO_LARGE_ERROR MoveTimedResultCode::ErrorMoveTooLarge + +enum class DelayResultCode : int8_t { OK = 0, TOO_LOW = -1, TOO_HIGH = -2 }; + +static inline bool delayIsValid(DelayResultCode status) { + return status == DelayResultCode::OK; +} + +static inline const char* toString(DelayResultCode status) { + switch (status) { + case DelayResultCode::OK: + return "OK"; + case DelayResultCode::TOO_LOW: + return "Delay Too Low"; + case DelayResultCode::TOO_HIGH: + return "Delay Too High"; + default: + return "Unknown Delay Status"; + } +} + +// Macros for simpler usage if needed +#define DELAY_OK DelayResultCode::OK +#define DELAY_TOO_LOW DelayResultCode::TOO_LOW +#define DELAY_TOO_HIGH DelayResultCode::TOO_HIGH + +#endif /* FAS_RESULT_CODES_H */ diff --git a/src/fas_arch/test_pc.h b/src/fas_arch/test_pc.h index 99a25d4..2db793a 100644 --- a/src/fas_arch/test_pc.h +++ b/src/fas_arch/test_pc.h @@ -26,6 +26,7 @@ #define fas_queue_A fas_queue[0] #define fas_queue_B fas_queue[1] #define QUEUE_LEN 16 +#define PART_SIZE 31 // timing definitions for pc-based testing #define TICKS_PER_S 16000000L @@ -33,7 +34,7 @@ #define MIN_DIR_DELAY_US (MIN_CMD_TICKS / (TICKS_PER_S / 1000000)) #define MAX_DIR_DELAY_US (65535 / (TICKS_PER_S / 1000000)) #define DELAY_MS_BASE 1 -#define SUPPORT_UNSAFE_ABS_SPEED_LIMIT_SETTING 0 +#define SUPPORT_UNSAFE_ABS_SPEED_LIMIT_SETTING 1 #define noop_or_wait diff --git a/src/fas_arch/test_probe.h b/src/fas_arch/test_probe.h index 733b490..f84c7e7 100644 --- a/src/fas_arch/test_probe.h +++ b/src/fas_arch/test_probe.h @@ -8,10 +8,12 @@ // #define ESP32C3_TEST_PROBE // in rmt: -// TEST_PROBE_1 on startQueue and queue stop, with double toggle at -// startQueue TEST_PROBE_2 end interrupt, when rmt transmission hits buffer -// end TEST_PROBE_3 threshold interrupt, after first buffer half transmission -// is complete +// - TEST_PROBE_1: on startQueue and queue stop, with double toggle at +// startQueue +// - TEST_PROBE_2: end interrupt, when rmt transmission hits buffer end +// - TEST_PROBE_3: threshold interrupt, after first buffer half transmission is +// complete +// - TEST_PROBE_4: on command completed, read pointer advanced #ifdef ESP32_TEST_PROBE #define TEST_PROBE_1 18 @@ -52,5 +54,14 @@ #define PROBE_3(x) #define PROBE_3_TOGGLE #endif +#ifdef TEST_PROBE_4 +#define PROBE_4(x) digitalWrite(TEST_PROBE_4, x) +#define PROBE_4_TOGGLE \ + pinMode(TEST_PROBE_4, OUTPUT); \ + digitalWrite(TEST_PROBE_4, digitalRead(TEST_PROBE_4) == HIGH ? LOW : HIGH) +#else +#define PROBE_4(x) +#define PROBE_4_TOGGLE +#endif #endif diff --git a/src/pico_pio.cpp b/src/pico_pio.cpp new file mode 100644 index 0000000..8bd37a7 --- /dev/null +++ b/src/pico_pio.cpp @@ -0,0 +1,181 @@ +// Analyze the following code for raspberry pi pico to create a pio program. Understand the register changes and cycle count of the instructions. Based on your understanding create a mermaid diagram with states describing the current cycle count and register content. The transitions shall give info about the executed instructions and their respecive instruction cycles. Instructions without jump inbetween can be intelligently combined. Any cycle mismatch of parallel paths shall be highlighted. In the dir parallel path two cycle info may be needed. Important to note,that wrap is not in use. Every instruction is one cycle, unless an additional delay in cycles is given. The code is: +#if defined(PICO_RP2040) || defined(PICO_RP2350) +#include + +#include +#include +#include + +#include "pico_pio.h" + +static stepper_pio_program program; + +static void add_step(uint instruction) { + if (program.pc < 32) { + program.code[program.pc++] = (uint16_t)instruction; + } +} + +// fifo data NEW is +// FEDCBA9876543210fedcba9876543210 = 32bits +// <- period ->| |loopcnt| +// ^-------------Dir pin value +// ^--------------Count up if 1 +// +// For a pause the loop_cnt shall be 1. +// For steps, the loop_cnt = 2*steps +// The LSB of loop_cnt is the inverted output of the step pin. +// In other words, after loop_cnt decrement, the LSB shall equal step pin value. +// +// Timing: 27+3*period cycles per loop iteration +// Pause: 27+3*period cycles +// Steps: (27+3*period)*steps*2 cycles +// +// Combined: (27+3*period)*loop_cnt = cycles per cmd +// +// +uint32_t stepper_calc_period(uint8_t steps, + uint16_t cycles_in_16th_us) { + uint32_t cycles_in_80MHz = cycles_in_16th_us; + // should be yielding multiplication with 5 + cycles_in_80MHz *= program.sys_clk/1000000; + cycles_in_80MHz /= 16000000/1000000; + if (steps > 1) { + cycles_in_80MHz *= steps; + } + uint16_t loop_cnt = steps == 0 ? 1 : 2 * (uint16_t)steps; // pause or steps + cycles_in_80MHz /= loop_cnt; + cycles_in_80MHz -= 27; // 27 cycles for the loop overhead + cycles_in_80MHz /= 3; + return cycles_in_80MHz; +} +uint32_t stepper_make_fifo_entry(bool dir_high, bool count_up, uint8_t steps, + uint16_t cycles_16th_us) { + uint32_t period = stepper_calc_period(steps, cycles_16th_us); + uint16_t loop_cnt = steps == 0 ? 1 : 2 * (uint16_t)steps; // pause or steps + uint32_t entry = + (period << 11) | (count_up ? 1024 : 0) | (dir_high ? 512 : 0) | loop_cnt; + //char out[200]; + //sprintf(out, "stepper_make_fifo_entry: dir_high: %d, count_up: %d, steps: %d, cycles_16th_us: %d, period: %d, loop_cnt: %d, entry: 0x%08X", + // dir_high, count_up, steps, cycles_16th_us, period, loop_cnt, entry); + //Serial.println(out); + return entry; +} + +// clang-format off +stepper_pio_program *stepper_make_program() { + program.sys_clk = clock_get_hz(clk_sys); + program.pc = 0; + // We assume, that isr is cleared on sm start ! + // ISR=position=0, X=invalid, Y=invalid, OSR=invalid + uint8_t label_main_loop = program.pc; + // ISR=position, X=invalid, Y=invalid, OSR=invalid + // Blocking load of next command + add_step(pio_encode_pull(false, true)); + // ISR=position, X=invalid, Y=invalid, OSR=period:up:dir:loop_cnt + // copy osr to x + add_step(pio_encode_mov(pio_x, pio_osr)); + // ISR=position, X=period:up:dir:loop_cnt, Y=invalid, OSR=period:up:dir:loop_cnt + // remove loop_cnt from osr (9 bits) + add_step(pio_encode_out(pio_null, 9)); + // ISR=position, X=period:up:dir:loop_cnt, Y=invalid, OSR=period:up:dir + uint8_t label_step_loop = program.pc; + // Move the dir pin value in y + add_step(pio_encode_out(pio_y, 1)); + // ISR=position, X=period:up:dir:loop_cnt, Y=dir, OSR=period:up + add_step(pio_encode_jmp_not_y(program.pc + 3)); + add_step(pio_encode_set(pio_pins, 1)); // set dir pin to 1 + add_step(pio_encode_jmp(program.pc + 2)); + add_step(pio_encode_set(pio_pins, 0) | pio_encode_delay(1)); // set dir pin to 0 + // Get the UP flag into Y for position update below + add_step(pio_encode_out(pio_y, 1)); + // ISR=position, X=period:up:dir:loop_cnt, Y=up, OSR=[period] + // decrement X in order to reduce loop_cnt by 1 + add_step(pio_encode_jmp_x_dec(program.pc + 1)); + // ISR=position, X=period:up:dir:loop_cnt-1, Y=up, OSR=[period] + // set step output to LSB of loop_cnt defined by sm_config_set_out_pins + add_step(pio_encode_mov(pio_pins, pio_x)); + // restore period:remaining_loop_cnt:dir in osr + add_step(pio_encode_mov(pio_osr, pio_x)); + // ISR=position, X=period:up:dir:loop_cnt-1, Y=up, OSR=period:up:dir:loop_cnt-1 + // If step pin is one, we need to update position + add_step(pio_encode_jmp_pin(program.pc + 2)); + // Step or Pause is completed + uint8_t forward_jump_1 = program.pc; + add_step(pio_encode_jmp(0) | pio_encode_delay(7)); + // + // Perform increment by one with only invert and decrement: + // position 0 1 2 .. d e f ef + // invert f e d .. 2 1 0 10 + // decrement e d c .. 1 0 f 0f + // invert 1 2 3 .. e f 0 f0 + // + // The following code section needs always 7 cycles + // Load from isr the position into x and use inverted position, if dir=0 + add_step(pio_encode_mov(pio_x, pio_isr)); + add_step(pio_encode_jmp_not_y(program.pc + 2)); // jump if y = 0 + add_step(pio_encode_mov_not(pio_x, pio_isr)); + // ISR=position, X=position/-position, Y=up, OSR=period:up:dir:loop_cnt-1 + add_step(pio_encode_jmp_x_dec(program.pc + 1)); // decrement x + // Store updated position in isr and invert, if dir=0 + add_step(pio_encode_mov_not(pio_isr, pio_x)); + add_step(pio_encode_jmp_y_dec(program.pc + 2)); // jump if y != 0 + add_step(pio_encode_mov(pio_isr, pio_x)); + // ISR=position+/-1, X=[position+/-1], Y=[up+0/-1], OSR=period:up:dir:loop_cnt-1 + // restore x from osr + add_step(pio_encode_mov(pio_x, pio_osr)); + // ISR=position+/-1, X=period:up:dir:loop_cnt-1, Y=[up+0/-1], OSR=period:up:dir:loop_cnt-1 + + uint8_t forward_jump_1_target = program.pc; // T=20 + // ISR=position, X=period:up:dir:loop_cnt, Y=[up], OSR=period:up:dir:loop_cnt + // isolate period into osr + add_step(pio_encode_out(pio_null, 9+1+1)); + // ISR=position, X=period:up:dir:loop_cnt, Y=[up], OSR=period + // copy period to y + add_step(pio_encode_mov(pio_y, pio_osr)); + // ISR=position, X=period:up:dir:loop_cnt, Y=period, OSR=period + // store period/remaining_loop_cnt in osr + add_step(pio_encode_mov(pio_osr, pio_x)); + // ISR=position, X=[period:up:dir:loop_cnt], Y=period, OSR=period:up:dir:loop_cnt + // move position into x + add_step(pio_encode_mov(pio_x, pio_isr)); + // ISR=position, X=position, Y=period, OSR=period:up:dir:loop_cnt + uint8_t label_period_loop = program.pc; // T=24 + // output position in fifo + add_step(pio_encode_push(false, false)); + // ISR=0, X=position, Y=period, OSR=period:up:dir:loop_cnt + // restore position in ISR + add_step(pio_encode_mov(pio_isr, pio_x)); + // ISR=position, X=position, Y=period, OSR=period:up:dir:loop_cnt + // loop until period in y is 0 + add_step(pio_encode_jmp_y_dec(label_period_loop)); // T=24+3*N + // ISR=position, X=[position], Y=[0], OSR=period:up:dir:loop_cnt + // store period:up:dir:loop_cnt in x + add_step(pio_encode_mov(pio_x, pio_osr)); + // ISR=position, X=period:up:dir:loop_cnt, Y=0, OSR=period:up:dir:loop_cnt + // get loop_cnt into y (9 bits) + add_step(pio_encode_out(pio_y, 9)); + // ISR=position, X=period:up:dir:loop_cnt, Y=loop_cnt, OSR=period:up:dir + // if loop_cnt is zero go to main loop + add_step(pio_encode_jmp_not_y(label_main_loop)); // T=27+3*N + // Otherwise continue loop using wrap around + // restore x to osr + add_step(pio_encode_jmp(label_step_loop) | pio_encode_delay(2)); + // ISR=position, X=[period:up:dir:loop_cnt], Y=loop_cnt, OSR=period:up:dir:loop_cnt + // wrap around does not need a cycle + + // patch forward jump address + program.code[forward_jump_1] |= forward_jump_1_target; + + // In this code we do not actually use the wrap around feature of PIO. + program.wrap_at = program.pc - 1; + program.wrap_target = label_step_loop; + + // delay(2000); + // Serial.println("stepper_make_program: pc: " + String(program.pc) + + // ", wrap_at: " + String(program.wrap_at) + + // ", wrap_target: " + String(program.wrap_target)); + return &program; +} +// clang-format on +#endif diff --git a/src/pico_pio.h b/src/pico_pio.h new file mode 100644 index 0000000..c0efe39 --- /dev/null +++ b/src/pico_pio.h @@ -0,0 +1,13 @@ +typedef struct pio_program_s { + uint16_t code[32]; // at max 32 instructions + uint8_t pc; + uint8_t wrap_at; + uint8_t wrap_target; + uint32_t sys_clk; +} stepper_pio_program; +stepper_pio_program *stepper_make_program(); + +uint32_t stepper_calc_period(bool dir_high, uint8_t steps, + uint16_t cycles_16th_us); +uint32_t stepper_make_fifo_entry(bool dir_high, bool count_up, uint8_t steps, + uint16_t cycles_16th_us);