diff --git a/src/drivers/hardware_specific/generic_mcu.cpp b/src/drivers/hardware_specific/generic_mcu.cpp index 73aa3c9f..170b9dae 100644 --- a/src/drivers/hardware_specific/generic_mcu.cpp +++ b/src/drivers/hardware_specific/generic_mcu.cpp @@ -14,6 +14,8 @@ #elif defined(_SAMD21_) // samd21 for the moment, samd51 in progress... +#elif defined(_SAMD51_) // samd21 for the moment, samd51 in progress... + #else // function setting the high pwm frequency to the supplied pins diff --git a/src/drivers/hardware_specific/samd21_mcu.cpp b/src/drivers/hardware_specific/samd21_mcu.cpp index cdbd82f8..0c56eef4 100644 --- a/src/drivers/hardware_specific/samd21_mcu.cpp +++ b/src/drivers/hardware_specific/samd21_mcu.cpp @@ -1,69 +1,142 @@ -#if defined(ARDUINO_ARCH_SAMD) -//#if defined(_SAMD21_) +#include "./samd_mcu.h" -#include "../hardware_api.h" -#include "wiring_private.h" -#include "./samd21_wo_associations.h" +#ifdef _SAMD21_ -#define SIMPLEFOC_SAMD_DEBUG - - -#ifndef SIMPLEFOC_SAMD_ALLOW_DIFFERENT_TCCS -#define SIMPLEFOC_SAMD_ALLOW_DIFFERENT_TCCS false +#ifndef TCC3_CH0 +#define TCC3_CH0 NOT_ON_TIMER #endif - -#ifndef SIMPLEFOC_SAMD_PWM_RESOLUTION -#define SIMPLEFOC_SAMD_PWM_RESOLUTION 1000 -#define SIMPLEFOC_SAMD_PWM_TC_RESOLUTION 250 +#ifndef TCC3_CH1 +#define TCC3_CH1 NOT_ON_TIMER #endif - -#ifndef SIMPLEFOC_SAMD_MAX_TCC_PINCONFIGURATIONS -#define SIMPLEFOC_SAMD_MAX_TCC_PINCONFIGURATIONS 12 +#ifndef TCC3_CH2 +#define TCC3_CH2 NOT_ON_TIMER +#endif +#ifndef TCC3_CH3 +#define TCC3_CH3 NOT_ON_TIMER +#endif +#ifndef TCC3_CH4 +#define TCC3_CH4 NOT_ON_TIMER +#endif +#ifndef TCC3_CH5 +#define TCC3_CH5 NOT_ON_TIMER +#endif +#ifndef TCC3_CH6 +#define TCC3_CH6 NOT_ON_TIMER +#endif +#ifndef TCC3_CH7 +#define TCC3_CH7 NOT_ON_TIMER +#endif +#ifndef TC6_CH0 +#define TC6_CH0 NOT_ON_TIMER +#endif +#ifndef TC6_CH1 +#define TC6_CH1 NOT_ON_TIMER +#endif +#ifndef TC7_CH0 +#define TC7_CH0 NOT_ON_TIMER +#endif +#ifndef TC7_CH1 +#define TC7_CH1 NOT_ON_TIMER #endif -// Wait for synchronization of registers between the clock domains -static __inline__ void syncTCC(Tcc* TCCx) __attribute__((always_inline, unused)); -static void syncTCC(Tcc* TCCx) { - while (TCCx->SYNCBUSY.reg & TCC_SYNCBUSY_MASK); -} +#define NUM_WO_ASSOCIATIONS 48 + +/* + * For SAM D21 A/B/C/D Variant Devices and SAM DA1 A/B Variant Devices + * Good for SAMD2xE, SAMD2xG and SAMD2xJ devices. Other SAMD21s currently not supported in arduino anyway? + * + * Note: only the pins which have timers associated are listed in this table. + * You can use the values from g_APinDescription.ulPort and g_APinDescription.ulPin to find the correct row in the table. + * + * See Microchip Technology datasheet DS40001882F-page 30 + */ +struct wo_association WO_associations[] = { + + { PORTA, 0, TCC2_CH0, 0, NOT_ON_TIMER, 0}, + { PORTA, 1, TCC2_CH1, 1, NOT_ON_TIMER, 0}, + { PORTA, 2, NOT_ON_TIMER, 0, TCC3_CH0, 0}, + { PORTA, 3, NOT_ON_TIMER, 0, TCC3_CH1, 1}, + // PB04, PB05, PB06, PB07 - no timers + { PORTB, 8, TC4_CH0, 0, TCC3_CH6, 6}, + { PORTB, 9, TC4_CH1, 1, TCC3_CH7, 7}, + { PORTA, 4, TCC0_CH0, 0, TCC3_CH2, 2}, + { PORTA, 5, TCC0_CH1, 1, TCC3_CH3, 3}, + { PORTA, 6, TCC1_CH0, 0, TCC3_CH4, 4}, + { PORTA, 7, TCC1_CH1, 1, TCC3_CH5, 5}, + { PORTA, 8, TCC0_CH0, 0, TCC1_CH2, 2}, + { PORTA, 9, TCC0_CH1, 1, TCC1_CH3, 3}, + { PORTA, 10, TCC1_CH0, 0, TCC0_CH2, 2}, + { PORTA, 11, TCC1_CH1, 1, TCC0_CH3, 3}, + { PORTB, 10, TC5_CH0, 0, TCC0_CH4, 4}, + { PORTB, 11, TC5_CH1, 1, TCC0_CH5, 5}, + { PORTB, 12, TC4_CH0, 0, TCC0_CH6, 6}, + { PORTB, 13, TC4_CH1, 1, TCC0_CH7, 7}, + { PORTB, 14, TC5_CH0, 0, NOT_ON_TIMER, 0}, + { PORTB, 15, TC5_CH1, 1, NOT_ON_TIMER, 0}, + { PORTA, 12, TCC2_CH0, 0, TCC0_CH6, 6}, + { PORTA, 13, TCC2_CH1, 1, TCC0_CH7, 7}, + { PORTA, 14, TC3_CH0, 0, TCC0_CH4, 4}, + { PORTA, 15, TC3_CH1, 1, TCC0_CH5, 5}, + { PORTA, 16, TCC2_CH0, 0, TCC0_CH6, 6}, + { PORTA, 17, TCC2_CH1, 1, TCC0_CH7, 7}, + { PORTA, 18, TC3_CH0, 0, TCC0_CH2, 2}, + { PORTA, 19, TC3_CH1, 1, TCC0_CH3, 3}, + { PORTB, 16, TC6_CH0, 0, TCC0_CH4, 4}, + { PORTB, 17, TC6_CH1, 1, TCC0_CH5, 5}, + { PORTA, 20, TC7_CH0, 0, TCC0_CH6, 6}, + { PORTA, 21, TC7_CH1, 1, TCC0_CH7, 7}, + { PORTA, 22, TC4_CH0, 0, TCC0_CH4, 4}, + { PORTA, 23, TC4_CH1, 1, TCC0_CH5, 5}, + { PORTA, 24, TC5_CH0, 0, TCC1_CH2, 2}, + { PORTA, 25, TC5_CH1, 1, TCC1_CH3, 3}, + { PORTB, 22, TC7_CH0, 0, TCC3_CH0, 0}, + { PORTB, 23, TC7_CH1, 1, TCC3_CH1, 1}, + { PORTA, 27, NOT_ON_TIMER, 0, TCC3_CH6, 6}, + { PORTA, 28, NOT_ON_TIMER, 0, TCC3_CH7, 7}, + { PORTA, 30, TCC1_CH0, 0, TCC3_CH4, 4}, + { PORTA, 31, TCC1_CH1, 1, TCC3_CH5, 5}, + { PORTB, 30, TCC0_CH0, 0, TCC1_CH2, 2}, + { PORTB, 31, TCC0_CH1, 1, TCC1_CH3, 3}, + { PORTB, 0, TC7_CH0, 0, NOT_ON_TIMER, 0}, + { PORTB, 1, TC7_CH1, 1, NOT_ON_TIMER, 0}, + { PORTB, 2, TC6_CH0, 0, TCC3_CH2, 2}, + { PORTB, 3, TC6_CH1, 1, TCC3_CH3, 3} +}; +wo_association ASSOCIATION_NOT_FOUND = { NOT_A_PORT, 0, NOT_ON_TIMER, 0, NOT_ON_TIMER, 0}; -struct tccConfiguration { - uint8_t pin; - uint8_t alternate; // 1=true, 0=false - uint8_t wo; - union tccChanInfo { - struct { - int8_t chan; - int8_t tccn; - }; - uint16_t chaninfo; - } tcc; +struct wo_association& getWOAssociation(EPortType port, uint32_t pin) { + for (int i=0;i>pin_position)&0x01)==0x1?PIO_TIMER_ALT:PIO_TIMER; +} -/** - * Global state - */ -tccConfiguration tccPinConfigurations[SIMPLEFOC_SAMD_MAX_TCC_PINCONFIGURATIONS]; -uint8_t numTccPinConfigurations = 0; -bool SAMDClockConfigured = false; -bool tccConfigured[TCC_INST_NUM+TC_INST_NUM]; + +void syncTCC(Tcc* TCCx) { + while (TCCx->SYNCBUSY.reg & TCC_SYNCBUSY_MASK); // Wait for synchronization of registers between the clock domains +} + + /** * Configure Clock 4 - we want all simplefoc PWMs to use the same clock. This ensures that @@ -71,6 +144,10 @@ bool tccConfigured[TCC_INST_NUM+TC_INST_NUM]; * clocks. */ void configureSAMDClock() { + + // TODO investigate using the FDPLL96M clock to get 96MHz timer clocks... this + // would enable 48KHz PWM clocks, and setting the frequency between 24Khz with resolution 2000, to 48KHz with resolution 1000 + if (!SAMDClockConfigured) { SAMDClockConfigured = true; // mark clock as configured for (int i=0;i>1) { - case 0: - GCLK_CLKCTRL_ID_ofthistcc = GCLK_CLKCTRL_ID(GCM_TCC0_TCC1);//GCLK_CLKCTRL_ID_TCC0_TCC1; - break; - case 1: - GCLK_CLKCTRL_ID_ofthistcc = GCLK_CLKCTRL_ID(GCM_TCC2_TC3);//GCLK_CLKCTRL_ID_TCC2_TC3; - break; - case 2: - GCLK_CLKCTRL_ID_ofthistcc = GCLK_CLKCTRL_ID(GCM_TC4_TC5);//GCLK_CLKCTRL_ID_TC4_TC5; - break; - case 3: - GCLK_CLKCTRL_ID_ofthistcc = GCLK_CLKCTRL_ID(GCM_TC6_TC7); - break; - default: - return; + case 0: GCLK_CLKCTRL_ID_ofthistcc = GCLK_CLKCTRL_ID(GCM_TCC0_TCC1); break; //GCLK_CLKCTRL_ID_TCC0_TCC1; + case 1: GCLK_CLKCTRL_ID_ofthistcc = GCLK_CLKCTRL_ID(GCM_TCC2_TC3); break; //GCLK_CLKCTRL_ID_TCC2_TC3; + case 2: GCLK_CLKCTRL_ID_ofthistcc = GCLK_CLKCTRL_ID(GCM_TC4_TC5); break; //GCLK_CLKCTRL_ID_TC4_TC5; + case 3: GCLK_CLKCTRL_ID_ofthistcc = GCLK_CLKCTRL_ID(GCM_TC6_TC7); break; + default: return; } // Feed GCLK4 to TCC REG_GCLK_CLKCTRL = (uint16_t) GCLK_CLKCTRL_CLKEN | // Enable GCLK4 GCLK_CLKCTRL_GEN_GCLK4 | // Select GCLK4 GCLK_CLKCTRL_ID_ofthistcc; // Feed GCLK4 to tcc - while (GCLK->STATUS.bit.SYNCBUSY); // Wait for synchronization + while (GCLK->STATUS.bit.SYNCBUSY); // Wait for synchronization tccConfigured[tccConfig.tcc.tccn] = true; if (tccConfig.tcc.tccn>=TCC_INST_NUM) { Tc* tc = (Tc*)GetTC(tccConfig.tcc.chaninfo); - // disable tc->COUNT8.CTRLA.bit.ENABLE = 0; while ( tc->COUNT8.STATUS.bit.SYNCBUSY == 1 ); @@ -149,7 +216,6 @@ void configureTCC(tccConfiguration& tccConfig, long pwm_frequency, bool negate=f // enable tc->COUNT8.CTRLA.bit.ENABLE = 1; while ( tc->COUNT8.STATUS.bit.SYNCBUSY == 1 ); - #ifdef SIMPLEFOC_SAMD_DEBUG Serial.print("Initialized TC "); Serial.println(tccConfig.tcc.tccn); @@ -184,10 +250,6 @@ void configureTCC(tccConfiguration& tccConfig, long pwm_frequency, bool negate=f while ( (tcc->SYNCBUSY.reg & chanbit) > 0 ); } - // enable double buffering - //tcc->CTRLBCLR.bit.LUPD = 1; - //while ( tcc->SYNCBUSY.bit.CTRLB == 1 ); - // Enable TC tcc->CTRLA.reg |= TCC_CTRLA_ENABLE | TCC_CTRLA_PRESCALER_DIV1; //48Mhz/1=48Mhz/2(up/down)=24MHz/1024=24KHz while ( tcc->SYNCBUSY.bit.ENABLE == 1 ); // wait for sync @@ -243,254 +305,26 @@ void configureTCC(tccConfiguration& tccConfig, long pwm_frequency, bool negate=f -/** - * Attach the TCC to the pin - */ -bool attachTCC(tccConfiguration& tccConfig) { - if (numTccPinConfigurations>=SIMPLEFOC_SAMD_MAX_TCC_PINCONFIGURATIONS) - return false; - pinMode(tccConfig.pin, OUTPUT); - pinPeripheral(tccConfig.pin, (tccConfig.alternate==1)?EPioType::PIO_TIMER_ALT:EPioType::PIO_TIMER); - tccPinConfigurations[numTccPinConfigurations++] = tccConfig; - return true; -} - - - - - - -/** - * Check if the configuration is in use already. - */ -bool inUse(tccConfiguration& tccConfig) { - for (int i=0;i=TCC_INST_NUM) - return false; - - if (pinAh.tcc.chan==pinBh.tcc.chan || pinAh.tcc.chan==pinBl.tcc.chan || pinAh.tcc.chan==pinCh.tcc.chan || pinAh.tcc.chan==pinCl.tcc.chan) - return false; - if (pinBh.tcc.chan==pinCh.tcc.chan || pinBh.tcc.chan==pinCl.tcc.chan) - return false; - if (pinAl.tcc.chan==pinBh.tcc.chan || pinAl.tcc.chan==pinBl.tcc.chan || pinAl.tcc.chan==pinCh.tcc.chan || pinAl.tcc.chan==pinCl.tcc.chan) - return false; - if (pinBl.tcc.chan==pinCh.tcc.chan || pinBl.tcc.chan==pinCl.tcc.chan) - return false; - - if (pinAh.tcc.chan!=pinAl.tcc.chan || pinBh.tcc.chan!=pinBl.tcc.chan || pinCh.tcc.chan!=pinCl.tcc.chan) - return false; - if (pinAh.wo==pinAl.wo || pinBh.wo==pinBl.wo || pinCh.wo!=pinCl.wo) - return false; - - return true; -} - - - - -bool checkPeripheralPermutationCompatible(tccConfiguration pins[], uint8_t num) { - for (int i=0;i=TCC_INST_NUM || pinAl.tcc.tccn>=TCC_INST_NUM || pinBh.tcc.tccn>=TCC_INST_NUM - || pinBl.tcc.tccn>=TCC_INST_NUM || pinCh.tcc.tccn>=TCC_INST_NUM || pinCl.tcc.tccn>=TCC_INST_NUM) - return false; - - // check we're not in use - if (inUse(pinAh) || inUse(pinAl) || inUse(pinBh) || inUse(pinBl) || inUse(pinCh) || inUse(pinCl)) - return false; - - // check pins are all different tccs/channels - if (pinAh.tcc.chaninfo==pinBh.tcc.chaninfo || pinAh.tcc.chaninfo==pinBl.tcc.chaninfo || pinAh.tcc.chaninfo==pinCh.tcc.chaninfo || pinAh.tcc.chaninfo==pinCl.tcc.chaninfo) - return false; - if (pinAl.tcc.chaninfo==pinBh.tcc.chaninfo || pinAl.tcc.chaninfo==pinBl.tcc.chaninfo || pinAl.tcc.chaninfo==pinCh.tcc.chaninfo || pinAl.tcc.chaninfo==pinCl.tcc.chaninfo) - return false; - if (pinBh.tcc.chaninfo==pinCh.tcc.chaninfo || pinBh.tcc.chaninfo==pinCl.tcc.chaninfo) - return false; - if (pinBl.tcc.chaninfo==pinCh.tcc.chaninfo || pinBl.tcc.chaninfo==pinCl.tcc.chaninfo) - return false; - - // check H/L pins are on same timer - if (pinAh.tcc.tccn!=pinAl.tcc.tccn || pinBh.tcc.tccn!=pinBl.tcc.tccn || pinCh.tcc.tccn!=pinCl.tcc.tccn) - return false; - - // check H/L pins aren't on both the same timer and wo - if (pinAh.wo==pinAl.wo || pinBh.wo==pinBl.wo || pinCh.wo==pinCl.wo) - return false; - - return true; -} - - - - - -int checkHardware6PWM(const int pinA_h, const int pinA_l, const int pinB_h, const int pinB_l, const int pinC_h, const int pinC_l) { - for (int i=0;i<64;i++) { - tccConfiguration pinAh = getTCCChannelNr(pinA_h, (i>>0&0x01)==0x1); - tccConfiguration pinAl = getTCCChannelNr(pinA_l, (i>>1&0x01)==0x1); - tccConfiguration pinBh = getTCCChannelNr(pinB_h, (i>>2&0x01)==0x1); - tccConfiguration pinBl = getTCCChannelNr(pinB_l, (i>>3&0x01)==0x1); - tccConfiguration pinCh = getTCCChannelNr(pinC_h, (i>>4&0x01)==0x1); - tccConfiguration pinCl = getTCCChannelNr(pinC_l, (i>>5&0x01)==0x1); - if (checkPeripheralPermutationSameTCC6(pinAh, pinAl, pinBh, pinBl, pinCh, pinCl)) - return i; - } - return -1; -} - - - - -int checkSoftware6PWM(const int pinA_h, const int pinA_l, const int pinB_h, const int pinB_l, const int pinC_h, const int pinC_l) { - for (int i=0;i<64;i++) { - tccConfiguration pinAh = getTCCChannelNr(pinA_h, (i>>0&0x01)==0x1); - tccConfiguration pinAl = getTCCChannelNr(pinA_l, (i>>1&0x01)==0x1); - tccConfiguration pinBh = getTCCChannelNr(pinB_h, (i>>2&0x01)==0x1); - tccConfiguration pinBl = getTCCChannelNr(pinB_l, (i>>3&0x01)==0x1); - tccConfiguration pinCh = getTCCChannelNr(pinC_h, (i>>4&0x01)==0x1); - tccConfiguration pinCl = getTCCChannelNr(pinC_l, (i>>5&0x01)==0x1); - if (checkPeripheralPermutationCompatible6(pinAh, pinAl, pinBh, pinBl, pinCh, pinCl)) - return i; - } - return -1; -} - - - -int scorePermutation(tccConfiguration pins[], uint8_t num) { - uint32_t usedtccs = 0; - for (int i=0;i>1; - } - for (int i=0;i>1; - } - return score; -} - - - - - -int checkPermutations(uint8_t num, int pins[], bool (*checkFunc)(tccConfiguration[], uint8_t) ) { - tccConfiguration tccConfs[num]; - int best = -1; - int bestscore = 1000000; - for (int i=0;i<(0x1<>j)&0x01)==0x1); - if (checkFunc(tccConfs, num)) { - int score = scorePermutation(tccConfs, num); - if (scoreCC[chan].reg = (uint32_t)((SIMPLEFOC_SAMD_PWM_RESOLUTION-1) * dc); - uint32_t chanbit = 0x1<<(TCC_SYNCBUSY_CC0_Pos+chan); - while ( (tcc->SYNCBUSY.reg & chanbit) > 0 ); + // set via CC + //tcc->CC[chan].reg = (uint32_t)((SIMPLEFOC_SAMD_PWM_RESOLUTION-1) * dc); + //uint32_t chanbit = 0x1<<(TCC_SYNCBUSY_CC0_Pos+chan); + //while ( (tcc->SYNCBUSY.reg & chanbit) > 0 ); // set via CCB -// tcc->CCB[chan].reg = (uint32_t)((SIMPLEFOC_SAMD_PWM_RESOLUTION-1) * dc); -// tcc->STATUS.vec.CCBV = tcc->STATUS.vec.CCBV | (1<SYNCBUSY.reg & chanbit) > 0 ); + tcc->CCB[chan].reg = (uint32_t)((SIMPLEFOC_SAMD_PWM_RESOLUTION-1) * dc); + while ( (tcc->SYNCBUSY.vec.CCB & (0x1< 0 ); + tcc->STATUS.vec.CCBV |= (0x1<SYNCBUSY.bit.STATUS > 0 ); + tcc->CTRLBSET.reg |= TCC_CTRLBSET_CMD(TCC_CTRLBSET_CMD_UPDATE_Val); + while ( tcc->SYNCBUSY.bit.CTRLB > 0 ); } else { Tc* tc = (Tc*)GetTC(chaninfo); - //tc->COUNT16.CC[chan].reg = (uint32_t)((SIMPLEFOC_SAMD_PWM_RESOLUTION-1) * dc); - tc->COUNT8.CC[chan].reg = (uint8_t)((SIMPLEFOC_SAMD_PWM_TC_RESOLUTION-1) * dc);; + tc->COUNT8.CC[chan].reg = (uint8_t)((SIMPLEFOC_SAMD_PWM_TC_RESOLUTION-1) * dc); while ( tc->COUNT8.STATUS.bit.SYNCBUSY == 1 ); } } @@ -498,468 +332,4 @@ void writeSAMDDutyCycle(int chaninfo, float dc) { - - - -/** - * Configuring PWM frequency, resolution and alignment - * - Stepper driver - 2PWM setting - * - hardware specific - * - * @param pwm_frequency - frequency in hertz - if applicable - * @param pinA pinA bldc driver - * @param pinB pinB bldc driver - */ -void _configure2PWM(long pwm_frequency, const int pinA, const int pinB) { -#ifdef SIMPLEFOC_SAMD_DEBUG - printAllPinInfos(); -#endif - int pins[2] = { pinA, pinB }; - int compatibility = checkPermutations(2, pins, checkPeripheralPermutationCompatible); - if (compatibility<0) { - // no result! -#ifdef SIMPLEFOC_SAMD_DEBUG - Serial.println("Bad combination!"); -#endif - return; - } - - tccConfiguration tccConfs[2] = { getTCCChannelNr(pinA, (compatibility>>0&0x01)==0x1), - getTCCChannelNr(pinB, (compatibility>>1&0x01)==0x1) }; - - -#ifdef SIMPLEFOC_SAMD_DEBUG - Serial.print("Found configuration: (score="); - Serial.print(scorePermutation(tccConfs, 2)); - Serial.println(")"); - printTCCConfiguration(tccConfs[0]); - printTCCConfiguration(tccConfs[1]); -#endif - - // attach pins to timer peripherals - attachTCC(tccConfs[0]); // in theory this can fail, but there is no way to signal it... - attachTCC(tccConfs[1]); -#ifdef SIMPLEFOC_SAMD_DEBUG - Serial.println("Attached pins..."); -#endif - - // set up clock - Note: if we did this right it should be possible to get all TCC units synchronized? - // e.g. attach all the timers, start them, and then start the clock... but this would require API-changes in SimpleFOC... - configureSAMDClock(); - - // configure the TCC (waveform, top-value, pre-scaler = frequency) - configureTCC(tccConfs[0], pwm_frequency); - configureTCC(tccConfs[1], pwm_frequency); -#ifdef SIMPLEFOC_SAMD_DEBUG - Serial.println("Configured TCCs..."); -#endif - - return; // Someone with a stepper-setup who can test it? -} - - - - - - - - - - - - -/** - * Configuring PWM frequency, resolution and alignment - * - BLDC driver - 3PWM setting - * - hardware specific - * - * SAMD21 will support up to 2 BLDC motors in 3-PWM: - * one on TCC0 using 3 of the channels 0,1,2 or 3 - * one on TCC3 using 3 of the channels 0,1,2 or 3 - * i.e. 8 different pins can be used, but only 4 different signals (WO[x]) on those 8 pins - * WO[0] and WO[4] are the same - * WO[1] and WO[5] are the same - * WO[2] and WO[6] are the same - * WO[3] and WO[7] are the same - * - * If you're on the Arduino Nano33 IoT, please see the Nano33 IoT pinout diagram to see which TCC0/WO[x] - * signal is on which pin of the Nano. You can drive one motor on TCC0. For other boards, consult their documentation. - * - * Note: - * That's if we want to keep the signals strictly in sync. - * - * If we can accept out-of-sync PWMs on the different phases, we could drive up to 4 BLDCs in 3-PWM mode, - * using all the TCC channels. (TCC0 & TCC3 - 4 channels each, TCC1 & TCC2 - 2 channels each) - * - * All channels will use the same resolution, prescaler and clock, but they will have different start-times leading - * to misaligned signals. - * - * - * @param pwm_frequency - frequency in hertz - if applicable - * @param pinA pinA bldc driver - * @param pinB pinB bldc driver - * @param pinC pinC bldc driver - */ -void _configure3PWM(long pwm_frequency, const int pinA, const int pinB, const int pinC) { -#ifdef SIMPLEFOC_SAMD_DEBUG - printAllPinInfos(); -#endif - int pins[3] = { pinA, pinB, pinC }; - int compatibility = checkPermutations(3, pins, checkPeripheralPermutationCompatible); - if (compatibility<0) { - // no result! -#ifdef SIMPLEFOC_SAMD_DEBUG - Serial.println("Bad combination!"); -#endif - return; - } - - tccConfiguration tccConfs[3] = { getTCCChannelNr(pinA, (compatibility>>0&0x01)==0x1), - getTCCChannelNr(pinB, (compatibility>>1&0x01)==0x1), - getTCCChannelNr(pinC, (compatibility>>2&0x01)==0x1) }; - - -#ifdef SIMPLEFOC_SAMD_DEBUG - Serial.print("Found configuration: (score="); - Serial.print(scorePermutation(tccConfs, 3)); - Serial.println(")"); - printTCCConfiguration(tccConfs[0]); - printTCCConfiguration(tccConfs[1]); - printTCCConfiguration(tccConfs[2]); -#endif - - // attach pins to timer peripherals - attachTCC(tccConfs[0]); // in theory this can fail, but there is no way to signal it... - attachTCC(tccConfs[1]); - attachTCC(tccConfs[2]); -#ifdef SIMPLEFOC_SAMD_DEBUG - Serial.println("Attached pins..."); -#endif - - // set up clock - Note: if we did this right it should be possible to get all TCC units synchronized? - // e.g. attach all the timers, start them, and then start the clock... but this would require API-changes in SimpleFOC... - configureSAMDClock(); - - // configure the TCC (waveform, top-value, pre-scaler = frequency) - configureTCC(tccConfs[0], pwm_frequency); - configureTCC(tccConfs[1], pwm_frequency); - configureTCC(tccConfs[2], pwm_frequency); -#ifdef SIMPLEFOC_SAMD_DEBUG - Serial.println("Configured TCCs..."); -#endif - -} - - - - - - - - -/** - * Configuring PWM frequency, resolution and alignment - * - Stepper driver - 4PWM setting - * - hardware specific - * - * @param pwm_frequency - frequency in hertz - if applicable - * @param pin1A pin1A stepper driver - * @param pin1B pin1B stepper driver - * @param pin2A pin2A stepper driver - * @param pin2B pin2B stepper driver - */ -void _configure4PWM(long pwm_frequency, const int pin1A, const int pin1B, const int pin2A, const int pin2B) { -#ifdef SIMPLEFOC_SAMD_DEBUG - printAllPinInfos(); -#endif - int pins[4] = { pin1A, pin1B, pin2A, pin2B }; - int compatibility = checkPermutations(4, pins, checkPeripheralPermutationCompatible); - if (compatibility<0) { - // no result! -#ifdef SIMPLEFOC_SAMD_DEBUG - Serial.println("Bad combination!"); -#endif - return; - } - - tccConfiguration tccConfs[4] = { getTCCChannelNr(pin1A, (compatibility>>0&0x01)==0x1), - getTCCChannelNr(pin1B, (compatibility>>1&0x01)==0x1), - getTCCChannelNr(pin2A, (compatibility>>2&0x01)==0x1), - getTCCChannelNr(pin2B, (compatibility>>3&0x01)==0x1) }; - - -#ifdef SIMPLEFOC_SAMD_DEBUG - Serial.print("Found configuration: (score="); - Serial.print(scorePermutation(tccConfs, 4)); - Serial.println(")"); - printTCCConfiguration(tccConfs[0]); - printTCCConfiguration(tccConfs[1]); - printTCCConfiguration(tccConfs[2]); - printTCCConfiguration(tccConfs[3]); -#endif - - // attach pins to timer peripherals - attachTCC(tccConfs[0]); // in theory this can fail, but there is no way to signal it... - attachTCC(tccConfs[1]); - attachTCC(tccConfs[2]); - attachTCC(tccConfs[3]); -#ifdef SIMPLEFOC_SAMD_DEBUG - Serial.println("Attached pins..."); -#endif - - // set up clock - Note: if we did this right it should be possible to get all TCC units synchronized? - // e.g. attach all the timers, start them, and then start the clock... but this would require API-changes in SimpleFOC... - configureSAMDClock(); - - // configure the TCC (waveform, top-value, pre-scaler = frequency) - configureTCC(tccConfs[0], pwm_frequency); - configureTCC(tccConfs[1], pwm_frequency); - configureTCC(tccConfs[2], pwm_frequency); - configureTCC(tccConfs[3], pwm_frequency); -#ifdef SIMPLEFOC_SAMD_DEBUG - Serial.println("Configured TCCs..."); -#endif - - return; // Someone with a stepper-setup who can test it? -} - - - - - - - - - -/** - * Configuring PWM frequency, resolution and alignment - * - BLDC driver - 6PWM setting - * - hardware specific - * - * SAMD21 will support up to 2 BLDC motors in 6-PWM: - * one on TCC0 using 3 of the channels 0,1,2 or 3 - * one on TCC3 using 3 of the channels 0,1,2 or 3 - * i.e. 6 out of 8 pins must be used, in the following high/low side pairs: - * WO[0] & WO[4] (high side & low side) - * WO[1] & WO[5] - * WO[2] & WO[6] - * WO[3] & WO[7] - * - * If you're on the Arduino Nano33 IoT, please see the Nano33 IoT pinout diagram to see which TCC0/WO[x] - * signal is on which pin of the Nano. You can drive 1 BLDC on TCC0. For other boards, consult their documentation. - * - * - * @param pwm_frequency - frequency in hertz - if applicable - * @param dead_zone duty cycle protection zone [0, 1] - both low and high side low - if applicable - * @param pinA_h pinA high-side bldc driver - * @param pinA_l pinA low-side bldc driver - * @param pinB_h pinA high-side bldc driver - * @param pinB_l pinA low-side bldc driver - * @param pinC_h pinA high-side bldc driver - * @param pinC_l pinA low-side bldc driver - * - * @return 0 if config good, -1 if failed - */ -int _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const int pinA_l, const int pinB_h, const int pinB_l, const int pinC_h, const int pinC_l) { - // we want to use a TCC channel with 1 non-inverted and 1 inverted output for each phase, with dead-time insertion -#ifdef SIMPLEFOC_SAMD_DEBUG - printAllPinInfos(); -#endif - int compatibility = checkHardware6PWM(pinA_h, pinA_l, pinB_h, pinB_l, pinC_h, pinC_l); - if (compatibility<0) { - compatibility = checkSoftware6PWM(pinA_h, pinA_l, pinB_h, pinB_l, pinC_h, pinC_l); - if (compatibility<0) { - // no result! -#ifdef SIMPLEFOC_SAMD_DEBUG - Serial.println("Bad combination!"); -#endif - return -1; - } - } - - tccConfiguration pinAh = getTCCChannelNr(pinA_h, (compatibility>>0&0x01)==0x1); - tccConfiguration pinAl = getTCCChannelNr(pinA_l, (compatibility>>1&0x01)==0x1); - tccConfiguration pinBh = getTCCChannelNr(pinB_h, (compatibility>>2&0x01)==0x1); - tccConfiguration pinBl = getTCCChannelNr(pinB_l, (compatibility>>3&0x01)==0x1); - tccConfiguration pinCh = getTCCChannelNr(pinC_h, (compatibility>>4&0x01)==0x1); - tccConfiguration pinCl = getTCCChannelNr(pinC_l, (compatibility>>5&0x01)==0x1); - -#ifdef SIMPLEFOC_SAMD_DEBUG - Serial.println("Found configuration: "); - printTCCConfiguration(pinAh); - printTCCConfiguration(pinAl); - printTCCConfiguration(pinBh); - printTCCConfiguration(pinBl); - printTCCConfiguration(pinCh); - printTCCConfiguration(pinCl); -#endif - - // attach pins to timer peripherals - bool allAttached = true; - allAttached |= attachTCC(pinAh); // in theory this can fail, but there is no way to signal it... - allAttached |= attachTCC(pinAl); - allAttached |= attachTCC(pinBh); - allAttached |= attachTCC(pinBl); - allAttached |= attachTCC(pinCh); - allAttached |= attachTCC(pinCl); - if (!allAttached) - return -1; -#ifdef SIMPLEFOC_SAMD_DEBUG - Serial.println("Attached pins..."); -#endif - // set up clock - if we did this right it should be possible to get all TCC units synchronized? - // e.g. attach all the timers, start them, and then start the clock... but this would require API changes in SimpleFOC driver API - configureSAMDClock(); - - // configure the TCC(s) - configureTCC(pinAh, pwm_frequency, false, (pinAh.tcc.chaninfo==pinAl.tcc.chaninfo)?dead_zone:-1); - if ((pinAh.tcc.chaninfo!=pinAl.tcc.chaninfo)) - configureTCC(pinAl, pwm_frequency, true, -1.0); - configureTCC(pinBh, pwm_frequency, false, (pinBh.tcc.chaninfo==pinBl.tcc.chaninfo)?dead_zone:-1); - if ((pinBh.tcc.chaninfo!=pinBl.tcc.chaninfo)) - configureTCC(pinBl, pwm_frequency, true, -1.0); - configureTCC(pinCh, pwm_frequency, false, (pinCh.tcc.chaninfo==pinCl.tcc.chaninfo)?dead_zone:-1); - if ((pinCh.tcc.chaninfo!=pinCl.tcc.chaninfo)) - configureTCC(pinCl, pwm_frequency, true, -1.0); -#ifdef SIMPLEFOC_SAMD_DEBUG - Serial.println("Configured TCCs..."); -#endif - - return 0; -} - - -/** - * Function setting the duty cycle to the pwm pin (ex. analogWrite()) - * - Stepper driver - 2PWM setting - * - hardware specific - * - * @param dc_a duty cycle phase A [0, 1] - * @param dc_b duty cycle phase B [0, 1] - * @param pinA phase A hardware pin number - * @param pinB phase B hardware pin number - */ -void _writeDutyCycle2PWM(float dc_a, float dc_b, int pinA, int pinB) { - tccConfiguration* tccI = getTccPinConfiguration(pinA); - writeSAMDDutyCycle(tccI->tcc.chaninfo, dc_a); - tccI = getTccPinConfiguration(pinB); - writeSAMDDutyCycle(tccI->tcc.chaninfo, dc_b); - return; -} - -/** - * Function setting the duty cycle to the pwm pin (ex. analogWrite()) - * - BLDC driver - 3PWM setting - * - hardware specific - * - * @param dc_a duty cycle phase A [0, 1] - * @param dc_b duty cycle phase B [0, 1] - * @param dc_c duty cycle phase C [0, 1] - * @param pinA phase A hardware pin number - * @param pinB phase B hardware pin number - * @param pinC phase C hardware pin number - */ -void _writeDutyCycle3PWM(float dc_a, float dc_b, float dc_c, int pinA, int pinB, int pinC) { - tccConfiguration* tccI = getTccPinConfiguration(pinA); - writeSAMDDutyCycle(tccI->tcc.chaninfo, dc_a); - tccI = getTccPinConfiguration(pinB); - writeSAMDDutyCycle(tccI->tcc.chaninfo, dc_b); - tccI = getTccPinConfiguration(pinC); - writeSAMDDutyCycle(tccI->tcc.chaninfo, dc_c); - return; -} - - -/** - * Function setting the duty cycle to the pwm pin (ex. analogWrite()) - * - Stepper driver - 4PWM setting - * - hardware specific - * - * @param dc_1a duty cycle phase 1A [0, 1] - * @param dc_1b duty cycle phase 1B [0, 1] - * @param dc_2a duty cycle phase 2A [0, 1] - * @param dc_2b duty cycle phase 2B [0, 1] - * @param pin1A phase 1A hardware pin number - * @param pin1B phase 1B hardware pin number - * @param pin2A phase 2A hardware pin number - * @param pin2B phase 2B hardware pin number - */ -void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, int pin1A, int pin1B, int pin2A, int pin2B){ - tccConfiguration* tccI = getTccPinConfiguration(pin1A); - writeSAMDDutyCycle(tccI->tcc.chaninfo, dc_1a); - tccI = getTccPinConfiguration(pin2A); - writeSAMDDutyCycle(tccI->tcc.chaninfo, dc_2a); - tccI = getTccPinConfiguration(pin1B); - writeSAMDDutyCycle(tccI->tcc.chaninfo, dc_1b); - tccI = getTccPinConfiguration(pin2B); - writeSAMDDutyCycle(tccI->tcc.chaninfo, dc_2b); - return; -} - -/** - * Function setting the duty cycle to the pwm pin (ex. analogWrite()) - * - BLDC driver - 6PWM setting - * - hardware specific - * - * Note: dead-time must be setup in advance, so parameter "dead_zone" is ignored - * the low side pins are automatically driven by the SAMD DTI module, so it is enough to set the high-side - * duty cycle. - * No sanity checks are perfomed to ensure the pinA, pinB, pinC are the same pins you used in configure method... - * so use appropriately. - * - * @param dc_a duty cycle phase A [0, 1] - * @param dc_b duty cycle phase B [0, 1] - * @param dc_c duty cycle phase C [0, 1] - * @param dead_zone duty cycle protection zone [0, 1] - both low and high side low - * @param pinA_h phase A high-side hardware pin number - * @param pinA_l phase A low-side hardware pin number - * @param pinB_h phase B high-side hardware pin number - * @param pinB_l phase B low-side hardware pin number - * @param pinC_h phase C high-side hardware pin number - * @param pinC_l phase C low-side hardware pin number - * - */ -void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, float dead_zone, int pinA_h, int pinA_l, int pinB_h, int pinB_l, int pinC_h, int pinC_l){ - tccConfiguration* tcc1 = getTccPinConfiguration(pinA_h); - tccConfiguration* tcc2 = getTccPinConfiguration(pinA_l); - if (tcc1->tcc.chaninfo!=tcc2->tcc.chaninfo) { - // low-side on a different pin of same TCC - do dead-time in software... - float ls = dc_a+(dead_zone*(SIMPLEFOC_SAMD_PWM_RESOLUTION-1)); - if (ls>1.0) ls = 1.0; // no off-time is better than too-short dead-time - writeSAMDDutyCycle(tcc1->tcc.chaninfo, dc_a); - writeSAMDDutyCycle(tcc2->tcc.chaninfo, ls); - } - else - writeSAMDDutyCycle(tcc1->tcc.chaninfo, dc_a); // dead-time is done is hardware, no need to set low side pin explicitly - - tcc1 = getTccPinConfiguration(pinB_h); - tcc2 = getTccPinConfiguration(pinB_l); - if (tcc1->tcc.chaninfo!=tcc2->tcc.chaninfo) { - float ls = dc_b+(dead_zone*(SIMPLEFOC_SAMD_PWM_RESOLUTION-1)); - if (ls>1.0) ls = 1.0; // no off-time is better than too-short dead-time - writeSAMDDutyCycle(tcc1->tcc.chaninfo, dc_b); - writeSAMDDutyCycle(tcc2->tcc.chaninfo, ls); - } - else - writeSAMDDutyCycle(tcc1->tcc.chaninfo, dc_b); - - tcc1 = getTccPinConfiguration(pinC_h); - tcc2 = getTccPinConfiguration(pinC_l); - if (tcc1->tcc.chaninfo!=tcc2->tcc.chaninfo) { - float ls = dc_c+(dead_zone*(SIMPLEFOC_SAMD_PWM_RESOLUTION-1)); - if (ls>1.0) ls = 1.0; // no off-time is better than too-short dead-time - writeSAMDDutyCycle(tcc1->tcc.chaninfo, dc_c); - writeSAMDDutyCycle(tcc2->tcc.chaninfo, ls); - } - else - writeSAMDDutyCycle(tcc1->tcc.chaninfo, dc_c); - return; -} - - - - - - - #endif diff --git a/src/drivers/hardware_specific/samd21_wo_associations.h b/src/drivers/hardware_specific/samd21_wo_associations.h deleted file mode 100644 index 3ab087e0..00000000 --- a/src/drivers/hardware_specific/samd21_wo_associations.h +++ /dev/null @@ -1,131 +0,0 @@ - - -struct wo_association { - EPortType port; - uint32_t pin; - ETCChannel tccE; - uint8_t woE; - ETCChannel tccF; - uint8_t woF; -}; - - - -#ifdef _SAMD21_ - - - -#ifndef TCC3_CH0 -#define TCC3_CH0 NOT_ON_TIMER -#endif -#ifndef TCC3_CH1 -#define TCC3_CH1 NOT_ON_TIMER -#endif -#ifndef TCC3_CH2 -#define TCC3_CH2 NOT_ON_TIMER -#endif -#ifndef TCC3_CH3 -#define TCC3_CH3 NOT_ON_TIMER -#endif -#ifndef TCC3_CH4 -#define TCC3_CH4 NOT_ON_TIMER -#endif -#ifndef TCC3_CH5 -#define TCC3_CH5 NOT_ON_TIMER -#endif -#ifndef TCC3_CH6 -#define TCC3_CH6 NOT_ON_TIMER -#endif -#ifndef TCC3_CH7 -#define TCC3_CH7 NOT_ON_TIMER -#endif -#ifndef TC6_CH0 -#define TC6_CH0 NOT_ON_TIMER -#endif -#ifndef TC6_CH1 -#define TC6_CH1 NOT_ON_TIMER -#endif -#ifndef TC7_CH0 -#define TC7_CH0 NOT_ON_TIMER -#endif -#ifndef TC7_CH1 -#define TC7_CH1 NOT_ON_TIMER -#endif - - -/* - * For SAM D21 A/B/C/D Variant Devices and SAM DA1 A/B Variant Devices - * Good for SAMD2xE, SAMD2xG and SAMD2xJ devices. Other SAMD21s currently not supported in arduino anyway? - * - * Note: only the pins which have timers associated are listed in this table. - * You can use the values from g_APinDescription.ulPort and g_APinDescription.ulPin to find the correct row in the table. - * - * See Microchip Technology datasheet DS40001882F-page 30 - */ -struct wo_association WO_associations[] = { - - { PORTA, 0, TCC2_CH0, 0, NOT_ON_TIMER, 0}, - { PORTA, 1, TCC2_CH1, 1, NOT_ON_TIMER, 0}, - { PORTA, 2, NOT_ON_TIMER, 0, TCC3_CH0, 0}, - { PORTA, 3, NOT_ON_TIMER, 0, TCC3_CH1, 1}, - // PB04, PB05, PB06, PB07 - no timers - { PORTB, 8, TC4_CH0, 0, TCC3_CH6, 6}, - { PORTB, 9, TC4_CH1, 1, TCC3_CH7, 7}, - { PORTA, 4, TCC0_CH0, 0, TCC3_CH2, 2}, - { PORTA, 5, TCC0_CH1, 1, TCC3_CH3, 3}, - { PORTA, 6, TCC1_CH0, 0, TCC3_CH4, 4}, - { PORTA, 7, TCC1_CH1, 1, TCC3_CH5, 5}, - { PORTA, 8, TCC0_CH0, 0, TCC1_CH2, 2}, - { PORTA, 9, TCC0_CH1, 1, TCC1_CH3, 3}, - { PORTA, 10, TCC1_CH0, 0, TCC0_CH2, 2}, - { PORTA, 11, TCC1_CH1, 1, TCC0_CH3, 3}, - { PORTB, 10, TC5_CH0, 0, TCC0_CH4, 4}, - { PORTB, 11, TC5_CH1, 1, TCC0_CH5, 5}, - { PORTB, 12, TC4_CH0, 0, TCC0_CH6, 6}, - { PORTB, 13, TC4_CH1, 1, TCC0_CH7, 7}, - { PORTB, 14, TC5_CH0, 0, NOT_ON_TIMER, 0}, - { PORTB, 15, TC5_CH1, 1, NOT_ON_TIMER, 0}, - { PORTA, 12, TCC2_CH0, 0, TCC0_CH6, 6}, - { PORTA, 13, TCC2_CH1, 1, TCC0_CH7, 7}, - { PORTA, 14, TC3_CH0, 0, TCC0_CH4, 4}, - { PORTA, 15, TC3_CH1, 1, TCC0_CH5, 5}, - { PORTA, 16, TCC2_CH0, 0, TCC0_CH6, 6}, - { PORTA, 17, TCC2_CH1, 1, TCC0_CH7, 7}, - { PORTA, 18, TC3_CH0, 0, TCC0_CH2, 2}, - { PORTA, 19, TC3_CH1, 1, TCC0_CH3, 3}, - { PORTB, 16, TC6_CH0, 0, TCC0_CH4, 4}, - { PORTB, 17, TC6_CH1, 1, TCC0_CH5, 5}, - { PORTA, 20, TC7_CH0, 0, TCC0_CH6, 6}, - { PORTA, 21, TC7_CH1, 1, TCC0_CH7, 7}, - { PORTA, 22, TC4_CH0, 0, TCC0_CH4, 4}, - { PORTA, 23, TC4_CH1, 1, TCC0_CH5, 5}, - { PORTA, 24, TC5_CH0, 0, TCC1_CH2, 2}, - { PORTA, 25, TC5_CH1, 1, TCC1_CH3, 3}, - { PORTB, 22, TC7_CH0, 0, TCC3_CH0, 0}, - { PORTB, 23, TC7_CH1, 1, TCC3_CH1, 1}, - { PORTA, 27, NOT_ON_TIMER, 0, TCC3_CH6, 6}, - { PORTA, 28, NOT_ON_TIMER, 0, TCC3_CH7, 7}, - { PORTA, 30, TCC1_CH0, 0, TCC3_CH4, 4}, - { PORTA, 31, TCC1_CH1, 1, TCC3_CH5, 5}, - { PORTB, 30, TCC0_CH0, 0, TCC1_CH2, 2}, - { PORTB, 31, TCC0_CH1, 1, TCC1_CH3, 3}, - { PORTB, 0, TC7_CH0, 0, NOT_ON_TIMER, 0}, - { PORTB, 1, TC7_CH1, 1, NOT_ON_TIMER, 0}, - { PORTB, 2, TC6_CH0, 0, TCC3_CH2, 2}, - { PORTB, 3, TC6_CH1, 1, TCC3_CH3, 3} -}; -#define NUM_WO_ASSOCIATIONS 48 - -wo_association ASSOCIATION_NOT_FOUND = { NOT_A_PORT, 0, NOT_ON_TIMER, 0, NOT_ON_TIMER, 0}; - - -struct wo_association& getWOAssociation(EPortType port, uint32_t pin) { - for (int i=0;i no timers + { PORTA, 8, TC0_CH0, 0, TCC0_CH0, 0, TCC1_CH0, 4}, + { PORTA, 9, TC0_CH1, 1, TCC0_CH1, 1, TCC1_CH1, 5}, + { PORTA, 10, TC1_CH0, 0, TCC0_CH2, 2, TCC1_CH2, 6}, + { PORTA, 11, TC1_CH1, 1, TCC0_CH3, 3, TCC1_CH3, 7}, + { PORTB, 10, TC5_CH0, 0, TCC0_CH0, 4, TCC1_CH0, 0}, //? + { PORTB, 11, TC5_CH1, 1, TCC0_CH1, 5, TCC1_CH1, 1}, //? + { PORTB, 12, TC4_CH0, 0, TCC3_CH0, 0, TCC0_CH0, 0}, + { PORTB, 13, TC4_CH1, 1, TCC3_CH1, 1, TCC0_CH1, 1}, + { PORTB, 14, TC5_CH0, 0, TCC4_CH0, 0, TCC0_CH2, 2}, + { PORTB, 15, TC5_CH1, 1, TCC4_CH1, 1, TCC0_CH3, 3}, + { PORTD, 8, NOT_ON_TIMER, 0, TCC0_CH1, 1, NOT_ON_TIMER, 0}, + { PORTD, 9, NOT_ON_TIMER, 0, TCC0_CH2, 2, NOT_ON_TIMER, 0}, + { PORTD, 10, NOT_ON_TIMER, 0, TCC0_CH3, 3, NOT_ON_TIMER, 0}, + { PORTD, 11, NOT_ON_TIMER, 0, TCC0_CH0, 4, NOT_ON_TIMER, 0}, //? + { PORTD, 12, NOT_ON_TIMER, 0, TCC0_CH1, 5, NOT_ON_TIMER, 0}, //? + { PORTC, 10, NOT_ON_TIMER, 0, TCC0_CH0, 0, TCC1_CH0, 4}, + { PORTC, 11, NOT_ON_TIMER, 0, TCC0_CH1, 1, TCC1_CH1, 5}, + { PORTC, 12, NOT_ON_TIMER, 0, TCC0_CH2, 2, TCC1_CH2, 6}, + { PORTC, 13, NOT_ON_TIMER, 0, TCC0_CH3, 3, TCC1_CH3, 7}, + { PORTC, 14, NOT_ON_TIMER, 0, TCC0_CH0, 4, TCC1_CH0, 0}, //? + { PORTC, 15, NOT_ON_TIMER, 0, TCC0_CH1, 5, TCC1_CH1, 1}, //? + { PORTA, 12, TC2_CH0, 0, TCC0_CH2, 6, TCC1_CH2, 2}, + { PORTA, 13, TC2_CH1, 1, TCC0_CH3, 7, TCC1_CH3, 3}, + { PORTA, 14, TC3_CH0, 0, TCC2_CH0, 0, TCC1_CH2, 2}, //? + { PORTA, 15, TC3_CH1, 1, TCC1_CH1, 1, TCC1_CH3, 3}, //? + { PORTA, 16, TC2_CH0, 0, TCC1_CH0, 0, TCC0_CH0, 4}, + { PORTA, 17, TC2_CH1, 1, TCC1_CH1, 1, TCC0_CH1, 5}, + { PORTA, 18, TC3_CH0, 0, TCC1_CH2, 2, TCC0_CH2, 6}, + { PORTA, 19, TC3_CH1, 1, TCC1_CH3, 3, TCC0_CH3, 7}, + { PORTC, 16, NOT_ON_TIMER, 0, TCC0_CH0, 0, NOT_ON_TIMER, 0}, // PDEC0 + { PORTC, 17, NOT_ON_TIMER, 0, TCC0_CH1, 1, NOT_ON_TIMER, 0}, // PDEC1 + { PORTC, 18, NOT_ON_TIMER, 0, TCC0_CH2, 2, NOT_ON_TIMER, 0}, // PDEC2 + { PORTC, 19, NOT_ON_TIMER, 0, TCC0_CH3, 3, NOT_ON_TIMER, 0}, + { PORTC, 20, NOT_ON_TIMER, 0, TCC0_CH0, 4, NOT_ON_TIMER, 0}, + { PORTC, 21, NOT_ON_TIMER, 0, TCC0_CH1, 5, NOT_ON_TIMER, 0}, + { PORTC, 22, NOT_ON_TIMER, 0, TCC0_CH2, 6, NOT_ON_TIMER, 0}, + { PORTC, 23, NOT_ON_TIMER, 0, TCC0_CH3, 7, NOT_ON_TIMER, 0}, + { PORTD, 20, NOT_ON_TIMER, 0, TCC1_CH0, 0, NOT_ON_TIMER, 0}, + { PORTD, 21, NOT_ON_TIMER, 0, TCC1_CH1, 1, NOT_ON_TIMER, 0}, + { PORTB, 16, TC6_CH0, 0, TCC3_CH0, 0, TCC0_CH0, 4}, + { PORTB, 17, TC6_CH1, 1, TCC3_CH1, 1, TCC0_CH1, 5}, + { PORTB, 18, NOT_ON_TIMER, 0, TCC1_CH0, 0, NOT_ON_TIMER, 0}, // PDEC0 + { PORTB, 19, NOT_ON_TIMER, 0, TCC1_CH1, 1, NOT_ON_TIMER, 0}, // PDEC1 + { PORTB, 20, NOT_ON_TIMER, 0, TCC1_CH2, 2, NOT_ON_TIMER, 0}, // PDEC2 + { PORTB, 21, NOT_ON_TIMER, 0, TCC1_CH3, 3, NOT_ON_TIMER, 0}, + { PORTA, 20, TC7_CH0, 0, TCC1_CH0, 4, TCC0_CH0, 0}, + { PORTA, 21, TC7_CH1, 1, TCC1_CH1, 5, TCC0_CH1, 1}, + { PORTA, 22, TC4_CH0, 0, TCC1_CH2, 6, TCC0_CH2, 2}, + { PORTA, 23, TC4_CH1, 1, TCC1_CH3, 7, TCC0_CH3, 3}, + { PORTA, 24, TC5_CH0, 0, TCC2_CH2, 2, NOT_ON_TIMER, 0}, // PDEC0 + { PORTA, 25, TC5_CH1, 1, NOT_ON_TIMER, 0, NOT_ON_TIMER, 0}, // PDEC1 + { PORTB, 22, TC7_CH0, 0, NOT_ON_TIMER, 0, NOT_ON_TIMER, 0}, // PDEC2 + { PORTB, 23, TC7_CH1, 1, NOT_ON_TIMER, 0, NOT_ON_TIMER, 0}, // PDEC0 + { PORTB, 24, NOT_ON_TIMER, 0, NOT_ON_TIMER, 0, NOT_ON_TIMER, 0}, // PDEC1 + { PORTB, 25, NOT_ON_TIMER, 0, NOT_ON_TIMER, 0, NOT_ON_TIMER, 0}, // PDEC2 + { PORTB, 26, NOT_ON_TIMER, 0, TCC1_CH2, 2, NOT_ON_TIMER, 0}, + { PORTB, 27, NOT_ON_TIMER, 0, TCC1_CH3, 3, NOT_ON_TIMER, 0}, + { PORTB, 28, NOT_ON_TIMER, 0, TCC1_CH0, 4, NOT_ON_TIMER, 0}, + { PORTB, 29, NOT_ON_TIMER, 1, TCC1_CH1, 5, NOT_ON_TIMER, 0}, + // PC24-PC28, PA27, RESET -> no TC/TCC peripherals + { PORTA, 30, TC6_CH0, 0, TCC2_CH0, 0, NOT_ON_TIMER, 0}, + { PORTA, 31, TC6_CH1, 1, TCC2_CH1, 1, NOT_ON_TIMER, 0}, + { PORTB, 30, TC0_CH0, 0, TCC4_CH0, 0, TCC0_CH2, 6}, + { PORTB, 31, TC0_CH1, 1, TCC4_CH1, 1, TCC0_CH3, 7}, + // PC30, PC31 -> no TC/TCC peripherals + { PORTB, 0, TC7_CH0, 0, NOT_ON_TIMER, 0, NOT_ON_TIMER, 0}, + { PORTB, 1, TC7_CH1, 1, NOT_ON_TIMER, 0, NOT_ON_TIMER, 0}, + { PORTB, 2, TC6_CH0, 0, TCC2_CH2, 2, NOT_ON_TIMER, 0}, + +}; + +wo_association ASSOCIATION_NOT_FOUND = { NOT_A_PORT, 0, NOT_ON_TIMER, 0, NOT_ON_TIMER, 0, NOT_ON_TIMER, 0}; + +uint8_t TCC_CHANNEL_COUNT[] = { TCC0_CC_NUM, TCC1_CC_NUM, TCC2_CC_NUM, TCC3_CC_NUM, TCC4_CC_NUM }; + +struct wo_association& getWOAssociation(EPortType port, uint32_t pin) { + for (int i=0;i>pin_position)&0x01)==0x1?PIO_TIMER_ALT:PIO_TIMER; +} + + + +void syncTCC(Tcc* TCCx) { + while (TCCx->SYNCBUSY.reg & TCC_SYNCBUSY_MASK); +} + + + + +void writeSAMDDutyCycle(int chaninfo, float dc) { + uint8_t tccn = GetTCNumber(chaninfo); + uint8_t chan = GetTCChannelNumber(chaninfo); + if (tccnCCBUF[chan].reg = (uint32_t)((SIMPLEFOC_SAMD_PWM_RESOLUTION-1) * dc); // TODO pwm frequency! + tcc->STATUS.vec.CCBUFV |= (0x1<SYNCBUSY.bit.STATUS > 0 ); + tcc->CTRLBSET.reg |= TCC_CTRLBSET_CMD(TCC_CTRLBSET_CMD_UPDATE_Val); + while ( tcc->SYNCBUSY.bit.CTRLB > 0 ); + } + else { + // Tc* tc = (Tc*)GetTC(chaninfo); + // //tc->COUNT16.CC[chan].reg = (uint32_t)((SIMPLEFOC_SAMD_PWM_RESOLUTION-1) * dc); + // tc->COUNT8.CC[chan].reg = (uint8_t)((SIMPLEFOC_SAMD_PWM_TC_RESOLUTION-1) * dc); + // while ( tc->COUNT8.STATUS.bit.SYNCBUSY == 1 ); + } +} + + +#define DPLL_CLOCK_NUM 2 // use GCLK2 +#define PWM_CLOCK_NUM 3 // use GCLK3 + + +/** + * Configure Clock 4 - we want all simplefoc PWMs to use the same clock. This ensures that + * any compatible pin combination can be used without having to worry about configuring different + * clocks. + */ +void configureSAMDClock() { + if (!SAMDClockConfigured) { + SAMDClockConfigured = true; // mark clock as configured + for (int i=0;iGENCTRL[DPLL_CLOCK_NUM].reg = GCLK_GENCTRL_GENEN | GCLK_GENCTRL_DIV(1) + // | GCLK_GENCTRL_SRC(GCLK_GENCTRL_SRC_DFLL_Val); + // while (GCLK->SYNCBUSY.vec.GENCTRL&(0x1<PCHCTRL[1].reg = GCLK_PCHCTRL_GEN(DPLL_CLOCK_NUM)|GCLK_PCHCTRL_CHEN; + // while (GCLK->SYNCBUSY.vec.GENCTRL&(0x1<Dpll[0].DPLLCTRLA.bit.ENABLE = 0; + // while (OSCCTRL->Dpll[0].DPLLSYNCBUSY.reg!=0x0); + + // OSCCTRL->Dpll[0].DPLLCTRLB.bit.REFCLK = OSCCTRL_DPLLCTRLB_REFCLK_GCLK_Val; + // while (OSCCTRL->Dpll[0].DPLLSYNCBUSY.reg!=0x0); + // OSCCTRL->Dpll[0].DPLLRATIO.reg = 3; + // while (OSCCTRL->Dpll[0].DPLLSYNCBUSY.reg!=0x0); + + // OSCCTRL->Dpll[0].DPLLCTRLA.bit.ENABLE = 1; + // while (OSCCTRL->Dpll[0].DPLLSYNCBUSY.reg!=0x0); + + GCLK->GENCTRL[PWM_CLOCK_NUM].bit.GENEN = 0; + while (GCLK->SYNCBUSY.vec.GENCTRL&(0x1<GENCTRL[PWM_CLOCK_NUM].reg = GCLK_GENCTRL_GENEN | GCLK_GENCTRL_DIV(1) | GCLK_GENCTRL_IDC + | GCLK_GENCTRL_SRC(GCLK_GENCTRL_SRC_DFLL_Val); + //| GCLK_GENCTRL_SRC(GCLK_GENCTRL_SRC_DPLL0_Val); + while (GCLK->SYNCBUSY.vec.GENCTRL&(0x1<PCHCTRL[GCLK_CLKCTRL_ID_ofthistcc].reg = GCLK_PCHCTRL_GEN(PWM_CLOCK_NUM)|GCLK_PCHCTRL_CHEN; + while (GCLK->SYNCBUSY.vec.GENCTRL&(0x1<CTRLA.bit.ENABLE = 0; //switch off tcc + while ( tcc->SYNCBUSY.bit.ENABLE == 1 ); // wait for sync + + uint8_t invenMask = ~(1<DRVCTRL.vec.INVEN = (tcc->DRVCTRL.vec.INVEN&invenMask)|invenVal; + syncTCC(tcc); // wait for sync + + if (hw6pwm>0.0) { + tcc->WEXCTRL.vec.DTIEN |= (1<WEXCTRL.bit.DTLS = hw6pwm*(SIMPLEFOC_SAMD_PWM_RESOLUTION-1); + tcc->WEXCTRL.bit.DTHS = hw6pwm*(SIMPLEFOC_SAMD_PWM_RESOLUTION-1); + syncTCC(tcc); // wait for sync + } + + if (!tccConfigured[tccConfig.tcc.tccn]) { + tcc->WAVE.reg |= TCC_WAVE_POL(0xF)|TCC_WAVE_WAVEGEN_DSTOP; // Set wave form configuration - TODO check this... why set like this? + while ( tcc->SYNCBUSY.bit.WAVE == 1 ); // wait for sync + + tcc->PER.reg = SIMPLEFOC_SAMD_PWM_RESOLUTION - 1; // Set counter Top using the PER register + while ( tcc->SYNCBUSY.bit.PER == 1 ); // wait for sync + + // set all channels to 0% + for (int i=0;iCC[i].reg = 0; // start off at 0% duty cycle + uint32_t chanbit = 0x1<<(TCC_SYNCBUSY_CC0_Pos+i); + while ( (tcc->SYNCBUSY.reg & chanbit) > 0 ); + } + } + + // Enable TCC + tcc->CTRLA.reg |= TCC_CTRLA_ENABLE | TCC_CTRLA_PRESCALER_DIV1; //48Mhz/1=48Mhz/2(up/down)=24MHz/1024=24KHz + while ( tcc->SYNCBUSY.bit.ENABLE == 1 ); // wait for sync + +#ifdef SIMPLEFOC_SAMD_DEBUG + Serial.print("(Re-)Initialized TCC "); + Serial.print(tccConfig.tcc.tccn); + Serial.print("-"); + Serial.print(tccConfig.tcc.chan); + Serial.print("["); + Serial.print(tccConfig.wo); + Serial.println("]"); +#endif + } + else if (tccConfig.tcc.tccn>=TCC_INST_NUM) { + Tc* tc = (Tc*)GetTC(tccConfig.tcc.chaninfo); + + // disable + // tc->COUNT8.CTRLA.bit.ENABLE = 0; + // while ( tc->COUNT8.STATUS.bit.SYNCBUSY == 1 ); + // // unfortunately we need the 8-bit counter mode to use the PER register... + // tc->COUNT8.CTRLA.reg |= TC_CTRLA_MODE_COUNT8 | TC_CTRLA_WAVEGEN_NPWM ; + // while ( tc->COUNT8.STATUS.bit.SYNCBUSY == 1 ); + // // meaning prescaler of 8, since TC-Unit has no up/down mode, and has resolution of 250 rather than 1000... + // tc->COUNT8.CTRLA.bit.PRESCALER = TC_CTRLA_PRESCALER_DIV8_Val ; + // while ( tc->COUNT8.STATUS.bit.SYNCBUSY == 1 ); + // // period is 250, period cannot be higher than 256! + // tc->COUNT8.PER.reg = SIMPLEFOC_SAMD_PWM_TC_RESOLUTION-1; + // while ( tc->COUNT8.STATUS.bit.SYNCBUSY == 1 ); + // // initial duty cycle is 0 + // tc->COUNT8.CC[tccConfig.tcc.chan].reg = 0; + // while ( tc->COUNT8.STATUS.bit.SYNCBUSY == 1 ); + // // enable + // tc->COUNT8.CTRLA.bit.ENABLE = 1; + // while ( tc->COUNT8.STATUS.bit.SYNCBUSY == 1 ); + + #ifdef SIMPLEFOC_SAMD_DEBUG + Serial.print("Initialized TC "); + Serial.println(tccConfig.tcc.tccn); + #endif + } + + // set as configured + tccConfigured[tccConfig.tcc.tccn] = true; + + +} + + + + + +#endif diff --git a/src/drivers/hardware_specific/samd_debug.h b/src/drivers/hardware_specific/samd_debug.h deleted file mode 100644 index 936ac930..00000000 --- a/src/drivers/hardware_specific/samd_debug.h +++ /dev/null @@ -1,107 +0,0 @@ - - - -#include "../hardware_api.h" -#include "wiring_private.h" - -// Read count -//TCC0->CTRLBSET.reg = TCC_CTRLBSET_CMD_READSYNC; -//while (TCC0->SYNCBUSY.bit.CTRLB); // or while (TCC0->SYNCBUSY.reg); -//int count = TCC0->COUNT.reg; - - -/** - * Prints a table of pin assignments for your SAMD MCU. Very useful since the - * board pinout descriptions and variant.cpp are usually quite wrong, and this - * saves you hours of cross-referencing with the datasheet. - */ -void printAllPinInfos() { - Serial.println(); - for (uint8_t pin=0;pin=0) { - Serial.print(info.tcc.tccn); - Serial.print("-"); - Serial.print(info.tcc.chan); - Serial.print("["); - Serial.print(info.wo); - Serial.println("]"); - } - else - Serial.println(" None"); -} - - - diff --git a/src/drivers/hardware_specific/samd_mcu.cpp b/src/drivers/hardware_specific/samd_mcu.cpp new file mode 100644 index 00000000..c30b4a90 --- /dev/null +++ b/src/drivers/hardware_specific/samd_mcu.cpp @@ -0,0 +1,870 @@ + + + +#include "./samd_mcu.h" + +#if defined(_SAMD21_)||defined(_SAMD51_) + + + +/** + * Global state + */ +tccConfiguration tccPinConfigurations[SIMPLEFOC_SAMD_MAX_TCC_PINCONFIGURATIONS]; +uint8_t numTccPinConfigurations = 0; +bool SAMDClockConfigured = false; +bool tccConfigured[TCC_INST_NUM+TC_INST_NUM]; + + + + + +/** + * Attach the TCC to the pin + */ +bool attachTCC(tccConfiguration& tccConfig) { + if (numTccPinConfigurations>=SIMPLEFOC_SAMD_MAX_TCC_PINCONFIGURATIONS) + return false; + pinMode(tccConfig.pin, OUTPUT); + + pinPeripheral(tccConfig.pin, tccConfig.peripheral); + tccPinConfigurations[numTccPinConfigurations++] = tccConfig; + return true; +} + + + + + +int getPermutationNumber(int pins) { + int num = 1; + for (int i=0;i=TCC_INST_NUM) + return false; + + if (pinAh.tcc.chan==pinBh.tcc.chan || pinAh.tcc.chan==pinBl.tcc.chan || pinAh.tcc.chan==pinCh.tcc.chan || pinAh.tcc.chan==pinCl.tcc.chan) + return false; + if (pinBh.tcc.chan==pinCh.tcc.chan || pinBh.tcc.chan==pinCl.tcc.chan) + return false; + if (pinAl.tcc.chan==pinBh.tcc.chan || pinAl.tcc.chan==pinBl.tcc.chan || pinAl.tcc.chan==pinCh.tcc.chan || pinAl.tcc.chan==pinCl.tcc.chan) + return false; + if (pinBl.tcc.chan==pinCh.tcc.chan || pinBl.tcc.chan==pinCl.tcc.chan) + return false; + + if (pinAh.tcc.chan!=pinAl.tcc.chan || pinBh.tcc.chan!=pinBl.tcc.chan || pinCh.tcc.chan!=pinCl.tcc.chan) + return false; + if (pinAh.wo==pinAl.wo || pinBh.wo==pinBl.wo || pinCh.wo!=pinCl.wo) + return false; + + return true; +} + + + + +bool checkPeripheralPermutationCompatible(tccConfiguration pins[], uint8_t num) { + for (int i=0;i=TCC_INST_NUM || pinAl.tcc.tccn>=TCC_INST_NUM || pinBh.tcc.tccn>=TCC_INST_NUM + || pinBl.tcc.tccn>=TCC_INST_NUM || pinCh.tcc.tccn>=TCC_INST_NUM || pinCl.tcc.tccn>=TCC_INST_NUM) + return false; + + // check we're not in use + if (inUse(pinAh) || inUse(pinAl) || inUse(pinBh) || inUse(pinBl) || inUse(pinCh) || inUse(pinCl)) + return false; + + // check pins are all different tccs/channels + if (pinAh.tcc.chaninfo==pinBh.tcc.chaninfo || pinAh.tcc.chaninfo==pinBl.tcc.chaninfo || pinAh.tcc.chaninfo==pinCh.tcc.chaninfo || pinAh.tcc.chaninfo==pinCl.tcc.chaninfo) + return false; + if (pinAl.tcc.chaninfo==pinBh.tcc.chaninfo || pinAl.tcc.chaninfo==pinBl.tcc.chaninfo || pinAl.tcc.chaninfo==pinCh.tcc.chaninfo || pinAl.tcc.chaninfo==pinCl.tcc.chaninfo) + return false; + if (pinBh.tcc.chaninfo==pinCh.tcc.chaninfo || pinBh.tcc.chaninfo==pinCl.tcc.chaninfo) + return false; + if (pinBl.tcc.chaninfo==pinCh.tcc.chaninfo || pinBl.tcc.chaninfo==pinCl.tcc.chaninfo) + return false; + + // check H/L pins are on same timer + if (pinAh.tcc.tccn!=pinAl.tcc.tccn || pinBh.tcc.tccn!=pinBl.tcc.tccn || pinCh.tcc.tccn!=pinCl.tcc.tccn) + return false; + + // check H/L pins aren't on both the same timer and wo + if (pinAh.wo==pinAl.wo || pinBh.wo==pinBl.wo || pinCh.wo==pinCl.wo) + return false; + + return true; +} + + + + + +int checkHardware6PWM(const int pinA_h, const int pinA_l, const int pinB_h, const int pinB_l, const int pinC_h, const int pinC_l) { + for (int i=0;i<64;i++) { + tccConfiguration pinAh = getTCCChannelNr(pinA_h, getPeripheralOfPermutation(i, 0)); + tccConfiguration pinAl = getTCCChannelNr(pinA_l, getPeripheralOfPermutation(i, 1)); + tccConfiguration pinBh = getTCCChannelNr(pinB_h, getPeripheralOfPermutation(i, 2)); + tccConfiguration pinBl = getTCCChannelNr(pinB_l, getPeripheralOfPermutation(i, 3)); + tccConfiguration pinCh = getTCCChannelNr(pinC_h, getPeripheralOfPermutation(i, 4)); + tccConfiguration pinCl = getTCCChannelNr(pinC_l, getPeripheralOfPermutation(i, 5)); + if (checkPeripheralPermutationSameTCC6(pinAh, pinAl, pinBh, pinBl, pinCh, pinCl)) + return i; + } + return -1; +} + + + + +int checkSoftware6PWM(const int pinA_h, const int pinA_l, const int pinB_h, const int pinB_l, const int pinC_h, const int pinC_l) { + for (int i=0;i<64;i++) { + tccConfiguration pinAh = getTCCChannelNr(pinA_h, getPeripheralOfPermutation(i, 0)); + tccConfiguration pinAl = getTCCChannelNr(pinA_l, getPeripheralOfPermutation(i, 1)); + tccConfiguration pinBh = getTCCChannelNr(pinB_h, getPeripheralOfPermutation(i, 2)); + tccConfiguration pinBl = getTCCChannelNr(pinB_l, getPeripheralOfPermutation(i, 3)); + tccConfiguration pinCh = getTCCChannelNr(pinC_h, getPeripheralOfPermutation(i, 4)); + tccConfiguration pinCl = getTCCChannelNr(pinC_l, getPeripheralOfPermutation(i, 5)); + if (checkPeripheralPermutationCompatible6(pinAh, pinAl, pinBh, pinBl, pinCh, pinCl)) + return i; + } + return -1; +} + + + +int scorePermutation(tccConfiguration pins[], uint8_t num) { + uint32_t usedtccs = 0; + for (int i=0;i>1; + } + for (int i=0;i>1; + } + return score; +} + + + + + + + + +int checkPermutations(uint8_t num, int pins[], bool (*checkFunc)(tccConfiguration[], uint8_t) ) { + tccConfiguration tccConfs[num]; + int best = -1; + int bestscore = 1000000; + for (int i=0;i<(0x1<tcc.chaninfo, dc_a); + tccI = getTccPinConfiguration(pinB); + writeSAMDDutyCycle(tccI->tcc.chaninfo, dc_b); + return; +} + + + + + +/** + * Function setting the duty cycle to the pwm pin (ex. analogWrite()) + * - BLDC driver - 3PWM setting + * - hardware specific + * + * @param dc_a duty cycle phase A [0, 1] + * @param dc_b duty cycle phase B [0, 1] + * @param dc_c duty cycle phase C [0, 1] + * @param pinA phase A hardware pin number + * @param pinB phase B hardware pin number + * @param pinC phase C hardware pin number + */ +void _writeDutyCycle3PWM(float dc_a, float dc_b, float dc_c, int pinA, int pinB, int pinC) { + tccConfiguration* tccI = getTccPinConfiguration(pinA); + writeSAMDDutyCycle(tccI->tcc.chaninfo, dc_a); + tccI = getTccPinConfiguration(pinB); + writeSAMDDutyCycle(tccI->tcc.chaninfo, dc_b); + tccI = getTccPinConfiguration(pinC); + writeSAMDDutyCycle(tccI->tcc.chaninfo, dc_c); + return; +} + + + + +/** + * Function setting the duty cycle to the pwm pin (ex. analogWrite()) + * - Stepper driver - 4PWM setting + * - hardware specific + * + * @param dc_1a duty cycle phase 1A [0, 1] + * @param dc_1b duty cycle phase 1B [0, 1] + * @param dc_2a duty cycle phase 2A [0, 1] + * @param dc_2b duty cycle phase 2B [0, 1] + * @param pin1A phase 1A hardware pin number + * @param pin1B phase 1B hardware pin number + * @param pin2A phase 2A hardware pin number + * @param pin2B phase 2B hardware pin number + */ +void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, int pin1A, int pin1B, int pin2A, int pin2B){ + tccConfiguration* tccI = getTccPinConfiguration(pin1A); + writeSAMDDutyCycle(tccI->tcc.chaninfo, dc_1a); + tccI = getTccPinConfiguration(pin2A); + writeSAMDDutyCycle(tccI->tcc.chaninfo, dc_2a); + tccI = getTccPinConfiguration(pin1B); + writeSAMDDutyCycle(tccI->tcc.chaninfo, dc_1b); + tccI = getTccPinConfiguration(pin2B); + writeSAMDDutyCycle(tccI->tcc.chaninfo, dc_2b); + return; +} + + + + +/** + * Function setting the duty cycle to the pwm pin (ex. analogWrite()) + * - BLDC driver - 6PWM setting + * - hardware specific + * + * Note: dead-time must be setup in advance, so parameter "dead_zone" is ignored + * the low side pins are automatically driven by the SAMD DTI module, so it is enough to set the high-side + * duty cycle. + * No sanity checks are perfomed to ensure the pinA, pinB, pinC are the same pins you used in configure method... + * so use appropriately. + * + * @param dc_a duty cycle phase A [0, 1] + * @param dc_b duty cycle phase B [0, 1] + * @param dc_c duty cycle phase C [0, 1] + * @param dead_zone duty cycle protection zone [0, 1] - both low and high side low + * @param pinA_h phase A high-side hardware pin number + * @param pinA_l phase A low-side hardware pin number + * @param pinB_h phase B high-side hardware pin number + * @param pinB_l phase B low-side hardware pin number + * @param pinC_h phase C high-side hardware pin number + * @param pinC_l phase C low-side hardware pin number + * + */ +void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, float dead_zone, int pinA_h, int pinA_l, int pinB_h, int pinB_l, int pinC_h, int pinC_l){ + tccConfiguration* tcc1 = getTccPinConfiguration(pinA_h); + tccConfiguration* tcc2 = getTccPinConfiguration(pinA_l); + if (tcc1->tcc.chaninfo!=tcc2->tcc.chaninfo) { + // low-side on a different pin of same TCC - do dead-time in software... + float ls = dc_a+(dead_zone*(SIMPLEFOC_SAMD_PWM_RESOLUTION-1)); + if (ls>1.0) ls = 1.0; // no off-time is better than too-short dead-time + writeSAMDDutyCycle(tcc1->tcc.chaninfo, dc_a); + writeSAMDDutyCycle(tcc2->tcc.chaninfo, ls); + } + else + writeSAMDDutyCycle(tcc1->tcc.chaninfo, dc_a); // dead-time is done is hardware, no need to set low side pin explicitly + + tcc1 = getTccPinConfiguration(pinB_h); + tcc2 = getTccPinConfiguration(pinB_l); + if (tcc1->tcc.chaninfo!=tcc2->tcc.chaninfo) { + float ls = dc_b+(dead_zone*(SIMPLEFOC_SAMD_PWM_RESOLUTION-1)); + if (ls>1.0) ls = 1.0; // no off-time is better than too-short dead-time + writeSAMDDutyCycle(tcc1->tcc.chaninfo, dc_b); + writeSAMDDutyCycle(tcc2->tcc.chaninfo, ls); + } + else + writeSAMDDutyCycle(tcc1->tcc.chaninfo, dc_b); + + tcc1 = getTccPinConfiguration(pinC_h); + tcc2 = getTccPinConfiguration(pinC_l); + if (tcc1->tcc.chaninfo!=tcc2->tcc.chaninfo) { + float ls = dc_c+(dead_zone*(SIMPLEFOC_SAMD_PWM_RESOLUTION-1)); + if (ls>1.0) ls = 1.0; // no off-time is better than too-short dead-time + writeSAMDDutyCycle(tcc1->tcc.chaninfo, dc_c); + writeSAMDDutyCycle(tcc2->tcc.chaninfo, ls); + } + else + writeSAMDDutyCycle(tcc1->tcc.chaninfo, dc_c); + return; +} + + + + +#ifdef SIMPLEFOC_SAMD_DEBUG + +/** + * Prints a table of pin assignments for your SAMD MCU. Very useful since the + * board pinout descriptions and variant.cpp are usually quite wrong, and this + * saves you hours of cross-referencing with the datasheet. + */ +void printAllPinInfos() { + Serial.println(); + for (uint8_t pin=0;pin=TCC_INST_NUM) + Serial.print(": TC Peripheral"); + else + Serial.print(": TCC Peripheral"); + switch (info.peripheral) { + case PIO_TIMER: + Serial.print(" E "); break; + case PIO_TIMER_ALT: + Serial.print(" F "); break; +#ifdef _SAMD51_ + case PIO_TCC_PDEC: + Serial.print(" G "); break; +#endif + default: + Serial.print(" ? "); break; + } + if (info.tcc.tccn>=0) { + Serial.print(info.tcc.tccn); + Serial.print("-"); + Serial.print(info.tcc.chan); + Serial.print("["); + Serial.print(info.wo); + Serial.println("]"); + } + else + Serial.println(" None"); +} + + + +#endif + +#endif + + diff --git a/src/drivers/hardware_specific/samd_mcu.h b/src/drivers/hardware_specific/samd_mcu.h new file mode 100644 index 00000000..be07e0ba --- /dev/null +++ b/src/drivers/hardware_specific/samd_mcu.h @@ -0,0 +1,103 @@ + +#ifndef SAMD_MCU_H +#define SAMD_MCU_H + + +// uncomment to enable debug output to Serial port +#define SIMPLEFOC_SAMD_DEBUG + + + +#include "Arduino.h" +#include "variant.h" +#include "wiring_private.h" +#include "../hardware_api.h" + + + + +#if defined(_SAMD21_)||defined(_SAMD51_) + + +#ifndef SIMPLEFOC_SAMD_PWM_RESOLUTION +#define SIMPLEFOC_SAMD_PWM_RESOLUTION 1000 +#define SIMPLEFOC_SAMD_PWM_TC_RESOLUTION 250 +#endif + +#ifndef SIMPLEFOC_SAMD_MAX_TCC_PINCONFIGURATIONS +#define SIMPLEFOC_SAMD_MAX_TCC_PINCONFIGURATIONS 24 +#endif + + + +struct tccConfiguration { + uint8_t pin; + EPioType peripheral; // 1=true, 0=false + uint8_t wo; + union tccChanInfo { + struct { + int8_t chan; + int8_t tccn; + }; + uint16_t chaninfo; + } tcc; +}; + + + + + + +struct wo_association { + EPortType port; + uint32_t pin; + ETCChannel tccE; + uint8_t woE; + ETCChannel tccF; + uint8_t woF; +#if defined(_SAMD51_) + ETCChannel tccG; + uint8_t woG; +#endif +}; + + + +#if defined(_SAMD21_) +#define NUM_PIO_TIMER_PERIPHERALS 2 +#elif defined(_SAMD51_) +#define NUM_PIO_TIMER_PERIPHERALS 3 +#endif + + + +/** + * Global state + */ +extern struct wo_association WO_associations[]; +extern uint8_t TCC_CHANNEL_COUNT[]; +extern tccConfiguration tccPinConfigurations[SIMPLEFOC_SAMD_MAX_TCC_PINCONFIGURATIONS]; +extern uint8_t numTccPinConfigurations; +extern bool SAMDClockConfigured; +extern bool tccConfigured[TCC_INST_NUM+TC_INST_NUM]; + + + +struct wo_association& getWOAssociation(EPortType port, uint32_t pin); +void writeSAMDDutyCycle(int chaninfo, float dc); +void configureSAMDClock(); +void configureTCC(tccConfiguration& tccConfig, long pwm_frequency, bool negate=false, float hw6pwm=-1); +__inline__ void syncTCC(Tcc* TCCx) __attribute__((always_inline, unused)); +EPioType getPeripheralOfPermutation(int permutation, int pin_position); + +#ifdef SIMPLEFOC_SAMD_DEBUG +void printTCCConfiguration(tccConfiguration& info); +void printAllPinInfos(); +#endif + + + +#endif + + +#endif