From 494ced58187b3a5116b515cefa10f1b6df61764f Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Wed, 10 Nov 2021 21:59:28 +0100 Subject: [PATCH 1/2] SAMD driver improvements --- src/drivers/hardware_specific/samd21_mcu.cpp | 11 +-- src/drivers/hardware_specific/samd51_mcu.cpp | 63 ++++++++++------- src/drivers/hardware_specific/samd_mcu.cpp | 71 +++++++++++++++----- src/drivers/hardware_specific/samd_mcu.h | 13 +++- 4 files changed, 108 insertions(+), 50 deletions(-) diff --git a/src/drivers/hardware_specific/samd21_mcu.cpp b/src/drivers/hardware_specific/samd21_mcu.cpp index ab41b745..9be2eee0 100644 --- a/src/drivers/hardware_specific/samd21_mcu.cpp +++ b/src/drivers/hardware_specific/samd21_mcu.cpp @@ -159,6 +159,7 @@ void configureSAMDClock() { REG_GCLK_GENCTRL = GCLK_GENCTRL_IDC | // Set the duty cycle to 50/50 HIGH/LOW GCLK_GENCTRL_GENEN | // Enable GCLK4 GCLK_GENCTRL_SRC_DFLL48M | // Set the 48MHz clock source + // GCLK_GENCTRL_SRC_FDPLL | // Set the 96MHz clock source GCLK_GENCTRL_ID(4); // Select GCLK4 while (GCLK->STATUS.bit.SYNCBUSY); // Wait for synchronization @@ -305,11 +306,11 @@ void configureTCC(tccConfiguration& tccConfig, long pwm_frequency, bool negate, -void writeSAMDDutyCycle(int chaninfo, float dc) { - uint8_t tccn = GetTCNumber(chaninfo); - uint8_t chan = GetTCChannelNumber(chaninfo); +void writeSAMDDutyCycle(tccConfiguration* info, float dc) { + uint8_t tccn = GetTCNumber(info->tcc.chaninfo); + uint8_t chan = GetTCChannelNumber(info->tcc.chaninfo); if (tccntcc.chaninfo); // set via CC // tcc->CC[chan].reg = (uint32_t)((SIMPLEFOC_SAMD_PWM_RESOLUTION-1) * dc); // uint32_t chanbit = 0x1<<(TCC_SYNCBUSY_CC0_Pos+chan); @@ -324,7 +325,7 @@ void writeSAMDDutyCycle(int chaninfo, float dc) { // while ( tcc->SYNCBUSY.bit.CTRLB > 0 ); } else { - Tc* tc = (Tc*)GetTC(chaninfo); + Tc* tc = (Tc*)GetTC(info->tcc.chaninfo); tc->COUNT8.CC[chan].reg = (uint8_t)((SIMPLEFOC_SAMD_PWM_TC_RESOLUTION-1) * dc); while ( tc->COUNT8.STATUS.bit.SYNCBUSY == 1 ); } diff --git a/src/drivers/hardware_specific/samd51_mcu.cpp b/src/drivers/hardware_specific/samd51_mcu.cpp index b59a43fc..19474d03 100644 --- a/src/drivers/hardware_specific/samd51_mcu.cpp +++ b/src/drivers/hardware_specific/samd51_mcu.cpp @@ -7,6 +7,16 @@ +// expected frequency on DPLL, since we don't configure it ourselves. Typically this is the CPU frequency. +// for custom boards or overclockers you can override it using this define. +#ifndef SIMPLEFOC_SAMD51_DPLL_FREQ +#define SIMPLEFOC_SAMD51_DPLL_FREQ 120000000 +#endif + + + + + // TCC# Channels WO_NUM Counter size Fault Dithering Output matrix DTI SWAP Pattern generation // 0 6 8 24-bit Yes Yes Yes Yes Yes Yes // 1 4 8 24-bit Yes Yes Yes Yes Yes Yes @@ -17,7 +27,6 @@ #define NUM_WO_ASSOCIATIONS 72 - struct wo_association WO_associations[] = { { PORTB, 9, TC4_CH1, 1, NOT_ON_TIMER, 0, NOT_ON_TIMER, 0}, @@ -112,7 +121,7 @@ struct wo_association& getWOAssociation(EPortType port, uint32_t pin) { EPioType getPeripheralOfPermutation(int permutation, int pin_position) { - return ((permutation>>pin_position)&0x01)==0x1?PIO_TIMER_ALT:PIO_TIMER; + return ((permutation>>pin_position)&0x01)==0x1?PIO_TCC_PDEC:PIO_TIMER_ALT; } @@ -124,24 +133,17 @@ void syncTCC(Tcc* TCCx) { -void writeSAMDDutyCycle(int chaninfo, float dc) { - uint8_t tccn = GetTCNumber(chaninfo); - uint8_t chan = GetTCChannelNumber(chaninfo); +void writeSAMDDutyCycle(tccConfiguration* info, float dc) { + uint8_t tccn = GetTCNumber(info->tcc.chaninfo); + uint8_t chan = GetTCChannelNumber(info->tcc.chaninfo); if (tccntcc.chaninfo); // set via CCBUF - while ( (tcc->SYNCBUSY.vec.CC & (0x1< 0 ); - tcc->CCBUF[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 ); +// while ( (tcc->SYNCBUSY.vec.CC & (0x1< 0 ); + tcc->CCBUF[chan].reg = (uint32_t)((info->pwm_res-1) * dc); // TODO pwm frequency! } - 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 ); + else { + // we don't support the TC channels on SAMD51, isn't worth it. } } @@ -183,8 +185,8 @@ void configureSAMDClock() { 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); + //| GCLK_GENCTRL_SRC(GCLK_GENCTRL_SRC_DFLL_Val); + | GCLK_GENCTRL_SRC(GCLK_GENCTRL_SRC_DPLL0_Val); while (GCLK->SYNCBUSY.vec.GENCTRL&(0x1<DRVCTRL.vec.INVEN = (tcc->DRVCTRL.vec.INVEN&invenMask)|invenVal; syncTCC(tcc); // wait for sync + // work out pwm resolution for desired frequency and constrain to max/min values + long pwm_resolution = (SIMPLEFOC_SAMD51_DPLL_FREQ/2) / pwm_frequency; + if (pwm_resolution>SIMPLEFOC_SAMD_MAX_PWM_RESOLUTION) + pwm_resolution = SIMPLEFOC_SAMD_MAX_PWM_RESOLUTION; + if (pwm_resolution0.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); + tcc->WEXCTRL.bit.DTLS = hw6pwm*(pwm_resolution-1); + tcc->WEXCTRL.bit.DTHS = hw6pwm*(pwm_resolution-1); syncTCC(tcc); // wait for sync } @@ -232,7 +243,7 @@ void configureTCC(tccConfiguration& tccConfig, long pwm_frequency, bool negate, 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 + tcc->PER.reg = pwm_resolution - 1; // Set counter Top using the PER register while ( tcc->SYNCBUSY.bit.PER == 1 ); // wait for sync // set all channels to 0% @@ -254,11 +265,12 @@ void configureTCC(tccConfiguration& tccConfig, long pwm_frequency, bool negate, SIMPLEFOC_SAMD_DEBUG_SERIAL.print(tccConfig.tcc.chan); SIMPLEFOC_SAMD_DEBUG_SERIAL.print("["); SIMPLEFOC_SAMD_DEBUG_SERIAL.print(tccConfig.wo); - SIMPLEFOC_SAMD_DEBUG_SERIAL.println("]"); + SIMPLEFOC_SAMD_DEBUG_SERIAL.print("] pwm res "); + SIMPLEFOC_SAMD_DEBUG_SERIAL.println(pwm_resolution); #endif } else if (tccConfig.tcc.tccn>=TCC_INST_NUM) { - Tc* tc = (Tc*)GetTC(tccConfig.tcc.chaninfo); + //Tc* tc = (Tc*)GetTC(tccConfig.tcc.chaninfo); // disable // tc->COUNT8.CTRLA.bit.ENABLE = 0; @@ -280,8 +292,9 @@ void configureTCC(tccConfiguration& tccConfig, long pwm_frequency, bool negate, // while ( tc->COUNT8.STATUS.bit.SYNCBUSY == 1 ); #ifdef SIMPLEFOC_SAMD_DEBUG - SIMPLEFOC_SAMD_DEBUG_SERIAL.print("Initialized TC "); + SIMPLEFOC_SAMD_DEBUG_SERIAL.print("Not initialized: TC "); SIMPLEFOC_SAMD_DEBUG_SERIAL.println(tccConfig.tcc.tccn); + SIMPLEFOC_SAMD_DEBUG_SERIAL.print("TC units not supported on SAMD51"); #endif } diff --git a/src/drivers/hardware_specific/samd_mcu.cpp b/src/drivers/hardware_specific/samd_mcu.cpp index e1d38a80..5c0bb71e 100644 --- a/src/drivers/hardware_specific/samd_mcu.cpp +++ b/src/drivers/hardware_specific/samd_mcu.cpp @@ -315,9 +315,16 @@ void _configure2PWM(long pwm_frequency, const int pinA, const int pinB) { // e.g. attach all the timers, start them, and then start the clock... but this would require API-changes in SimpleFOC... configureSAMDClock(); + if (pwm_frequency==NOT_SET) { + // use default frequency + pwm_frequency = SIMPLEFOC_SAMD_DEFAULT_PWM_FREQUENCY_HZ; + } + // configure the TCC (waveform, top-value, pre-scaler = frequency) configureTCC(tccConfs[0], pwm_frequency); configureTCC(tccConfs[1], pwm_frequency); + getTccPinConfiguration(pinA)->pwm_res = tccConfs[0].pwm_res; + getTccPinConfiguration(pinB)->pwm_res = tccConfs[1].pwm_res; #ifdef SIMPLEFOC_SAMD_DEBUG SIMPLEFOC_SAMD_DEBUG_SERIAL.println("Configured TCCs..."); #endif @@ -408,10 +415,18 @@ void _configure3PWM(long pwm_frequency, const int pinA, const int pinB, const in // e.g. attach all the timers, start them, and then start the clock... but this would require API-changes in SimpleFOC... configureSAMDClock(); + if (pwm_frequency==NOT_SET) { + // use default frequency + pwm_frequency = SIMPLEFOC_SAMD_DEFAULT_PWM_FREQUENCY_HZ; + } + // configure the TCC (waveform, top-value, pre-scaler = frequency) configureTCC(tccConfs[0], pwm_frequency); configureTCC(tccConfs[1], pwm_frequency); configureTCC(tccConfs[2], pwm_frequency); + getTccPinConfiguration(pinA)->pwm_res = tccConfs[0].pwm_res; + getTccPinConfiguration(pinB)->pwm_res = tccConfs[1].pwm_res; + getTccPinConfiguration(pinC)->pwm_res = tccConfs[2].pwm_res; #ifdef SIMPLEFOC_SAMD_DEBUG SIMPLEFOC_SAMD_DEBUG_SERIAL.println("Configured TCCs..."); #endif @@ -479,11 +494,20 @@ void _configure4PWM(long pwm_frequency, const int pin1A, const int pin1B, const // e.g. attach all the timers, start them, and then start the clock... but this would require API-changes in SimpleFOC... configureSAMDClock(); + if (pwm_frequency==NOT_SET) { + // use default frequency + pwm_frequency = SIMPLEFOC_SAMD_DEFAULT_PWM_FREQUENCY_HZ; + } + // 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); + getTccPinConfiguration(pin1A)->pwm_res = tccConfs[0].pwm_res; + getTccPinConfiguration(pin2A)->pwm_res = tccConfs[1].pwm_res; + getTccPinConfiguration(pin1B)->pwm_res = tccConfs[2].pwm_res; + getTccPinConfiguration(pin2B)->pwm_res = tccConfs[3].pwm_res; #ifdef SIMPLEFOC_SAMD_DEBUG SIMPLEFOC_SAMD_DEBUG_SERIAL.println("Configured TCCs..."); #endif @@ -579,6 +603,11 @@ int _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const // e.g. attach all the timers, start them, and then start the clock... but this would require API changes in SimpleFOC driver API configureSAMDClock(); + if (pwm_frequency==NOT_SET) { + // use default frequency + pwm_frequency = SIMPLEFOC_SAMD_DEFAULT_PWM_FREQUENCY_HZ; + } + // 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)) @@ -589,6 +618,12 @@ int _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const 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); + getTccPinConfiguration(pinA_h)->pwm_res = pinAh.pwm_res; + getTccPinConfiguration(pinA_l)->pwm_res = pinAh.pwm_res; // use the high phase resolution, in case we didn't set it + getTccPinConfiguration(pinB_h)->pwm_res = pinBh.pwm_res; + getTccPinConfiguration(pinB_l)->pwm_res = pinBh.pwm_res; + getTccPinConfiguration(pinC_h)->pwm_res = pinCh.pwm_res; + getTccPinConfiguration(pinC_l)->pwm_res = pinCh.pwm_res; #ifdef SIMPLEFOC_SAMD_DEBUG SIMPLEFOC_SAMD_DEBUG_SERIAL.println("Configured TCCs..."); #endif @@ -611,9 +646,9 @@ int _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const */ void _writeDutyCycle2PWM(float dc_a, float dc_b, int pinA, int pinB) { tccConfiguration* tccI = getTccPinConfiguration(pinA); - writeSAMDDutyCycle(tccI->tcc.chaninfo, dc_a); + writeSAMDDutyCycle(tccI, dc_a); tccI = getTccPinConfiguration(pinB); - writeSAMDDutyCycle(tccI->tcc.chaninfo, dc_b); + writeSAMDDutyCycle(tccI, dc_b); return; } @@ -635,11 +670,11 @@ void _writeDutyCycle2PWM(float dc_a, float dc_b, int pinA, int pinB) { */ 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); + writeSAMDDutyCycle(tccI, dc_a); tccI = getTccPinConfiguration(pinB); - writeSAMDDutyCycle(tccI->tcc.chaninfo, dc_b); + writeSAMDDutyCycle(tccI, dc_b); tccI = getTccPinConfiguration(pinC); - writeSAMDDutyCycle(tccI->tcc.chaninfo, dc_c); + writeSAMDDutyCycle(tccI, dc_c); return; } @@ -662,13 +697,13 @@ void _writeDutyCycle3PWM(float dc_a, float dc_b, float dc_c, int pinA, int pinB */ 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); + writeSAMDDutyCycle(tccI, dc_1a); tccI = getTccPinConfiguration(pin2A); - writeSAMDDutyCycle(tccI->tcc.chaninfo, dc_2a); + writeSAMDDutyCycle(tccI, dc_2a); tccI = getTccPinConfiguration(pin1B); - writeSAMDDutyCycle(tccI->tcc.chaninfo, dc_1b); + writeSAMDDutyCycle(tccI, dc_1b); tccI = getTccPinConfiguration(pin2B); - writeSAMDDutyCycle(tccI->tcc.chaninfo, dc_2b); + writeSAMDDutyCycle(tccI, dc_2b); return; } @@ -705,33 +740,33 @@ void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, float dead_zone, i // 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.0f; // no off-time is better than too-short dead-time - writeSAMDDutyCycle(tcc1->tcc.chaninfo, dc_a); - writeSAMDDutyCycle(tcc2->tcc.chaninfo, ls); + writeSAMDDutyCycle(tcc1, dc_a); + writeSAMDDutyCycle(tcc2, ls); } else - writeSAMDDutyCycle(tcc1->tcc.chaninfo, dc_a); // dead-time is done is hardware, no need to set low side pin explicitly + writeSAMDDutyCycle(tcc1, 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.0f; // no off-time is better than too-short dead-time - writeSAMDDutyCycle(tcc1->tcc.chaninfo, dc_b); - writeSAMDDutyCycle(tcc2->tcc.chaninfo, ls); + writeSAMDDutyCycle(tcc1, dc_b); + writeSAMDDutyCycle(tcc2, ls); } else - writeSAMDDutyCycle(tcc1->tcc.chaninfo, dc_b); + writeSAMDDutyCycle(tcc1, 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.0f; // no off-time is better than too-short dead-time - writeSAMDDutyCycle(tcc1->tcc.chaninfo, dc_c); - writeSAMDDutyCycle(tcc2->tcc.chaninfo, ls); + writeSAMDDutyCycle(tcc1, dc_c); + writeSAMDDutyCycle(tcc2, ls); } else - writeSAMDDutyCycle(tcc1->tcc.chaninfo, dc_c); + writeSAMDDutyCycle(tcc1, dc_c); return; } diff --git a/src/drivers/hardware_specific/samd_mcu.h b/src/drivers/hardware_specific/samd_mcu.h index 7faf7442..8fafeaaa 100644 --- a/src/drivers/hardware_specific/samd_mcu.h +++ b/src/drivers/hardware_specific/samd_mcu.h @@ -29,9 +29,17 @@ #ifndef SIMPLEFOC_SAMD_PWM_RESOLUTION #define SIMPLEFOC_SAMD_PWM_RESOLUTION 1000 -#define SIMPLEFOC_SAMD_PWM_TC_RESOLUTION 250 #endif +#define SIMPLEFOC_SAMD_DEFAULT_PWM_FREQUENCY_HZ 24000 +// arbitrary maximum. On SAMD51 with 120MHz clock this means 2kHz minimum pwm frequency +#define SIMPLEFOC_SAMD_MAX_PWM_RESOLUTION 30000 +// lets not go too low - 400 with clock speed of 120MHz on SAMD51 means 150kHz maximum PWM frequency... +// 400 with 48MHz clock on SAMD21 means 60kHz maximum PWM frequency... +#define SIMPLEFOC_SAMD_MIN_PWM_RESOLUTION 400 +// this is the most we can support on the TC units +#define SIMPLEFOC_SAMD_PWM_TC_RESOLUTION 250 + #ifndef SIMPLEFOC_SAMD_MAX_TCC_PINCONFIGURATIONS #define SIMPLEFOC_SAMD_MAX_TCC_PINCONFIGURATIONS 24 #endif @@ -49,6 +57,7 @@ struct tccConfiguration { }; uint16_t chaninfo; } tcc; + uint16_t pwm_res; }; @@ -92,7 +101,7 @@ 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 writeSAMDDutyCycle(tccConfiguration* info, 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)); From 64de2e4669e7ce18374d133291655068be0f0da9 Mon Sep 17 00:00:00 2001 From: Richard Unger Date: Wed, 10 Nov 2021 22:20:28 +0100 Subject: [PATCH 2/2] SAMD driver - pwm freq settable also on SAMD21 --- src/drivers/hardware_specific/samd21_mcu.cpp | 29 ++++++++++++++------ 1 file changed, 20 insertions(+), 9 deletions(-) diff --git a/src/drivers/hardware_specific/samd21_mcu.cpp b/src/drivers/hardware_specific/samd21_mcu.cpp index 9be2eee0..e4fb64b7 100644 --- a/src/drivers/hardware_specific/samd21_mcu.cpp +++ b/src/drivers/hardware_specific/samd21_mcu.cpp @@ -178,6 +178,15 @@ void configureSAMDClock() { * faster won't be possible without sacrificing resolution. */ void configureTCC(tccConfiguration& tccConfig, long pwm_frequency, bool negate, float hw6pwm) { + + long pwm_resolution = (24000000) / pwm_frequency; + if (pwm_resolution>SIMPLEFOC_SAMD_MAX_PWM_RESOLUTION) + pwm_resolution = SIMPLEFOC_SAMD_MAX_PWM_RESOLUTION; + if (pwm_resolution0.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); + tcc->WEXCTRL.bit.DTLS = hw6pwm*(pwm_resolution-1); + tcc->WEXCTRL.bit.DTHS = hw6pwm*(pwm_resolution-1); syncTCC(tcc); // wait for sync } - tcc->PER.reg = SIMPLEFOC_SAMD_PWM_RESOLUTION - 1; // Set counter Top using the PER register + tcc->PER.reg = pwm_resolution - 1; // Set counter Top using the PER register while ( tcc->SYNCBUSY.bit.PER == 1 ); // wait for sync // set all channels to 0% @@ -262,7 +271,8 @@ void configureTCC(tccConfiguration& tccConfig, long pwm_frequency, bool negate, SIMPLEFOC_SAMD_DEBUG_SERIAL.print(tccConfig.tcc.chan); SIMPLEFOC_SAMD_DEBUG_SERIAL.print("["); SIMPLEFOC_SAMD_DEBUG_SERIAL.print(tccConfig.wo); - SIMPLEFOC_SAMD_DEBUG_SERIAL.println("]"); + SIMPLEFOC_SAMD_DEBUG_SERIAL.print("] pwm res "); + SIMPLEFOC_SAMD_DEBUG_SERIAL.println(pwm_resolution); #endif } } @@ -280,8 +290,8 @@ void configureTCC(tccConfiguration& tccConfig, long pwm_frequency, bool negate, 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); + tcc->WEXCTRL.bit.DTLS = hw6pwm*(pwm_resolution-1); + tcc->WEXCTRL.bit.DTHS = hw6pwm*(pwm_resolution-1); syncTCC(tcc); // wait for sync } @@ -295,7 +305,8 @@ void configureTCC(tccConfiguration& tccConfig, long pwm_frequency, bool negate, SIMPLEFOC_SAMD_DEBUG_SERIAL.print(tccConfig.tcc.chan); SIMPLEFOC_SAMD_DEBUG_SERIAL.print("["); SIMPLEFOC_SAMD_DEBUG_SERIAL.print(tccConfig.wo); - SIMPLEFOC_SAMD_DEBUG_SERIAL.println("]"); + SIMPLEFOC_SAMD_DEBUG_SERIAL.print("] pwm res "); + SIMPLEFOC_SAMD_DEBUG_SERIAL.println(pwm_resolution); #endif } @@ -316,8 +327,8 @@ void writeSAMDDutyCycle(tccConfiguration* info, float dc) { // uint32_t chanbit = 0x1<<(TCC_SYNCBUSY_CC0_Pos+chan); // while ( (tcc->SYNCBUSY.reg & chanbit) > 0 ); // set via CCB - while ( (tcc->SYNCBUSY.vec.CC & (0x1< 0 ); - tcc->CCB[chan].reg = (uint32_t)((SIMPLEFOC_SAMD_PWM_RESOLUTION-1) * dc); + //while ( (tcc->SYNCBUSY.vec.CC & (0x1< 0 ); + tcc->CCB[chan].reg = (uint32_t)((info->pwm_res-1) * dc); // while ( (tcc->SYNCBUSY.vec.CCB & (0x1< 0 ); // tcc->STATUS.vec.CCBV |= (0x1<SYNCBUSY.bit.STATUS > 0 );