From 21993b75f4e32bd687edadedb2306ebfebc0e43d Mon Sep 17 00:00:00 2001
From: Giuliano Zaro <3684609+GMagician@users.noreply.github.com>
Date: Fri, 2 Aug 2019 14:37:41 +0200
Subject: [PATCH] SAMD51 Servo class (#14781)
---
Marlin/src/HAL/HAL_AVR/ServoTimers.h | 4 +-
Marlin/src/HAL/HAL_DUE/ServoTimers.h | 4 +-
Marlin/src/HAL/HAL_SAMD51/HAL.cpp | 58 ++---
Marlin/src/HAL/HAL_SAMD51/HAL.h | 3 +-
.../src/HAL/HAL_SAMD51/HAL_timers_SAMD51.cpp | 4 +-
Marlin/src/HAL/HAL_SAMD51/HAL_timers_SAMD51.h | 9 +-
Marlin/src/HAL/HAL_SAMD51/SAMD51.h | 4 +-
Marlin/src/HAL/HAL_SAMD51/ServoTimers.h | 39 +++
Marlin/src/HAL/HAL_SAMD51/Servo_SAMD51.cpp | 226 ++++++++++++++++++
Marlin/src/HAL/HAL_SAMD51/Tone.cpp | 59 -----
Marlin/src/HAL/HAL_STM32/HAL_timers_STM32.cpp | 2 -
.../STM32F4/HAL_timers_STM32F4.cpp | 2 -
Marlin/src/HAL/shared/servo.cpp | 5 +-
Marlin/src/HAL/shared/servo.h | 4 +-
Marlin/src/HAL/shared/servo_private.h | 12 +-
15 files changed, 318 insertions(+), 117 deletions(-)
create mode 100644 Marlin/src/HAL/HAL_SAMD51/ServoTimers.h
create mode 100644 Marlin/src/HAL/HAL_SAMD51/Servo_SAMD51.cpp
delete mode 100644 Marlin/src/HAL/HAL_SAMD51/Tone.cpp
diff --git a/Marlin/src/HAL/HAL_AVR/ServoTimers.h b/Marlin/src/HAL/HAL_AVR/ServoTimers.h
index 07d3070297..4991caefe6 100644
--- a/Marlin/src/HAL/HAL_AVR/ServoTimers.h
+++ b/Marlin/src/HAL/HAL_AVR/ServoTimers.h
@@ -53,8 +53,8 @@
* --------------------
*/
-#define TRIM_DURATION 2 // compensation ticks to trim adjust for digitalWrite delays
-#define PRESCALER 8 // timer prescaler
+#define TRIM_DURATION 2 // compensation ticks to trim adjust for digitalWrite delays
+#define SERVO_TIMER_PRESCALER 8 // timer prescaler
// Say which 16 bit timers can be used and in what order
#if defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__)
diff --git a/Marlin/src/HAL/HAL_DUE/ServoTimers.h b/Marlin/src/HAL/HAL_DUE/ServoTimers.h
index 84129ef2b6..c32c938253 100644
--- a/Marlin/src/HAL/HAL_DUE/ServoTimers.h
+++ b/Marlin/src/HAL/HAL_DUE/ServoTimers.h
@@ -36,8 +36,8 @@
//!#define _useTimer4
#define _useTimer5
-#define TRIM_DURATION 2 // compensation ticks to trim adjust for digitalWrite delays
-#define PRESCALER 32 // timer prescaler
+#define TRIM_DURATION 2 // compensation ticks to trim adjust for digitalWrite delays
+#define SERVO_TIMER_PRESCALER 32 // timer prescaler
/*
TC0, chan 0 => TC0_Handler
diff --git a/Marlin/src/HAL/HAL_SAMD51/HAL.cpp b/Marlin/src/HAL/HAL_SAMD51/HAL.cpp
index 0e35610a42..e9f378b666 100644
--- a/Marlin/src/HAL/HAL_SAMD51/HAL.cpp
+++ b/Marlin/src/HAL/HAL_SAMD51/HAL.cpp
@@ -193,8 +193,8 @@ uint16_t HAL_adc_result;
uint16_t HAL_adc_results[COUNT(adc_pins)];
#if ADC0_IS_REQUIRED
- Adafruit_ZeroDMA adc0ProgramDMA,
- adc0ReadDMA;
+ Adafruit_ZeroDMA adc0DMAProgram,
+ adc0DMARead;
const HAL_DMA_DAC_Registers adc0_dma_regs_list[] = {
#if GET_TEMP_0_ADC() == 0
@@ -233,8 +233,8 @@ uint16_t HAL_adc_result;
#endif // ADC0_IS_REQUIRED
#if ADC1_IS_REQUIRED
- Adafruit_ZeroDMA adc1ProgramDMA,
- adc1ReadDMA;
+ Adafruit_ZeroDMA adc1DMAProgram,
+ adc1DMARead;
const HAL_DMA_DAC_Registers adc1_dma_regs_list[] = {
#if GET_TEMP_0_ADC() == 1
@@ -284,11 +284,11 @@ uint16_t HAL_adc_result;
DmacDescriptor *descriptor;
#if ADC0_IS_REQUIRED
- adc0ProgramDMA.setTrigger(ADC0_DMAC_ID_SEQ);
- adc0ProgramDMA.setAction(DMA_TRIGGER_ACTON_BEAT);
- adc0ProgramDMA.loop(true);
- if (adc0ProgramDMA.allocate() == DMA_STATUS_OK) {
- descriptor = adc0ProgramDMA.addDescriptor(
+ adc0DMAProgram.setTrigger(ADC0_DMAC_ID_SEQ);
+ adc0DMAProgram.setAction(DMA_TRIGGER_ACTON_BEAT);
+ adc0DMAProgram.loop(true);
+ if (adc0DMAProgram.allocate() == DMA_STATUS_OK) {
+ descriptor = adc0DMAProgram.addDescriptor(
(void *)adc0_dma_regs_list, // SRC
(void *)&ADC0->DSEQDATA.reg, // DEST
sizeof(adc0_dma_regs_list) / 4, // CNT
@@ -300,14 +300,14 @@ uint16_t HAL_adc_result;
);
if (descriptor != nullptr)
descriptor->BTCTRL.bit.EVOSEL = DMA_EVENT_OUTPUT_BEAT;
- adc0ProgramDMA.startJob();
+ adc0DMAProgram.startJob();
}
- adc0ReadDMA.setTrigger(ADC0_DMAC_ID_RESRDY);
- adc0ReadDMA.setAction(DMA_TRIGGER_ACTON_BEAT);
- adc0ReadDMA.loop(true);
- if (adc0ReadDMA.allocate() == DMA_STATUS_OK) {
- adc0ReadDMA.addDescriptor(
+ adc0DMARead.setTrigger(ADC0_DMAC_ID_RESRDY);
+ adc0DMARead.setAction(DMA_TRIGGER_ACTON_BEAT);
+ adc0DMARead.loop(true);
+ if (adc0DMARead.allocate() == DMA_STATUS_OK) {
+ adc0DMARead.addDescriptor(
(void *)&ADC0->RESULT.reg, // SRC
&HAL_adc_results, // DEST
ADC0_AINCOUNT, // CNT
@@ -317,15 +317,15 @@ uint16_t HAL_adc_result;
DMA_ADDRESS_INCREMENT_STEP_SIZE_1, // STEPSIZE
DMA_STEPSEL_DST // STEPSEL
);
- adc0ReadDMA.startJob();
+ adc0DMARead.startJob();
}
#endif
#if ADC1_IS_REQUIRED
- adc1ProgramDMA.setTrigger(ADC1_DMAC_ID_SEQ);
- adc1ProgramDMA.setAction(DMA_TRIGGER_ACTON_BEAT);
- adc1ProgramDMA.loop(true);
- if (adc1ProgramDMA.allocate() == DMA_STATUS_OK) {
- descriptor = adc1ProgramDMA.addDescriptor(
+ adc1DMAProgram.setTrigger(ADC1_DMAC_ID_SEQ);
+ adc1DMAProgram.setAction(DMA_TRIGGER_ACTON_BEAT);
+ adc1DMAProgram.loop(true);
+ if (adc1DMAProgram.allocate() == DMA_STATUS_OK) {
+ descriptor = adc1DMAProgram.addDescriptor(
(void *)adc1_dma_regs_list, // SRC
(void *)&ADC1->DSEQDATA.reg, // DEST
sizeof(adc1_dma_regs_list) / 4, // CNT
@@ -337,14 +337,14 @@ uint16_t HAL_adc_result;
);
if (descriptor != nullptr)
descriptor->BTCTRL.bit.EVOSEL = DMA_EVENT_OUTPUT_BEAT;
- adc1ProgramDMA.startJob();
+ adc1DMAProgram.startJob();
}
- adc1ReadDMA.setTrigger(ADC1_DMAC_ID_RESRDY);
- adc1ReadDMA.setAction(DMA_TRIGGER_ACTON_BEAT);
- adc1ReadDMA.loop(true);
- if (adc1ReadDMA.allocate() == DMA_STATUS_OK) {
- adc1ReadDMA.addDescriptor(
+ adc1DMARead.setTrigger(ADC1_DMAC_ID_RESRDY);
+ adc1DMARead.setAction(DMA_TRIGGER_ACTON_BEAT);
+ adc1DMARead.loop(true);
+ if (adc1DMARead.allocate() == DMA_STATUS_OK) {
+ adc1DMARead.addDescriptor(
(void *)&ADC1->RESULT.reg, // SRC
&HAL_adc_results[ADC0_AINCOUNT], // DEST
ADC1_AINCOUNT, // CNT
@@ -354,11 +354,11 @@ uint16_t HAL_adc_result;
DMA_ADDRESS_INCREMENT_STEP_SIZE_1, // STEPSIZE
DMA_STEPSEL_DST // STEPSEL
);
- adc1ReadDMA.startJob();
+ adc1DMARead.startJob();
}
#endif
- DMAC->PRICTRL0.bit.RRLVLEN0 = true; // Activate round robin for DMA channels used by ADCs
+ DMAC->PRICTRL0.bit.RRLVLEN0 = true; // Activate round robin for DMA channels required by ADCs
}
#endif // DMA_IS_REQUIRED
diff --git a/Marlin/src/HAL/HAL_SAMD51/HAL.h b/Marlin/src/HAL/HAL_SAMD51/HAL.h
index 3dd1313462..7c6d7e37ee 100644
--- a/Marlin/src/HAL/HAL_SAMD51/HAL.h
+++ b/Marlin/src/HAL/HAL_SAMD51/HAL.h
@@ -76,7 +76,8 @@
typedef int8_t pin_t;
-//#define HAL_SERVO_LIB Servo
+#define SHARED_SERVOS HAS_SERVOS
+#define HAL_SERVO_LIB Servo
//
// Interrupts
diff --git a/Marlin/src/HAL/HAL_SAMD51/HAL_timers_SAMD51.cpp b/Marlin/src/HAL/HAL_SAMD51/HAL_timers_SAMD51.cpp
index 70733e2415..8868acc874 100644
--- a/Marlin/src/HAL/HAL_SAMD51/HAL_timers_SAMD51.cpp
+++ b/Marlin/src/HAL/HAL_SAMD51/HAL_timers_SAMD51.cpp
@@ -40,8 +40,8 @@
const tTimerConfig TimerConfig[NUM_HARDWARE_TIMERS] = {
{ TC0, TC0_IRQn, TC_PRIORITY(0) },
{ TC1, TC1_IRQn, TC_PRIORITY(1) },
- { TC2, TC2_IRQn, TC_PRIORITY(2) },
- { TC3, TC3_IRQn, TC_PRIORITY(3) },
+ { TC2, TC2_IRQn, TC_PRIORITY(2) }, // Reserved by framework tone function
+ { TC3, TC3_IRQn, TC_PRIORITY(3) }, // Reserved by servo library
{ TC4, TC4_IRQn, TC_PRIORITY(4) },
{ TC5, TC5_IRQn, TC_PRIORITY(5) },
{ TC6, TC6_IRQn, TC_PRIORITY(6) },
diff --git a/Marlin/src/HAL/HAL_SAMD51/HAL_timers_SAMD51.h b/Marlin/src/HAL/HAL_SAMD51/HAL_timers_SAMD51.h
index 60d2b32962..37cdf7a9bd 100644
--- a/Marlin/src/HAL/HAL_SAMD51/HAL_timers_SAMD51.h
+++ b/Marlin/src/HAL/HAL_SAMD51/HAL_timers_SAMD51.h
@@ -33,7 +33,6 @@ typedef uint32_t hal_timer_t;
#define STEP_TIMER_NUM 0 // index of timer to use for stepper (also +1 for 32bits counter)
#define PULSE_TIMER_NUM STEP_TIMER_NUM
-#define TONE_TIMER_NUM 2 // index of timer to use for beeper tones (also +1 for 32bits counter)
#define TEMP_TIMER_NUM 4 // index of timer to use for temperature (also +1 for 32bits counter)
#define TEMP_TIMER_FREQUENCY 1000 // temperature interrupt frequency
@@ -53,9 +52,10 @@ typedef uint32_t hal_timer_t;
#define ENABLE_TEMPERATURE_INTERRUPT() HAL_timer_enable_interrupt(TEMP_TIMER_NUM)
#define DISABLE_TEMPERATURE_INTERRUPT() HAL_timer_disable_interrupt(TEMP_TIMER_NUM)
-#define TC_PRIORITY(t) (t == STEP_TIMER_NUM || t == PULSE_TIMER_NUM) ? 2 \
- : (t == TEMP_TIMER_NUM) ? 6 \
- : (t == TONE_TIMER_NUM) ? 5 : 7
+#define TC_PRIORITY(t) (t == STEP_TIMER_NUM || t == PULSE_TIMER_NUM) ? 2 \
+ : (t == TEMP_TIMER_NUM) ? 6 \
+ : 7
+
#define _TC_HANDLER(t) void TC##t##_Handler()
#define TC_HANDLER(t) _TC_HANDLER(t)
#define HAL_STEP_TIMER_ISR() TC_HANDLER(STEP_TIMER_NUM)
@@ -63,7 +63,6 @@ typedef uint32_t hal_timer_t;
#define HAL_PULSE_TIMER_ISR() TC_HANDLER(PULSE_TIMER_NUM)
#endif
#define HAL_TEMP_TIMER_ISR() TC_HANDLER(TEMP_TIMER_NUM)
-#define HAL_TONE_TIMER_ISR() TC_HANDLER(TONE_TIMER_NUM)
// --------------------------------------------------------------------------
// Types
diff --git a/Marlin/src/HAL/HAL_SAMD51/SAMD51.h b/Marlin/src/HAL/HAL_SAMD51/SAMD51.h
index 63d53a9600..242423f881 100644
--- a/Marlin/src/HAL/HAL_SAMD51/SAMD51.h
+++ b/Marlin/src/HAL/HAL_SAMD51/SAMD51.h
@@ -20,8 +20,8 @@
*/
#pragma once
-#define SYNC(sc) while (sc) { \
- asm(""); \
+#define SYNC(sc) while (sc) { \
+ asm(""); \
}
// Get SAMD port/pin from specified arduino pin
diff --git a/Marlin/src/HAL/HAL_SAMD51/ServoTimers.h b/Marlin/src/HAL/HAL_SAMD51/ServoTimers.h
new file mode 100644
index 0000000000..b1e18729c6
--- /dev/null
+++ b/Marlin/src/HAL/HAL_SAMD51/ServoTimers.h
@@ -0,0 +1,39 @@
+/**
+ * Marlin 3D Printer Firmware
+ *
+ * Copyright (C) 2019 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ * SAMD51 HAL developed by Giuliano Zaro (AKA GMagician)
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ */
+#pragma once
+
+#define _useTimer1
+#define _useTimer2
+
+#define TRIM_DURATION 5 // compensation ticks to trim adjust for digitalWrite delays
+#define SERVO_TIMER_PRESCALER 64 // timer prescaler factor to 64 (avoid overflowing 16-bit clock counter, at 120MHz this is 1831 ticks per millisecond
+
+#define SERVO_TC 3
+
+typedef enum {
+ #ifdef _useTimer1
+ _timer1,
+ #endif
+ #ifdef _useTimer2
+ _timer2,
+ #endif
+ _Nbr_16timers
+} timer16_Sequence_t;
diff --git a/Marlin/src/HAL/HAL_SAMD51/Servo_SAMD51.cpp b/Marlin/src/HAL/HAL_SAMD51/Servo_SAMD51.cpp
new file mode 100644
index 0000000000..1e3c4f0161
--- /dev/null
+++ b/Marlin/src/HAL/HAL_SAMD51/Servo_SAMD51.cpp
@@ -0,0 +1,226 @@
+/**
+ * Marlin 3D Printer Firmware
+ *
+ * Copyright (C) 2019 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ * SAMD51 HAL developed by Giuliano Zaro (AKA GMagician)
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ */
+
+/**
+ * This comes from Arduino library which at the moment is buggy and uncompilable
+ */
+
+#ifdef __SAMD51__
+
+#include "../../inc/MarlinConfig.h"
+
+#if HAS_SERVOS
+
+#include "../shared/Marduino.h"
+#include "../shared/servo.h"
+#include "../shared/servo_private.h"
+#include "SAMD51.h"
+#include "HAL_timers_SAMD51.h"
+
+#define __TC_GCLK_ID(t) TC##t##_GCLK_ID
+#define _TC_GCLK_ID(t) __TC_GCLK_ID(t)
+#define TC_GCLK_ID _TC_GCLK_ID(SERVO_TC)
+
+#define _TC_PRESCALER(d) TC_CTRLA_PRESCALER_DIV##d##_Val
+#define TC_PRESCALER(d) _TC_PRESCALER(d)
+
+#define __SERVO_IRQn(t) TC##t##_IRQn
+#define _SERVO_IRQn(t) __SERVO_IRQn(t)
+#define SERVO_IRQn _SERVO_IRQn(SERVO_TC)
+
+#define HAL_SERVO_TIMER_ISR() TC_HANDLER(SERVO_TC)
+
+#define TIMER_TCCHANNEL(t) ((t) & 1)
+#define TC_COUNTER_START_VAL 0xFFFF
+
+
+static volatile int8_t currentServoIndex[_Nbr_16timers]; // index for the servo being pulsed for each timer (or -1 if refresh interval)
+
+FORCE_INLINE static uint16_t getTimerCount() {
+ Tc * const tc = TimerConfig[SERVO_TC].pTimer;
+
+ tc->COUNT16.CTRLBSET.reg = TC_CTRLBCLR_CMD_READSYNC;
+ SYNC(tc->COUNT16.SYNCBUSY.bit.CTRLB || tc->COUNT16.SYNCBUSY.bit.COUNT);
+
+ return tc->COUNT16.COUNT.reg;
+}
+
+// ----------------------------
+// Interrupt handler for the TC
+// ----------------------------
+HAL_SERVO_TIMER_ISR() {
+ Tc * const tc = TimerConfig[SERVO_TC].pTimer;
+ const timer16_Sequence_t timer =
+ #ifndef _useTimer1
+ _timer2
+ #elif !defined(_useTimer2)
+ _timer1
+ #else
+ (tc->COUNT16.INTFLAG.reg & tc->COUNT16.INTENSET.reg & TC_INTFLAG_MC0) ? _timer1 : _timer2
+ #endif
+ ;
+ const uint8_t tcChannel = TIMER_TCCHANNEL(timer);
+
+ if (currentServoIndex[timer] < 0) {
+ #if defined(_useTimer1) && defined(_useTimer2)
+ if (currentServoIndex[timer ^ 1] >= 0) {
+ // Wait for both channels
+ // Clear the interrupt
+ tc->COUNT16.INTFLAG.reg = (tcChannel == 0) ? TC_INTFLAG_MC0 : TC_INTFLAG_MC1;
+ return;
+ }
+ #endif
+ tc->COUNT16.COUNT.reg = TC_COUNTER_START_VAL;
+ SYNC(tc->COUNT16.SYNCBUSY.bit.COUNT);
+ }
+ else if (SERVO_INDEX(timer, currentServoIndex[timer]) < ServoCount && SERVO(timer, currentServoIndex[timer]).Pin.isActive)
+ digitalWrite(SERVO(timer, currentServoIndex[timer]).Pin.nbr, LOW); // pulse this channel low if activated
+
+ // Select the next servo controlled by this timer
+ currentServoIndex[timer]++;
+
+ if (SERVO_INDEX(timer, currentServoIndex[timer]) < ServoCount && currentServoIndex[timer] < SERVOS_PER_TIMER) {
+ if (SERVO(timer, currentServoIndex[timer]).Pin.isActive) // check if activated
+ digitalWrite(SERVO(timer, currentServoIndex[timer]).Pin.nbr, HIGH); // it's an active channel so pulse it high
+
+ tc->COUNT16.CC[tcChannel].reg = getTimerCount() - (uint16_t)SERVO(timer, currentServoIndex[timer]).ticks;
+ }
+ else {
+ // finished all channels so wait for the refresh period to expire before starting over
+ currentServoIndex[timer] = -1; // this will get incremented at the end of the refresh period to start again at the first channel
+
+ const uint16_t tcCounterValue = getTimerCount();
+
+ if ((TC_COUNTER_START_VAL - tcCounterValue) + 4UL < usToTicks(REFRESH_INTERVAL)) // allow a few ticks to ensure the next OCR1A not missed
+ tc->COUNT16.CC[tcChannel].reg = TC_COUNTER_START_VAL - (uint16_t)usToTicks(REFRESH_INTERVAL);
+ else
+ tc->COUNT16.CC[tcChannel].reg = (uint16_t)(tcCounterValue - 4UL); // at least REFRESH_INTERVAL has elapsed
+ }
+ if (tcChannel == 0) {
+ SYNC(tc->COUNT16.SYNCBUSY.bit.CC0);
+ // Clear the interrupt
+ tc->COUNT16.INTFLAG.reg = TC_INTFLAG_MC0;
+ }
+ else {
+ SYNC(tc->COUNT16.SYNCBUSY.bit.CC1);
+ // Clear the interrupt
+ tc->COUNT16.INTFLAG.reg = TC_INTFLAG_MC1;
+ }
+}
+
+void initISR(timer16_Sequence_t timer) {
+ Tc * const tc = TimerConfig[SERVO_TC].pTimer;
+ const uint8_t tcChannel = TIMER_TCCHANNEL(timer);
+
+ static bool initialized = false; // Servo TC has been initialized
+ if (!initialized) {
+ NVIC_DisableIRQ(SERVO_IRQn);
+
+ // Disable the timer
+ tc->COUNT16.CTRLA.bit.ENABLE = false;
+ SYNC(tc->COUNT16.SYNCBUSY.bit.ENABLE);
+
+ // Select GCLK0 as timer/counter input clock source
+ GCLK->PCHCTRL[TC_GCLK_ID].bit.CHEN = false;
+ SYNC(GCLK->PCHCTRL[TC_GCLK_ID].bit.CHEN);
+ GCLK->PCHCTRL[TC_GCLK_ID].reg = GCLK_PCHCTRL_GEN_GCLK0 | GCLK_PCHCTRL_CHEN; // 120MHz startup code programmed
+ SYNC(!GCLK->PCHCTRL[TC_GCLK_ID].bit.CHEN);
+
+ // Reset the timer
+ tc->COUNT16.CTRLA.bit.SWRST = true;
+ SYNC(tc->COUNT16.SYNCBUSY.bit.SWRST);
+ SYNC(tc->COUNT16.CTRLA.bit.SWRST);
+
+ // Set timer counter mode to 16 bits
+ tc->COUNT16.CTRLA.reg = TC_CTRLA_MODE_COUNT16;
+
+ // Set timer counter mode as normal PWM
+ tc->COUNT16.WAVE.bit.WAVEGEN = TCC_WAVE_WAVEGEN_NPWM_Val;
+
+ // Set the prescaler factor
+ tc->COUNT16.CTRLA.bit.PRESCALER = TC_PRESCALER(SERVO_TIMER_PRESCALER);
+
+ // Count down
+ tc->COUNT16.CTRLBSET.reg = TC_CTRLBCLR_DIR;
+ SYNC(tc->COUNT16.SYNCBUSY.bit.CTRLB);
+
+ // Reset all servo indexes
+ memset(currentServoIndex, 0xFF, sizeof(currentServoIndex));
+
+ // Configure interrupt request
+ NVIC_ClearPendingIRQ(SERVO_IRQn);
+ NVIC_SetPriority(SERVO_IRQn, 5);
+ NVIC_EnableIRQ(SERVO_IRQn);
+
+ initialized = true;
+ }
+
+ if (!tc->COUNT16.CTRLA.bit.ENABLE) {
+ // Reset the timer counter
+ tc->COUNT16.COUNT.reg = TC_COUNTER_START_VAL;
+ SYNC(tc->COUNT16.SYNCBUSY.bit.COUNT);
+
+ // Enable the timer and start it
+ tc->COUNT16.CTRLA.bit.ENABLE = true;
+ SYNC(tc->COUNT16.SYNCBUSY.bit.ENABLE);
+ }
+ // First interrupt request after 1 ms
+ tc->COUNT16.CC[tcChannel].reg = getTimerCount() - (uint16_t)usToTicks(1000UL);
+
+ if (tcChannel == 0 ) {
+ SYNC(tc->COUNT16.SYNCBUSY.bit.CC0);
+
+ // Clear pending match interrupt
+ tc->COUNT16.INTFLAG.reg = TC_INTENSET_MC0;
+ // Enable the match channel interrupt request
+ tc->COUNT16.INTENSET.reg = TC_INTENSET_MC0;
+ }
+ else {
+ SYNC(tc->COUNT16.SYNCBUSY.bit.CC1);
+
+ // Clear pending match interrupt
+ tc->COUNT16.INTFLAG.reg = TC_INTENSET_MC1;
+ // Enable the match channel interrupt request
+ tc->COUNT16.INTENSET.reg = TC_INTENSET_MC1;
+ }
+}
+
+void finISR(timer16_Sequence_t timer) {
+ Tc * const tc = TimerConfig[SERVO_TC].pTimer;
+ const uint8_t tcChannel = TIMER_TCCHANNEL(timer);
+
+ // Disable the match channel interrupt request
+ tc->COUNT16.INTENCLR.reg = (tcChannel == 0) ? TC_INTENCLR_MC0 : TC_INTENCLR_MC1;
+
+ if (true
+ #if defined(_useTimer1) && defined(_useTimer2)
+ && (tc->COUNT16.INTENCLR.reg & (TC_INTENCLR_MC0|TC_INTENCLR_MC1)) == 0
+ #endif
+ ) {
+ // Disable the timer if not used
+ tc->COUNT16.CTRLA.bit.ENABLE = false;
+ SYNC(tc->COUNT16.SYNCBUSY.bit.ENABLE);
+ }
+}
+
+#endif // HAS_SERVOS
+
+#endif // __SAMD51__
diff --git a/Marlin/src/HAL/HAL_SAMD51/Tone.cpp b/Marlin/src/HAL/HAL_SAMD51/Tone.cpp
deleted file mode 100644
index 58b9698251..0000000000
--- a/Marlin/src/HAL/HAL_SAMD51/Tone.cpp
+++ /dev/null
@@ -1,59 +0,0 @@
-/**
- * Marlin 3D Printer Firmware
- *
- * Copyright (C) 2019 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
- * SAMD51 HAL developed by Giuliano Zaro (AKA GMagician)
- *
- * This program is free software: you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation, either version 3 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- *
- */
-
-/**
- * Description: Tone function for Arduino Due and compatible (SAM3X8E)
- * Derived from http://forum.arduino.cc/index.php?topic=136500.msg2903012#msg2903012
- */
-
-#ifdef __SAMD51__
-
-#include "../../inc/MarlinConfig.h"
-#include "HAL_timers_SAMD51.h"
-
-static pin_t tone_pin;
-volatile static int32_t toggles;
-
-void tone(const pin_t _pin, const unsigned int frequency, const unsigned long duration) {
- tone_pin = _pin;
- toggles = 2 * frequency * duration / 1000;
- HAL_timer_start(TONE_TIMER_NUM, 2 * frequency);
-}
-
-void noTone(const pin_t _pin) {
- HAL_timer_disable_interrupt(TONE_TIMER_NUM);
- extDigitalWrite(_pin, LOW);
-}
-
-HAL_TONE_TIMER_ISR() {
- static bool pin_state = false;
- HAL_timer_isr_prologue(TONE_TIMER_NUM);
-
- if (toggles) {
- toggles--;
- extDigitalWrite(tone_pin, (pin_state = !pin_state));
- }
- else noTone(tone_pin); // turn off interrupt
-
- HAL_timer_isr_epilogue(TONE_TIMER_NUM);
-}
-
-#endif // __SAMD51__
diff --git a/Marlin/src/HAL/HAL_STM32/HAL_timers_STM32.cpp b/Marlin/src/HAL/HAL_STM32/HAL_timers_STM32.cpp
index 7a2de123d2..beefdb3083 100644
--- a/Marlin/src/HAL/HAL_STM32/HAL_timers_STM32.cpp
+++ b/Marlin/src/HAL/HAL_STM32/HAL_timers_STM32.cpp
@@ -32,8 +32,6 @@
#define NUM_HARDWARE_TIMERS 2
-//#define PRESCALER 1
-
// ------------------------
// Private Variables
// ------------------------
diff --git a/Marlin/src/HAL/HAL_STM32_F4_F7/STM32F4/HAL_timers_STM32F4.cpp b/Marlin/src/HAL/HAL_STM32_F4_F7/STM32F4/HAL_timers_STM32F4.cpp
index 85ab5fb896..45ea7fc984 100644
--- a/Marlin/src/HAL/HAL_STM32_F4_F7/STM32F4/HAL_timers_STM32F4.cpp
+++ b/Marlin/src/HAL/HAL_STM32_F4_F7/STM32F4/HAL_timers_STM32F4.cpp
@@ -33,8 +33,6 @@
#define STEP_TIMER_IRQ_ID TIM5_IRQn
#define TEMP_TIMER_IRQ_ID TIM7_IRQn
-//#define PRESCALER 1
-
// ------------------------
// Private Variables
// ------------------------
diff --git a/Marlin/src/HAL/shared/servo.cpp b/Marlin/src/HAL/shared/servo.cpp
index af33853665..22cf6bc88f 100644
--- a/Marlin/src/HAL/shared/servo.cpp
+++ b/Marlin/src/HAL/shared/servo.cpp
@@ -116,9 +116,8 @@ void Servo::detach() {
}
void Servo::write(int value) {
- if (value < MIN_PULSE_WIDTH) { // treat values less than 544 as angles in degrees (valid values in microseconds are handled as microseconds)
+ if (value < MIN_PULSE_WIDTH) // treat values less than 544 as angles in degrees (valid values in microseconds are handled as microseconds)
value = map(constrain(value, 0, 180), 0, 180, SERVO_MIN(), SERVO_MAX());
- }
this->writeMicroseconds(value);
}
@@ -140,7 +139,7 @@ void Servo::writeMicroseconds(int value) {
int Servo::read() { return map(this->readMicroseconds() + 1, SERVO_MIN(), SERVO_MAX(), 0, 180); }
int Servo::readMicroseconds() {
- return (this->servoIndex == INVALID_SERVO) ? 0 : ticksToUs(servo_info[this->servoIndex].ticks) + TRIM_DURATION;
+ return (this->servoIndex == INVALID_SERVO) ? 0 : ticksToUs(servo_info[this->servoIndex].ticks) + (TRIM_DURATION);
}
bool Servo::attached() { return servo_info[this->servoIndex].Pin.isActive; }
diff --git a/Marlin/src/HAL/shared/servo.h b/Marlin/src/HAL/shared/servo.h
index 96644ee262..d1a83bf169 100644
--- a/Marlin/src/HAL/shared/servo.h
+++ b/Marlin/src/HAL/shared/servo.h
@@ -84,10 +84,10 @@
#else
#include <stdint.h>
- #if defined(__AVR__) || defined(ARDUINO_ARCH_SAM)
+ #if defined(__AVR__) || defined(ARDUINO_ARCH_SAM) || defined (__SAMD51__)
// we're good to go
#else
- #error "This library only supports boards with an AVR or SAM3X processor."
+ #error "This library only supports boards with an AVR, SAM3X or SAMD51 processor."
#endif
#define Servo_VERSION 2 // software version of this library
diff --git a/Marlin/src/HAL/shared/servo_private.h b/Marlin/src/HAL/shared/servo_private.h
index 12d4740525..1d4cdc7e3e 100644
--- a/Marlin/src/HAL/shared/servo_private.h
+++ b/Marlin/src/HAL/shared/servo_private.h
@@ -47,8 +47,10 @@
#include "../HAL_AVR/ServoTimers.h"
#elif defined(ARDUINO_ARCH_SAM)
#include "../HAL_DUE/ServoTimers.h"
+#elif defined(__SAMD51__)
+ #include "../HAL_SAMD51/ServoTimers.h"
#else
- #error "This library only supports boards with an AVR or SAM3X processor."
+ #error "This library only supports boards with an AVR, SAM3X or SAMD51 processor."
#endif
// Macros
@@ -64,10 +66,8 @@
#define INVALID_SERVO 255 // flag indicating an invalid servo index
// Convert microseconds to ticks and back (PRESCALER depends on architecture)
-#define usToTicks(_us) (clockCyclesPerMicrosecond() * (_us) / (PRESCALER))
-#define ticksToUs(_ticks) (unsigned(_ticks) * (PRESCALER) / clockCyclesPerMicrosecond())
-
-//#define NBR_TIMERS ((MAX_SERVOS) / (SERVOS_PER_TIMER))
+#define usToTicks(_us) (clockCyclesPerMicrosecond() * (_us) / (SERVO_TIMER_PRESCALER))
+#define ticksToUs(_ticks) (unsigned(_ticks) * (SERVO_TIMER_PRESCALER) / clockCyclesPerMicrosecond())
// convenience macros
#define SERVO_INDEX_TO_TIMER(_servo_nbr) ((timer16_Sequence_t)(_servo_nbr / (SERVOS_PER_TIMER))) // returns the timer controlling this servo
@@ -78,7 +78,7 @@
// Types
typedef struct {
- uint8_t nbr : 6 ; // a pin number from 0 to 63
+ uint8_t nbr : 7 ; // a pin number from 0 to 127
uint8_t isActive : 1 ; // true if this channel is enabled, pin not pulsed if false
} ServoPin_t;