From b34bd626526acc0c66b8698ca46e15d17dead021 Mon Sep 17 00:00:00 2001 From: askuric Date: Sat, 3 Feb 2024 16:41:25 +0100 Subject: [PATCH 1/9] initial support for teensy4 low-side current sensing (only for 6pwm at the motment) - in progress --- .../hardware_specific/teensy/teensy4_mcu.cpp | 191 ++++++++++++++++++ .../hardware_specific/teensy/teensy4_mcu.h | 77 +++++++ .../{ => teensy}/teensy_mcu.cpp | 2 +- .../hardware_specific/teensy/teensy4_mcu.cpp | 14 +- 4 files changed, 273 insertions(+), 11 deletions(-) create mode 100644 src/current_sense/hardware_specific/teensy/teensy4_mcu.cpp create mode 100644 src/current_sense/hardware_specific/teensy/teensy4_mcu.h rename src/current_sense/hardware_specific/{ => teensy}/teensy_mcu.cpp (94%) diff --git a/src/current_sense/hardware_specific/teensy/teensy4_mcu.cpp b/src/current_sense/hardware_specific/teensy/teensy4_mcu.cpp new file mode 100644 index 00000000..8f2b441c --- /dev/null +++ b/src/current_sense/hardware_specific/teensy/teensy4_mcu.cpp @@ -0,0 +1,191 @@ +#include "teensy4_mcu.h" +#include "../../../drivers/hardware_specific/teensy/teensy4_mcu.h" + +// if defined +// - Teensy 4.0 +// - Teensy 4.1 +#if defined(__arm__) && defined(CORE_TEENSY) && ( defined(__IMXRT1062__) || defined(ARDUINO_TEENSY40) || defined(ARDUINO_TEENSY41) || defined(ARDUINO_TEENSY_MICROMOD) ) + +// function finding the TRIG event given the flexpwm timer and the submodule +// returning -1 if the submodule is not valid or no trigger is available +// allowing flexpwm1-4 and submodule 0-3 +// +// the flags are defined in the imxrt.h file +// https://github.com/PaulStoffregen/cores/blob/dd6aa8419ee173a0a6593eab669fbff54ed85f48/teensy4/imxrt.h#L9662 +int flextim__submodule_to_trig(IMXRT_FLEXPWM_t* flexpwm, int submodule){ + if(submodule <0 && submodule > 3) return -1; + if(flexpwm == &IMXRT_FLEXPWM1){ + return XBARA1_IN_FLEXPWM1_PWM1_OUT_TRIG0 + submodule; + }else if(flexpwm == &IMXRT_FLEXPWM2){ + return XBARA1_IN_FLEXPWM2_PWM1_OUT_TRIG0 + submodule; + }else if(flexpwm == &IMXRT_FLEXPWM3){ + return XBARA1_IN_FLEXPWM3_PWM1_OUT_TRIG0 + submodule; + }else if(flexpwm == &IMXRT_FLEXPWM4){ + return XBARA1_IN_FLEXPWM4_PWM1_OUT_TRIG0 + submodule; + } + return -1; +} + +volatile uint32_t val0, val1; + +void read_currents(uint32_t *a, uint32_t*b){ + *a = val0; + *b = val1; +} + +// interrupt service routine for the ADC_ETC0 +// reading the ADC values and clearing the interrupt +void adcetc0_isr() { + digitalWrite(30,HIGH); + ADC_ETC_DONE0_1_IRQ |= 1; // clear + val0 = ADC_ETC_TRIG0_RESULT_1_0 & 4095; + val1 = (ADC_ETC_TRIG0_RESULT_1_0 >> 16) & 4095; + asm("dsb"); + digitalWrite(30,LOW); +} + +// function initializing the ADC2 +// and the ADC_ETC trigger for the low side current sensing +void adc1_init() { + //Tried many configurations, but this seems to be best: + ADC1_CFG = ADC_CFG_OVWREN //Allow overwriting of the next converted Data onto the existing + | ADC_CFG_ADICLK(0) // input clock select - IPG clock + | ADC_CFG_MODE(2) // 12-bit conversion 0 8-bit conversion 1 10-bit conversion 2 12-bit conversion + | ADC_CFG_ADIV(2) // Input clock / 4 + | ADC_CFG_ADSTS(0) // Sample period (ADC clocks) = 3 if ADLSMP=0b + | ADC_CFG_ADHSC // High speed operation + | ADC_CFG_ADTRG; // Hardware trigger selected + + + //Calibration of ADC1 + ADC1_GC |= ADC_GC_CAL; // begin cal ADC1 + while (ADC1_GC & ADC_GC_CAL) ; + + ADC1_HC0 = 16; // ADC_ETC channel + // use the second interrupt if necessary (for more than 2 channels) + // ADC1_HC1 = 16; +} + +// function initializing the ADC2 +// and the ADC_ETC trigger for the low side current sensing +void adc2_init(){ + + // configuring ADC2 + //Tried many configurations, but this seems to be best: + ADC1_CFG = ADC_CFG_OVWREN //Allow overwriting of the next converted Data onto the existing + | ADC_CFG_ADICLK(0) // input clock select - IPG clock + | ADC_CFG_MODE(2) // 12-bit conversion 0 8-bit conversion 1 10-bit conversion 2 12-bit conversion + | ADC_CFG_ADIV(2) // Input clock / 4 + | ADC_CFG_ADSTS(0) // Sample period (ADC clocks) = 3 if ADLSMP=0b + | ADC_CFG_ADHSC // High speed operation + | ADC_CFG_ADTRG; // Hardware trigger selected + + //Calibration of ADC2 + ADC2_GC |= ADC_GC_CAL; // begin cal ADC2 + while (ADC2_GC & ADC_GC_CAL) ; + + ADC2_HC0 = 16; // ADC_ETC channel + // use the second interrupt if necessary (for more than 2 channels) + // ADC2_HC1 = 16; +} + +void adc_etc_init(int pin1, int pin2) { + ADC_ETC_CTRL &= ~(1 << 31); // SOFTRST + ADC_ETC_CTRL = 0x40000001; // start with trigger 0 + ADC_ETC_TRIG0_CTRL = 0x100; // chainlength -1 + + // ADC1 7 8, chain channel, HWTS, IE, B2B + // pg 3516, section 66.5.1.8 + ADC_ETC_TRIG0_CHAIN_1_0 = + ADC_ETC_TRIG_CHAIN_IE1(0) | // no interrupt on first or set 2 if interrupt when Done1 + ADC_ETC_TRIG_CHAIN_B2B1 | // Enable B2B, back to back ADC trigger + ADC_ETC_TRIG_CHAIN_HWTS1(1) | + ADC_ETC_TRIG_CHAIN_CSEL1(pin_to_channel[pin1]) | // ADC channel 8 + ADC_ETC_TRIG_CHAIN_IE0(1) | // interrupt when Done0 + ADC_ETC_TRIG_CHAIN_B2B1 | // Enable B2B, back to back ADC trigger + ADC_ETC_TRIG_CHAIN_HWTS0(1) | + ADC_ETC_TRIG_CHAIN_CSEL0(pin_to_channel[pin2]); // ADC channel 7 + + attachInterruptVector(IRQ_ADC_ETC0, adcetc0_isr); + NVIC_ENABLE_IRQ(IRQ_ADC_ETC0); + // use the second interrupt if necessary (for more than 2 channels) + // attachInterruptVector(IRQ_ADC_ETC1, adcetc1_isr); + // NVIC_ENABLE_IRQ(IRQ_ADC_ETC1); +} + + +void xbar_connect(unsigned int input, unsigned int output) +{ + if (input >= 88) return; + if (output >= 132) return; + volatile uint16_t *xbar = &XBARA1_SEL0 + (output / 2); + uint16_t val = *xbar; + if (!(output & 1)) { + val = (val & 0xFF00) | input; + } else { + val = (val & 0x00FF) | (input << 8); + } + *xbar = val; +} +void xbar_init() { + CCM_CCGR2 |= CCM_CCGR2_XBAR1(CCM_CCGR_ON); //turn clock on for xbara1 +} + + +// function reading an ADC value and returning the read voltage +float _readADCVoltageLowSide(const int pinA, const void* cs_params){ + + GenericCurrentSenseParams* params = (GenericCurrentSenseParams*) cs_params; + float adc_voltage_conv = params->adc_voltage_conv; + if (pinA == params->pins[0]) { + return val0 * adc_voltage_conv; + } else if (pinA == params->pins[1]) { + return val1 * adc_voltage_conv; + } + return 0.0; +} + +// Configure low side for generic mcu +// cannot do much but +void* _configureADCLowSide(const void* driver_params, const int pinA,const int pinB,const int pinC){ + _UNUSED(driver_params); + + pinMode(30,OUTPUT); + + if( _isset(pinA) ) pinMode(pinA, INPUT); + if( _isset(pinB) ) pinMode(pinB, INPUT); + if( _isset(pinC) ) pinMode(pinC, INPUT); + + adc1_init(); + adc_etc_init(pinA, pinB); + xbar_init(); + GenericCurrentSenseParams* params = new GenericCurrentSenseParams { + .pins = { pinA, pinB, pinC }, + .adc_voltage_conv = (_ADC_VOLTAGE)/(_ADC_RESOLUTION) + }; + return params; +} + +// sync driver and the adc +void _driverSyncLowSide(void* driver_params, void* cs_params){ + Teensy4DriverParams* par = (Teensy4DriverParams*) driver_params; + IMXRT_FLEXPWM_t* flexpwm = par->flextimers[0]; + int submodule = par->submodules[0]; + + // do xbar connect here + + int xbar_trig_pwm = flextim__submodule_to_trig(flexpwm, submodule); + if(xbar_trig_pwm<0) return; + + xbar_connect((uint32_t)xbar_trig_pwm, XBARA1_OUT_ADC_ETC_TRIG00); //FlexPWM to adc_etc + + // setup the ADC_ETC trigger + flexpwm->SM[submodule].TCTRL = FLEXPWM_SMTCTRL_OUT_TRIG_EN(1<<4); + // setup this val4 for interrupt on val5 match for ADC sync + // reading two ARC takes about 5us. So put the interrupt 2.5us befor the center + flexpwm->SM[submodule].VAL4 = -int(2.5e-6*par->pwm_frequency*flexpwm->SM[submodule].VAL1) ; // 2.5us before center + +} + + +#endif \ No newline at end of file diff --git a/src/current_sense/hardware_specific/teensy/teensy4_mcu.h b/src/current_sense/hardware_specific/teensy/teensy4_mcu.h new file mode 100644 index 00000000..2cf77dfb --- /dev/null +++ b/src/current_sense/hardware_specific/teensy/teensy4_mcu.h @@ -0,0 +1,77 @@ + +#ifndef TEENSY4_CURRENTSENSE_MCU_DEF +#define TEENSY4_CURRENTSENSE_MCU_DEF + +#include "../../hardware_api.h" +#include "../../../common/foc_utils.h" + +// if defined +// - Teensy 4.0 +// - Teensy 4.1 +#if defined(__arm__) && defined(CORE_TEENSY) && ( defined(__IMXRT1062__) || defined(ARDUINO_TEENSY40) || defined(ARDUINO_TEENSY41) || defined(ARDUINO_TEENSY_MICROMOD) ) + +#define _ADC_VOLTAGE 3.3f +#define _ADC_RESOLUTION 4026.0f + +// generic implementation of the hardware specific structure +// containing all the necessary current sense parameters +// will be returned as a void pointer from the _configureADCx functions +// will be provided to the _readADCVoltageX() as a void pointer +typedef struct Teensy4CurrentSenseParams { + int pins[3] = {(int)NOT_SET}; + float adc_voltage_conv; +} Teensy4CurrentSenseParams; + + + +const uint8_t pin_to_channel[] = { // pg 482 + 7, // 0/A0 AD_B1_02 + 8, // 1/A1 AD_B1_03 + 12, // 2/A2 AD_B1_07 + 11, // 3/A3 AD_B1_06 + 6, // 4/A4 AD_B1_01 + 5, // 5/A5 AD_B1_00 + 15, // 6/A6 AD_B1_10 + 0, // 7/A7 AD_B1_11 + 13, // 8/A8 AD_B1_08 + 14, // 9/A9 AD_B1_09 + 1, // 24/A10 AD_B0_12 + 2, // 25/A11 AD_B0_13 + 128+3, // 26/A12 AD_B1_14 - only on ADC2, 3 + 128+4, // 27/A13 AD_B1_15 - only on ADC2, 4 + 7, // 14/A0 AD_B1_02 + 8, // 15/A1 AD_B1_03 + 12, // 16/A2 AD_B1_07 + 11, // 17/A3 AD_B1_06 + 6, // 18/A4 AD_B1_01 + 5, // 19/A5 AD_B1_00 + 15, // 20/A6 AD_B1_10 + 0, // 21/A7 AD_B1_11 + 13, // 22/A8 AD_B1_08 + 14, // 23/A9 AD_B1_09 + 1, // 24/A10 AD_B0_12 + 2, // 25/A11 AD_B0_13 + 128+3, // 26/A12 AD_B1_14 - only on ADC2, 3 + 128+4, // 27/A13 AD_B1_15 - only on ADC2, 4 +#ifdef ARDUINO_TEENSY41 + 255, // 28 + 255, // 29 + 255, // 30 + 255, // 31 + 255, // 32 + 255, // 33 + 255, // 34 + 255, // 35 + 255, // 36 + 255, // 37 + 128+1, // 38/A14 AD_B1_12 - only on ADC2, 1 + 128+2, // 39/A15 AD_B1_13 - only on ADC2, 2 + 9, // 40/A16 AD_B1_04 + 10, // 41/A17 AD_B1_05 +#endif +}; + + +#endif + +#endif \ No newline at end of file diff --git a/src/current_sense/hardware_specific/teensy_mcu.cpp b/src/current_sense/hardware_specific/teensy/teensy_mcu.cpp similarity index 94% rename from src/current_sense/hardware_specific/teensy_mcu.cpp rename to src/current_sense/hardware_specific/teensy/teensy_mcu.cpp index 7ab370a4..7669edc8 100644 --- a/src/current_sense/hardware_specific/teensy_mcu.cpp +++ b/src/current_sense/hardware_specific/teensy/teensy_mcu.cpp @@ -1,4 +1,4 @@ -#include "../hardware_api.h" +#include "../../hardware_api.h" #if defined(__arm__) && defined(CORE_TEENSY) diff --git a/src/drivers/hardware_specific/teensy/teensy4_mcu.cpp b/src/drivers/hardware_specific/teensy/teensy4_mcu.cpp index 322d5a34..d3738aed 100644 --- a/src/drivers/hardware_specific/teensy/teensy4_mcu.cpp +++ b/src/drivers/hardware_specific/teensy/teensy4_mcu.cpp @@ -177,18 +177,15 @@ void setup_pwm_pair (IMXRT_FLEXPWM_t * flexpwm, int submodule, const long freque FLEXPWM_SMCTRL2_FRCEN | FLEXPWM_SMCTRL2_INIT_SEL(0) | FLEXPWM_SMCTRL2_FORCE_SEL(6); flexpwm->SM[submodule].CTRL = FLEXPWM_SMCTRL_FULL | FLEXPWM_SMCTRL_HALF | FLEXPWM_SMCTRL_PRSC(prescale) ; // https://github.com/PaulStoffregen/cores/blob/70ba01accd728abe75ebfc8dcd8b3d3a8f3e3f25/teensy4/imxrt.h#L4948 - flexpwm->SM[submodule].OCTRL = 0;//channel_to_invert==2 ? 0 : FLEXPWM_SMOCTRL_POLA | FLEXPWM_SMOCTRL_POLB ; + flexpwm->SM[submodule].OCTRL = 0; //FLEXPWM_SMOCTRL_POLA | FLEXPWM_SMOCTRL_POLB;//channel_to_invert==2 ? 0 : FLEXPWM_SMOCTRL_POLA | FLEXPWM_SMOCTRL_POLB ; flexpwm->SM[submodule].DTCNT0 = dead_time ; // should try this out (deadtime control) flexpwm->SM[submodule].DTCNT1 = dead_time ; // should try this out (deadtime control) - // flexpwm->SM[submodule].TCTRL = FLEXPWM_SMTCTRL_OUT_TRIG_EN (0b000010) ; // sync trig out on VAL1 match. flexpwm->SM[submodule].INIT = -half_cycle; // count from -HALFCYCLE to +HALFCYCLE - flexpwm->SM[submodule].VAL0 = 0 ; + flexpwm->SM[submodule].VAL0 = 0; flexpwm->SM[submodule].VAL1 = half_cycle ; flexpwm->SM[submodule].VAL2 = -mid_pwm ; flexpwm->SM[submodule].VAL3 = +mid_pwm ; - // flexpwm->SM[submodule].VAL4 = -mid_pwm ; - // flexpwm->SM[submodule].VAL5 = +mid_pwm ; - + flexpwm->MCTRL |= FLEXPWM_MCTRL_LDOK (submodule_mask) ; // loading reenabled flexpwm->MCTRL |= FLEXPWM_MCTRL_RUN (submodule_mask) ; // start it running } @@ -207,14 +204,11 @@ void startup_pwm_pair (IMXRT_FLEXPWM_t * flexpwm, int submodule) // PWM setting on the high and low pair of the PWM channels void write_pwm_pair(IMXRT_FLEXPWM_t * flexpwm, int submodule, float duty){ - int mid_pwm = int((half_cycle)/2.0f); int count_pwm = int(mid_pwm*(duty*2-1)) + mid_pwm; flexpwm->SM[submodule].VAL2 = count_pwm; // A on flexpwm->SM[submodule].VAL3 = -count_pwm ; // A off - // flexpwm->SM[submodule].VAL4 = - count_pwm ; // B off (assuming B inverted) - // flexpwm->SM[submodule].VAL5 = + count_pwm ; // B on flexpwm->MCTRL |= FLEXPWM_MCTRL_LDOK (1< Date: Sun, 11 Feb 2024 19:47:10 +0100 Subject: [PATCH 2/9] added support for more than 2/3 adc pins --- .../hardware_specific/teensy/teensy4_mcu.cpp | 89 ++++++++++++++----- 1 file changed, 67 insertions(+), 22 deletions(-) diff --git a/src/current_sense/hardware_specific/teensy/teensy4_mcu.cpp b/src/current_sense/hardware_specific/teensy/teensy4_mcu.cpp index 8f2b441c..eca862a1 100644 --- a/src/current_sense/hardware_specific/teensy/teensy4_mcu.cpp +++ b/src/current_sense/hardware_specific/teensy/teensy4_mcu.cpp @@ -1,5 +1,6 @@ #include "teensy4_mcu.h" #include "../../../drivers/hardware_specific/teensy/teensy4_mcu.h" +#include "../../../common/foc_utils.h" // if defined // - Teensy 4.0 @@ -12,7 +13,7 @@ // // the flags are defined in the imxrt.h file // https://github.com/PaulStoffregen/cores/blob/dd6aa8419ee173a0a6593eab669fbff54ed85f48/teensy4/imxrt.h#L9662 -int flextim__submodule_to_trig(IMXRT_FLEXPWM_t* flexpwm, int submodule){ +int flexpwm_submodule_to_trig(IMXRT_FLEXPWM_t* flexpwm, int submodule){ if(submodule <0 && submodule > 3) return -1; if(flexpwm == &IMXRT_FLEXPWM1){ return XBARA1_IN_FLEXPWM1_PWM1_OUT_TRIG0 + submodule; @@ -26,32 +27,45 @@ int flextim__submodule_to_trig(IMXRT_FLEXPWM_t* flexpwm, int submodule){ return -1; } -volatile uint32_t val0, val1; +volatile uint32_t val0, val1, val2; -void read_currents(uint32_t *a, uint32_t*b){ +void read_currents(uint32_t *a, uint32_t*b, uint32_t *c=nullptr){ *a = val0; *b = val1; + *c = val2; } // interrupt service routine for the ADC_ETC0 // reading the ADC values and clearing the interrupt void adcetc0_isr() { digitalWrite(30,HIGH); - ADC_ETC_DONE0_1_IRQ |= 1; // clear + // page 3509 , section 66.5.1.3.3 + ADC_ETC_DONE0_1_IRQ |= 1; // clear Done0 for trg0 at 1st bit val0 = ADC_ETC_TRIG0_RESULT_1_0 & 4095; val1 = (ADC_ETC_TRIG0_RESULT_1_0 >> 16) & 4095; asm("dsb"); digitalWrite(30,LOW); } + +void adcetc1_isr() { + digitalWrite(30,HIGH); + // page 3509 , section 66.5.1.3.3 + ADC_ETC_DONE0_1_IRQ |= 1 << 16; // clear Done1 for trg0 at 16th bit + val2 = ADC_ETC_TRIG0_RESULT_3_2 & 4095; + // val2 = (ADC_ETC_TRIG0_RESULT_3_2 >> 16) & 4095; + asm("dsb"); + digitalWrite(30,LOW); +} + // function initializing the ADC2 // and the ADC_ETC trigger for the low side current sensing -void adc1_init() { +void adc1_init(int pin1, int pin2, int pin3=NOT_SET) { //Tried many configurations, but this seems to be best: ADC1_CFG = ADC_CFG_OVWREN //Allow overwriting of the next converted Data onto the existing | ADC_CFG_ADICLK(0) // input clock select - IPG clock | ADC_CFG_MODE(2) // 12-bit conversion 0 8-bit conversion 1 10-bit conversion 2 12-bit conversion - | ADC_CFG_ADIV(2) // Input clock / 4 + | ADC_CFG_ADIV(1) // Input clock / 2 (0 for /1, 1 for /2 and 2 for / 4) | ADC_CFG_ADSTS(0) // Sample period (ADC clocks) = 3 if ADLSMP=0b | ADC_CFG_ADHSC // High speed operation | ADC_CFG_ADTRG; // Hardware trigger selected @@ -63,7 +77,9 @@ void adc1_init() { ADC1_HC0 = 16; // ADC_ETC channel // use the second interrupt if necessary (for more than 2 channels) - // ADC1_HC1 = 16; + if(_isset(pin3)) { + ADC1_HC1 = 16; + } } // function initializing the ADC2 @@ -75,7 +91,7 @@ void adc2_init(){ ADC1_CFG = ADC_CFG_OVWREN //Allow overwriting of the next converted Data onto the existing | ADC_CFG_ADICLK(0) // input clock select - IPG clock | ADC_CFG_MODE(2) // 12-bit conversion 0 8-bit conversion 1 10-bit conversion 2 12-bit conversion - | ADC_CFG_ADIV(2) // Input clock / 4 + | ADC_CFG_ADIV(1) // Input clock / 2 (0 for /1, 1 for /2 and 2 for / 4) | ADC_CFG_ADSTS(0) // Sample period (ADC clocks) = 3 if ADLSMP=0b | ADC_CFG_ADHSC // High speed operation | ADC_CFG_ADTRG; // Hardware trigger selected @@ -89,10 +105,13 @@ void adc2_init(){ // ADC2_HC1 = 16; } -void adc_etc_init(int pin1, int pin2) { +// function initializing the ADC_ETC trigger for the low side current sensing +// it uses only the ADC1 +// if the pin3 is not set it uses only 2 channels +void adc_etc_init(int pin1, int pin2, int pin3=NOT_SET) { ADC_ETC_CTRL &= ~(1 << 31); // SOFTRST ADC_ETC_CTRL = 0x40000001; // start with trigger 0 - ADC_ETC_TRIG0_CTRL = 0x100; // chainlength -1 + ADC_ETC_TRIG0_CTRL = ADC_ETC_TRIG_CTRL_TRIG_CHAIN( _isset(pin3) ? 2 : 1) ; // 2 if 3 channels, 1 if 2 channels // ADC1 7 8, chain channel, HWTS, IE, B2B // pg 3516, section 66.5.1.8 @@ -109,11 +128,18 @@ void adc_etc_init(int pin1, int pin2) { attachInterruptVector(IRQ_ADC_ETC0, adcetc0_isr); NVIC_ENABLE_IRQ(IRQ_ADC_ETC0); // use the second interrupt if necessary (for more than 2 channels) - // attachInterruptVector(IRQ_ADC_ETC1, adcetc1_isr); - // NVIC_ENABLE_IRQ(IRQ_ADC_ETC1); + if(_isset(pin3)) { + ADC_ETC_TRIG0_CHAIN_3_2 = + ADC_ETC_TRIG_CHAIN_IE0(2) | // interrupt when Done1 + ADC_ETC_TRIG_CHAIN_B2B0 | // Enable B2B, back to back ADC trigger + ADC_ETC_TRIG_CHAIN_HWTS0(1) | + ADC_ETC_TRIG_CHAIN_CSEL0(pin_to_channel[pin3]); + + attachInterruptVector(IRQ_ADC_ETC1, adcetc1_isr); + NVIC_ENABLE_IRQ(IRQ_ADC_ETC1); + } } - void xbar_connect(unsigned int input, unsigned int output) { if (input >= 88) return; @@ -135,12 +161,15 @@ void xbar_init() { // function reading an ADC value and returning the read voltage float _readADCVoltageLowSide(const int pinA, const void* cs_params){ + if(!_isset(pinA)) return 0.0; // if the pin is not set return 0 GenericCurrentSenseParams* params = (GenericCurrentSenseParams*) cs_params; float adc_voltage_conv = params->adc_voltage_conv; if (pinA == params->pins[0]) { return val0 * adc_voltage_conv; } else if (pinA == params->pins[1]) { return val1 * adc_voltage_conv; + }else if (pinA == params->pins[2]) { + return val2 * adc_voltage_conv; } return 0.0; } @@ -149,6 +178,7 @@ float _readADCVoltageLowSide(const int pinA, const void* cs_params){ // cannot do much but void* _configureADCLowSide(const void* driver_params, const int pinA,const int pinB,const int pinC){ _UNUSED(driver_params); + // _UNUSED(pinC); pinMode(30,OUTPUT); @@ -156,11 +186,25 @@ void* _configureADCLowSide(const void* driver_params, const int pinA,const int p if( _isset(pinB) ) pinMode(pinB, INPUT); if( _isset(pinC) ) pinMode(pinC, INPUT); - adc1_init(); - adc_etc_init(pinA, pinB); + // check if either of the pins are not set + // and dont use it if it isn't + int pin_count = 0; + int pins[3] = {NOT_SET, NOT_SET, NOT_SET}; + if(_isset(pinA)) pins[pin_count++] = pinA; + if(_isset(pinB)) pins[pin_count++] = pinB; + if(_isset(pinC)) pins[pin_count++] = pinC; + + + adc1_init(pins[0], pins[1], pins[2]); + SIMPLEFOC_DEBUG("pins: ",pins[0]); + SIMPLEFOC_DEBUG("pins: ",pins[1]); + SIMPLEFOC_DEBUG("pins: ",pins[2]); + adc_etc_init(pins[0], pins[1], pins[2]); + xbar_init(); + GenericCurrentSenseParams* params = new GenericCurrentSenseParams { - .pins = { pinA, pinB, pinC }, + .pins = {pins[0], pins[1], pins[2] }, .adc_voltage_conv = (_ADC_VOLTAGE)/(_ADC_RESOLUTION) }; return params; @@ -172,17 +216,18 @@ void _driverSyncLowSide(void* driver_params, void* cs_params){ IMXRT_FLEXPWM_t* flexpwm = par->flextimers[0]; int submodule = par->submodules[0]; - // do xbar connect here - - int xbar_trig_pwm = flextim__submodule_to_trig(flexpwm, submodule); + // find the xbar trigger for the flexpwm + int xbar_trig_pwm = flexpwm_submodule_to_trig(flexpwm, submodule); if(xbar_trig_pwm<0) return; + // allow theFlexPWM to trigger the ADC_ETC xbar_connect((uint32_t)xbar_trig_pwm, XBARA1_OUT_ADC_ETC_TRIG00); //FlexPWM to adc_etc - // setup the ADC_ETC trigger + // setup the ADC_ETC trigger to be triggered by the FlexPWM channel 4 (val4) flexpwm->SM[submodule].TCTRL = FLEXPWM_SMTCTRL_OUT_TRIG_EN(1<<4); - // setup this val4 for interrupt on val5 match for ADC sync - // reading two ARC takes about 5us. So put the interrupt 2.5us befor the center + // setup this val4 for interrupt on match for ADC sync + // this code assumes that the val4 is not used for anything else! + // reading two ADC takes about 2.5us. So put the interrupt 2.5us befor the center flexpwm->SM[submodule].VAL4 = -int(2.5e-6*par->pwm_frequency*flexpwm->SM[submodule].VAL1) ; // 2.5us before center } From 0588d3845188237acf070fc0a5cab50fa6f286d4 Mon Sep 17 00:00:00 2001 From: askuric Date: Mon, 26 Feb 2024 17:41:35 +0100 Subject: [PATCH 3/9] added teensy 4 6pwm tiemer sync and better support for low-side current sensing --- .../hardware_specific/teensy/teensy4_mcu.cpp | 94 +++++----- .../hardware_specific/teensy/teensy4_mcu.cpp | 164 ++++++++++++++++-- .../hardware_specific/teensy/teensy4_mcu.h | 11 ++ 3 files changed, 207 insertions(+), 62 deletions(-) diff --git a/src/current_sense/hardware_specific/teensy/teensy4_mcu.cpp b/src/current_sense/hardware_specific/teensy/teensy4_mcu.cpp index eca862a1..b76624bc 100644 --- a/src/current_sense/hardware_specific/teensy/teensy4_mcu.cpp +++ b/src/current_sense/hardware_specific/teensy/teensy4_mcu.cpp @@ -1,5 +1,6 @@ #include "teensy4_mcu.h" #include "../../../drivers/hardware_specific/teensy/teensy4_mcu.h" +// #include "../../../common/lowpass_filter.h" #include "../../../common/foc_utils.h" // if defined @@ -7,28 +8,16 @@ // - Teensy 4.1 #if defined(__arm__) && defined(CORE_TEENSY) && ( defined(__IMXRT1062__) || defined(ARDUINO_TEENSY40) || defined(ARDUINO_TEENSY41) || defined(ARDUINO_TEENSY_MICROMOD) ) -// function finding the TRIG event given the flexpwm timer and the submodule -// returning -1 if the submodule is not valid or no trigger is available -// allowing flexpwm1-4 and submodule 0-3 -// -// the flags are defined in the imxrt.h file -// https://github.com/PaulStoffregen/cores/blob/dd6aa8419ee173a0a6593eab669fbff54ed85f48/teensy4/imxrt.h#L9662 -int flexpwm_submodule_to_trig(IMXRT_FLEXPWM_t* flexpwm, int submodule){ - if(submodule <0 && submodule > 3) return -1; - if(flexpwm == &IMXRT_FLEXPWM1){ - return XBARA1_IN_FLEXPWM1_PWM1_OUT_TRIG0 + submodule; - }else if(flexpwm == &IMXRT_FLEXPWM2){ - return XBARA1_IN_FLEXPWM2_PWM1_OUT_TRIG0 + submodule; - }else if(flexpwm == &IMXRT_FLEXPWM3){ - return XBARA1_IN_FLEXPWM3_PWM1_OUT_TRIG0 + submodule; - }else if(flexpwm == &IMXRT_FLEXPWM4){ - return XBARA1_IN_FLEXPWM4_PWM1_OUT_TRIG0 + submodule; - } - return -1; -} +// #define TEENSY4_ADC_INTERRUPT_DEBUG + +// #define _BANDWIDTH_CS 10000.0f // [Hz] bandwidth for the current sense volatile uint32_t val0, val1, val2; +// LowPassFilter lp1 = LowPassFilter(1.0/_BANDWIDTH_CS); +// LowPassFilter lp2 = LowPassFilter(1.0/_BANDWIDTH_CS); +// LowPassFilter lp3 = LowPassFilter(1.0/_BANDWIDTH_CS); + void read_currents(uint32_t *a, uint32_t*b, uint32_t *c=nullptr){ *a = val0; *b = val1; @@ -38,24 +27,32 @@ void read_currents(uint32_t *a, uint32_t*b, uint32_t *c=nullptr){ // interrupt service routine for the ADC_ETC0 // reading the ADC values and clearing the interrupt void adcetc0_isr() { +#ifdef TEENSY4_ADC_INTERRUPT_DEBUG digitalWrite(30,HIGH); +#endif // page 3509 , section 66.5.1.3.3 ADC_ETC_DONE0_1_IRQ |= 1; // clear Done0 for trg0 at 1st bit - val0 = ADC_ETC_TRIG0_RESULT_1_0 & 4095; + // val0 = lp1(ADC_ETC_TRIG0_RESULT_1_0 & 4095); + val0 = (ADC_ETC_TRIG0_RESULT_1_0 & 4095); + // val1 = lp2((ADC_ETC_TRIG0_RESULT_1_0 >> 16) & 4095); val1 = (ADC_ETC_TRIG0_RESULT_1_0 >> 16) & 4095; - asm("dsb"); +#ifdef TEENSY4_ADC_INTERRUPT_DEBUG digitalWrite(30,LOW); +#endif } void adcetc1_isr() { - digitalWrite(30,HIGH); +#ifdef TEENSY4_ADC_INTERRUPT_DEBUG + digitalWrite(30,HIGH); +#endif // page 3509 , section 66.5.1.3.3 ADC_ETC_DONE0_1_IRQ |= 1 << 16; // clear Done1 for trg0 at 16th bit val2 = ADC_ETC_TRIG0_RESULT_3_2 & 4095; - // val2 = (ADC_ETC_TRIG0_RESULT_3_2 >> 16) & 4095; - asm("dsb"); - digitalWrite(30,LOW); +// val2 = lp3( ADC_ETC_TRIG0_RESULT_3_2 & 4095); +#ifdef TEENSY4_ADC_INTERRUPT_DEBUG + digitalWrite(30,LOW); +#endif } // function initializing the ADC2 @@ -65,7 +62,7 @@ void adc1_init(int pin1, int pin2, int pin3=NOT_SET) { ADC1_CFG = ADC_CFG_OVWREN //Allow overwriting of the next converted Data onto the existing | ADC_CFG_ADICLK(0) // input clock select - IPG clock | ADC_CFG_MODE(2) // 12-bit conversion 0 8-bit conversion 1 10-bit conversion 2 12-bit conversion - | ADC_CFG_ADIV(1) // Input clock / 2 (0 for /1, 1 for /2 and 2 for / 4) + | ADC_CFG_ADIV(2) // Input clock / 2 (0 for /1, 1 for /2 and 2 for / 4) (1 is faster and maybe with some filtering could provide better results but 2 for now) | ADC_CFG_ADSTS(0) // Sample period (ADC clocks) = 3 if ADLSMP=0b | ADC_CFG_ADHSC // High speed operation | ADC_CFG_ADTRG; // Hardware trigger selected @@ -91,7 +88,7 @@ void adc2_init(){ ADC1_CFG = ADC_CFG_OVWREN //Allow overwriting of the next converted Data onto the existing | ADC_CFG_ADICLK(0) // input clock select - IPG clock | ADC_CFG_MODE(2) // 12-bit conversion 0 8-bit conversion 1 10-bit conversion 2 12-bit conversion - | ADC_CFG_ADIV(1) // Input clock / 2 (0 for /1, 1 for /2 and 2 for / 4) + | ADC_CFG_ADIV(2) // Input clock / 2 (0 for /1, 1 for /2 and 2 for / 4) | ADC_CFG_ADSTS(0) // Sample period (ADC clocks) = 3 if ADLSMP=0b | ADC_CFG_ADHSC // High speed operation | ADC_CFG_ADTRG; // Hardware trigger selected @@ -140,22 +137,6 @@ void adc_etc_init(int pin1, int pin2, int pin3=NOT_SET) { } } -void xbar_connect(unsigned int input, unsigned int output) -{ - if (input >= 88) return; - if (output >= 132) return; - volatile uint16_t *xbar = &XBARA1_SEL0 + (output / 2); - uint16_t val = *xbar; - if (!(output & 1)) { - val = (val & 0xFF00) | input; - } else { - val = (val & 0x00FF) | (input << 8); - } - *xbar = val; -} -void xbar_init() { - CCM_CCGR2 |= CCM_CCGR2_XBAR1(CCM_CCGR_ON); //turn clock on for xbara1 -} // function reading an ADC value and returning the read voltage @@ -178,9 +159,10 @@ float _readADCVoltageLowSide(const int pinA, const void* cs_params){ // cannot do much but void* _configureADCLowSide(const void* driver_params, const int pinA,const int pinB,const int pinC){ _UNUSED(driver_params); - // _UNUSED(pinC); +#ifdef TEENSY4_ADC_INTERRUPT_DEBUG pinMode(30,OUTPUT); +#endif if( _isset(pinA) ) pinMode(pinA, INPUT); if( _isset(pinB) ) pinMode(pinB, INPUT); @@ -196,9 +178,6 @@ void* _configureADCLowSide(const void* driver_params, const int pinA,const int p adc1_init(pins[0], pins[1], pins[2]); - SIMPLEFOC_DEBUG("pins: ",pins[0]); - SIMPLEFOC_DEBUG("pins: ",pins[1]); - SIMPLEFOC_DEBUG("pins: ",pins[2]); adc_etc_init(pins[0], pins[1], pins[2]); xbar_init(); @@ -223,12 +202,29 @@ void _driverSyncLowSide(void* driver_params, void* cs_params){ // allow theFlexPWM to trigger the ADC_ETC xbar_connect((uint32_t)xbar_trig_pwm, XBARA1_OUT_ADC_ETC_TRIG00); //FlexPWM to adc_etc + // setup the ADC_ETC trigger to be triggered by the FlexPWM channel 1 (val1) + //This val1 interrupt on match is in the center of the PWM + flexpwm->SM[submodule].TCTRL = FLEXPWM_SMTCTRL_OUT_TRIG_EN(1<<1); + + + // if needed the interrupt can be moved to some other point in the PWM cycle by using an addional val register example: VAL4 // setup the ADC_ETC trigger to be triggered by the FlexPWM channel 4 (val4) - flexpwm->SM[submodule].TCTRL = FLEXPWM_SMTCTRL_OUT_TRIG_EN(1<<4); + // flexpwm->SM[submodule].TCTRL = FLEXPWM_SMTCTRL_OUT_TRIG_EN(1<<4); // setup this val4 for interrupt on match for ADC sync // this code assumes that the val4 is not used for anything else! // reading two ADC takes about 2.5us. So put the interrupt 2.5us befor the center - flexpwm->SM[submodule].VAL4 = -int(2.5e-6*par->pwm_frequency*flexpwm->SM[submodule].VAL1) ; // 2.5us before center + // flexpwm->SM[submodule].VAL4 = int(flexpwm->SM[submodule].VAL1*(1.0f - 2.5e-6*par->pwm_frequency)) ; // 2.5us before center + + +#ifdef TEENSY4_ADC_INTERRUPT_DEBUG + // pin 4 observes out trigger line for 'scope + xbar_connect (xbar_trig_pwm, XBARA1_OUT_IOMUX_XBAR_INOUT08) ; + IOMUXC_GPR_GPR6 |= IOMUXC_GPR_GPR6_IOMUXC_XBAR_DIR_SEL_8 ; // select output mode for INOUT8 + // Select alt 3 for EMC_06 (XBAR), rather than original 5 (GPIO) + CORE_PIN4_CONFIG = 3 ; // shorthand for IOMUXC_SW_MUX_CTL_PAD_GPIO_EMC_06 = 3 ; + // turn up drive & speed as very short pulse + IOMUXC_SW_PAD_CTL_PAD_GPIO_EMC_06 = IOMUXC_PAD_DSE(7) | IOMUXC_PAD_SPEED(3) | IOMUXC_PAD_SRE ; +#endif } diff --git a/src/drivers/hardware_specific/teensy/teensy4_mcu.cpp b/src/drivers/hardware_specific/teensy/teensy4_mcu.cpp index d3738aed..bf81e246 100644 --- a/src/drivers/hardware_specific/teensy/teensy4_mcu.cpp +++ b/src/drivers/hardware_specific/teensy/teensy4_mcu.cpp @@ -13,10 +13,135 @@ #pragma message("") -// half_cycle of the PWM variable -int half_cycle = 0; + +// function finding the TRIG event given the flexpwm timer and the submodule +// returning -1 if the submodule is not valid or no trigger is available +// allowing flexpwm1-4 and submodule 0-3 +// +// the flags are defined in the imxrt.h file +// https://github.com/PaulStoffregen/cores/blob/dd6aa8419ee173a0a6593eab669fbff54ed85f48/teensy4/imxrt.h#L9662-L9693 +int flexpwm_submodule_to_trig(IMXRT_FLEXPWM_t* flexpwm, int submodule){ + if(submodule <0 && submodule > 3) return -1; + if(flexpwm == &IMXRT_FLEXPWM1){ + return XBARA1_IN_FLEXPWM1_PWM1_OUT_TRIG0 + submodule; + }else if(flexpwm == &IMXRT_FLEXPWM2){ + return XBARA1_IN_FLEXPWM2_PWM1_OUT_TRIG0 + submodule; + }else if(flexpwm == &IMXRT_FLEXPWM3){ + return XBARA1_IN_FLEXPWM3_PWM1_OUT_TRIG0 + submodule; + }else if(flexpwm == &IMXRT_FLEXPWM4){ + return XBARA1_IN_FLEXPWM4_PWM1_OUT_TRIG0 + submodule; + } + return -1; +} + +// function finding the EXT_SYNC event given the flexpwm timer and the submodule +// returning -1 if the submodule is not valid or no trigger is available +// allowing flexpwm1-4 and submodule 0-3 +// +// the flags are defined in the imxrt.h file +// https://github.com/PaulStoffregen/cores/blob/dd6aa8419ee173a0a6593eab669fbff54ed85f48/teensy4/imxrt.h#L9757 +int flexpwm_submodule_to_ext_sync(IMXRT_FLEXPWM_t* flexpwm, int submodule){ + if(submodule <0 && submodule > 3) return -1; + if(flexpwm == &IMXRT_FLEXPWM1){ + return XBARA1_OUT_FLEXPWM1_PWM0_EXT_SYNC + submodule; + }else if(flexpwm == &IMXRT_FLEXPWM2){ + return XBARA1_OUT_FLEXPWM2_PWM0_EXT_SYNC + submodule; + }else if(flexpwm == &IMXRT_FLEXPWM3){ + return XBARA1_OUT_FLEXPWM3_EXT_SYNC0 + submodule; // TODO verify why they are not called PWM0_EXT_SYNC but EXT_SYNC0 + }else if(flexpwm == &IMXRT_FLEXPWM4){ + return XBARA1_OUT_FLEXPWM4_EXT_SYNC0 + submodule; // TODO verify why they are not called PWM0_EXT_SYNC but EXT_SYNC0 + } + return -1; +} + +// The i.MXRT1062 uses one config register per two XBAR outputs, so a helper +// function to make code more readable. +void xbar_connect(unsigned int input, unsigned int output) +{ + if (input >= 88) return; + if (output >= 132) return; + volatile uint16_t *xbar = &XBARA1_SEL0 + (output / 2); + uint16_t val = *xbar; + if (!(output & 1)) { + val = (val & 0xFF00) | input; + } else { + val = (val & 0x00FF) | (input << 8); + } + *xbar = val; +} + +void xbar_init() { + CCM_CCGR2 |= CCM_CCGR2_XBAR1(CCM_CCGR_ON); //turn clock on for xbara1 +} + +// function which finds the flexpwm instance for a pin +// if it does not belong to the flexpwm timer it returns a nullpointer +IMXRT_FLEXPWM_t* get_flexpwm(uint8_t pin){ + + const struct pwm_pin_info_struct *info; + if (pin >= CORE_NUM_DIGITAL) { +#ifdef SIMPLEFOC_TEENSY_DEBUG + char s[60]; + sprintf (s, "TEENSY-DRV: ERR: Pin: %d not Flextimer pin!", pin); + SIMPLEFOC_DEBUG(s); +#endif + return nullptr; + } + info = pwm_pin_info + pin; + // FlexPWM pin + IMXRT_FLEXPWM_t *flexpwm; + switch ((info->module >> 4) & 3) { + case 0: flexpwm = &IMXRT_FLEXPWM1; break; + case 1: flexpwm = &IMXRT_FLEXPWM2; break; + case 2: flexpwm = &IMXRT_FLEXPWM3; break; + default: flexpwm = &IMXRT_FLEXPWM4; + } + if(flexpwm != nullptr){ +#ifdef SIMPLEFOC_TEENSY_DEBUG + char s[60]; + sprintf (s, "TEENSY-DRV: Pin: %d on Flextimer %d.", pin, ((info->module >> 4) & 3) + 1); + SIMPLEFOC_DEBUG(s); +#endif + return flexpwm; + } + return nullptr; +} +// function which finds the timer submodule for a pin +// if it does not belong to the submodule it returns a -1 +int get_submodule(uint8_t pin){ + + const struct pwm_pin_info_struct *info; + if (pin >= CORE_NUM_DIGITAL){ +#ifdef SIMPLEFOC_TEENSY_DEBUG + char s[60]; + sprintf (s, "TEENSY-DRV: ERR: Pin: %d not Flextimer pin!", pin); + SIMPLEFOC_DEBUG(s); +#endif + return -1; + } + + info = pwm_pin_info + pin; + int sm1 = info->module&0x3; + + if (sm1 >= 0 && sm1 < 4) { +#ifdef SIMPLEFOC_TEENSY_DEBUG + char s[60]; + sprintf (s, "TEENSY-DRV: Pin %d on submodule %d.", pin, sm1); + SIMPLEFOC_DEBUG(s); +#endif + return sm1; + } else { +#ifdef SIMPLEFOC_TEENSY_DEBUG + char s[50]; + sprintf (s, "TEENSY-DRV: ERR: Pin: %d not in submodule!", pin); + SIMPLEFOC_DEBUG(s); +#endif + return -1; + } +} + // function which finds the flexpwm instance for a pair of pins // if they do not belong to the same timer it returns a nullpointer IMXRT_FLEXPWM_t* get_flexpwm(uint8_t pin, uint8_t pin1){ @@ -145,7 +270,7 @@ return ch2; // can configure sync, prescale and B inversion. // sets the desired frequency of the PWM // sets the center-aligned pwm -void setup_pwm_pair (IMXRT_FLEXPWM_t * flexpwm, int submodule, const long frequency, float dead_zone ) +void setup_pwm_pair (IMXRT_FLEXPWM_t * flexpwm, int submodule, bool ext_sync, const long frequency, float dead_zone ) { int submodule_mask = 1 << submodule ; flexpwm->MCTRL &= ~ FLEXPWM_MCTRL_RUN (submodule_mask) ; // stop it if its already running @@ -167,17 +292,21 @@ void setup_pwm_pair (IMXRT_FLEXPWM_t * flexpwm, int submodule, const long freque } // the halfcycle of the PWM - half_cycle = int(newdiv/2.0f); + int half_cycle = int(newdiv/2.0f); int dead_time = int(dead_zone*half_cycle); //default dead-time - 2% int mid_pwm = int((half_cycle)/2.0f); + // if the timer should be externally synced with the master timer + int sel = ext_sync ? 3 : 0; + // setup the timer // https://github.com/PaulStoffregen/cores/blob/master/teensy4/imxrt.h flexpwm->SM[submodule].CTRL2 = FLEXPWM_SMCTRL2_WAITEN | FLEXPWM_SMCTRL2_DBGEN | - FLEXPWM_SMCTRL2_FRCEN | FLEXPWM_SMCTRL2_INIT_SEL(0) | FLEXPWM_SMCTRL2_FORCE_SEL(6); + FLEXPWM_SMCTRL2_FRCEN | FLEXPWM_SMCTRL2_INIT_SEL(sel) | FLEXPWM_SMCTRL2_FORCE_SEL(6); flexpwm->SM[submodule].CTRL = FLEXPWM_SMCTRL_FULL | FLEXPWM_SMCTRL_HALF | FLEXPWM_SMCTRL_PRSC(prescale) ; // https://github.com/PaulStoffregen/cores/blob/70ba01accd728abe75ebfc8dcd8b3d3a8f3e3f25/teensy4/imxrt.h#L4948 flexpwm->SM[submodule].OCTRL = 0; //FLEXPWM_SMOCTRL_POLA | FLEXPWM_SMOCTRL_POLB;//channel_to_invert==2 ? 0 : FLEXPWM_SMOCTRL_POLA | FLEXPWM_SMOCTRL_POLB ; + if (!ext_sync) flexpwm->SM[submodule].TCTRL = FLEXPWM_SMTCTRL_OUT_TRIG_EN (0b000010) ; // sync trig out on VAL1 match if master timer flexpwm->SM[submodule].DTCNT0 = dead_time ; // should try this out (deadtime control) flexpwm->SM[submodule].DTCNT1 = dead_time ; // should try this out (deadtime control) flexpwm->SM[submodule].INIT = -half_cycle; // count from -HALFCYCLE to +HALFCYCLE @@ -204,17 +333,15 @@ void startup_pwm_pair (IMXRT_FLEXPWM_t * flexpwm, int submodule) // PWM setting on the high and low pair of the PWM channels void write_pwm_pair(IMXRT_FLEXPWM_t * flexpwm, int submodule, float duty){ + int half_cycle = int(flexpwm->SM[submodule].VAL1); int mid_pwm = int((half_cycle)/2.0f); int count_pwm = int(mid_pwm*(duty*2-1)) + mid_pwm; - flexpwm->SM[submodule].VAL2 = count_pwm; // A on - flexpwm->SM[submodule].VAL3 = -count_pwm ; // A off + flexpwm->SM[submodule].VAL2 = -count_pwm; // A on + flexpwm->SM[submodule].VAL3 = count_pwm ; // A off flexpwm->MCTRL |= FLEXPWM_MCTRL_LDOK (1<flextimers[2], ((Teensy4DriverParams*)params)->submodules[2], dc_c); } + #endif \ No newline at end of file diff --git a/src/drivers/hardware_specific/teensy/teensy4_mcu.h b/src/drivers/hardware_specific/teensy/teensy4_mcu.h index 5e384623..a7d8c97a 100644 --- a/src/drivers/hardware_specific/teensy/teensy4_mcu.h +++ b/src/drivers/hardware_specific/teensy/teensy4_mcu.h @@ -7,6 +7,7 @@ // teensy 4 driver configuration parameters typedef struct Teensy4DriverParams { + int pins[6] = {(int)NOT_SET}; IMXRT_FLEXPWM_t* flextimers[3] = {NULL}; int submodules[3]; long pwm_frequency; @@ -105,6 +106,16 @@ const struct pwm_pin_info_struct pwm_pin_info[] = { #endif }; + +// find the trigger TRG0 for the given timer and submodule +int flexpwm_submodule_to_trig(IMXRT_FLEXPWM_t* flexpwm, int submodule); +// find the external trigger for the given timer and submodule +int flexpwm_submodule_to_ext_sync(IMXRT_FLEXPWM_t* flexpwm, int submodule); +// function to connecting the triggers +void xbar_connect(unsigned int input, unsigned int output); +// function to initialize the xbar +void xbar_init(); + #endif #endif \ No newline at end of file From 94f4fb5675873fa6f520e380988953d832ba0488 Mon Sep 17 00:00:00 2001 From: askuric Date: Thu, 29 Feb 2024 12:38:40 +0100 Subject: [PATCH 4/9] added support for center aligned 3pwm at least for one motor + low-side current sensing --- .../hardware_specific/teensy/teensy4_mcu.cpp | 16 +- .../hardware_specific/teensy/teensy4_mcu.cpp | 277 ++++++++- .../hardware_specific/teensy/teensy4_mcu.h | 10 +- .../teensy/teensy4_mcu1.cpp.new | 543 ++++++++++++++++++ .../hardware_specific/teensy/teensy_mcu.cpp | 98 ++-- .../hardware_specific/teensy/teensy_mcu.h | 13 + 6 files changed, 896 insertions(+), 61 deletions(-) create mode 100644 src/drivers/hardware_specific/teensy/teensy4_mcu1.cpp.new diff --git a/src/current_sense/hardware_specific/teensy/teensy4_mcu.cpp b/src/current_sense/hardware_specific/teensy/teensy4_mcu.cpp index b76624bc..e5934920 100644 --- a/src/current_sense/hardware_specific/teensy/teensy4_mcu.cpp +++ b/src/current_sense/hardware_specific/teensy/teensy4_mcu.cpp @@ -2,6 +2,7 @@ #include "../../../drivers/hardware_specific/teensy/teensy4_mcu.h" // #include "../../../common/lowpass_filter.h" #include "../../../common/foc_utils.h" +#include "../../../communication/SimpleFOCDebug.h" // if defined // - Teensy 4.0 @@ -158,7 +159,13 @@ float _readADCVoltageLowSide(const int pinA, const void* cs_params){ // Configure low side for generic mcu // cannot do much but void* _configureADCLowSide(const void* driver_params, const int pinA,const int pinB,const int pinC){ - _UNUSED(driver_params); + Teensy4DriverParams* par = (Teensy4DriverParams*) ((TeensyDriverParams*)driver_params)->additional_params; + if(par == nullptr){ + SIMPLEFOC_DEBUG("TEENSY-CS: Low side current sense failed, driver not supported!"); + return SIMPLEFOC_CURRENT_SENSE_INIT_FAILED; + } + + SIMPLEFOC_DEBUG("TEENSY-CS: Configuring low side current sense!"); #ifdef TEENSY4_ADC_INTERRUPT_DEBUG pinMode(30,OUTPUT); @@ -191,10 +198,15 @@ void* _configureADCLowSide(const void* driver_params, const int pinA,const int p // sync driver and the adc void _driverSyncLowSide(void* driver_params, void* cs_params){ - Teensy4DriverParams* par = (Teensy4DriverParams*) driver_params; + Teensy4DriverParams* par = (Teensy4DriverParams*) ((TeensyDriverParams*)driver_params)->additional_params; IMXRT_FLEXPWM_t* flexpwm = par->flextimers[0]; int submodule = par->submodules[0]; + SIMPLEFOC_DEBUG("TEENSY-CS: Syncing low side current sense!"); + char buff[50]; + sprintf(buff, "TEENSY-CS: Syncing to FlexPWM: %d, Submodule: %d", flexpwm_to_index(flexpwm), submodule); + SIMPLEFOC_DEBUG(buff); + // find the xbar trigger for the flexpwm int xbar_trig_pwm = flexpwm_submodule_to_trig(flexpwm, submodule); if(xbar_trig_pwm<0) return; diff --git a/src/drivers/hardware_specific/teensy/teensy4_mcu.cpp b/src/drivers/hardware_specific/teensy/teensy4_mcu.cpp index bf81e246..6815c10f 100644 --- a/src/drivers/hardware_specific/teensy/teensy4_mcu.cpp +++ b/src/drivers/hardware_specific/teensy/teensy4_mcu.cpp @@ -1,4 +1,3 @@ -#include "teensy_mcu.h" #include "teensy4_mcu.h" #include "../../../communication/SimpleFOCDebug.h" @@ -7,11 +6,11 @@ // - Teensy 4.1 #if defined(__arm__) && defined(CORE_TEENSY) && ( defined(__IMXRT1062__) || defined(ARDUINO_TEENSY40) || defined(ARDUINO_TEENSY41) || defined(ARDUINO_TEENSY_MICROMOD) ) - #pragma message("") #pragma message("SimpleFOC: compiling for Teensy 4.x") #pragma message("") +// #define AVOID_TEENSY4_CENTER_ALIGNED_3PWM // function finding the TRIG event given the flexpwm timer and the submodule @@ -41,7 +40,7 @@ int flexpwm_submodule_to_trig(IMXRT_FLEXPWM_t* flexpwm, int submodule){ // the flags are defined in the imxrt.h file // https://github.com/PaulStoffregen/cores/blob/dd6aa8419ee173a0a6593eab669fbff54ed85f48/teensy4/imxrt.h#L9757 int flexpwm_submodule_to_ext_sync(IMXRT_FLEXPWM_t* flexpwm, int submodule){ - if(submodule <0 && submodule > 3) return -1; + if(submodule < 0 && submodule > 3) return -1; if(flexpwm == &IMXRT_FLEXPWM1){ return XBARA1_OUT_FLEXPWM1_PWM0_EXT_SYNC + submodule; }else if(flexpwm == &IMXRT_FLEXPWM2){ @@ -54,6 +53,15 @@ int flexpwm_submodule_to_ext_sync(IMXRT_FLEXPWM_t* flexpwm, int submodule){ return -1; } +// function finding the flexpwm instance given the submodule +int flexpwm_to_index(IMXRT_FLEXPWM_t* flexpwm){ + if(flexpwm == &IMXRT_FLEXPWM1) return 1; + if(flexpwm == &IMXRT_FLEXPWM2) return 2; + if(flexpwm == &IMXRT_FLEXPWM3) return 3; + if(flexpwm == &IMXRT_FLEXPWM4) return 4; + return -1; +} + // The i.MXRT1062 uses one config register per two XBAR outputs, so a helper // function to make code more readable. void xbar_connect(unsigned int input, unsigned int output) @@ -223,11 +231,34 @@ int get_submodule(uint8_t pin, uint8_t pin1){ } +// function which finds the channel for flexpwm timer pin +// 0 - X +// 1 - A +// 2 - B +int get_channel(uint8_t pin){ + + const struct pwm_pin_info_struct *info; + if (pin >= CORE_NUM_DIGITAL){ +#ifdef SIMPLEFOC_TEENSY_DEBUG + char s[60]; + sprintf (s, "TEENSY-DRV: ERR: Pin: %d not Flextimer pin!", pin); + SIMPLEFOC_DEBUG(s); +#endif + return -1; + } + info = pwm_pin_info + pin; +#ifdef SIMPLEFOC_TEENSY_DEBUG + char s[60]; + sprintf (s, "TEENSY-DRV: Pin: %d on channel %s.", pin, info->channel==0 ? "X" : info->channel==1 ? "A" : "B"); + SIMPLEFOC_DEBUG(s); +#endif + return info->channel; +} + // function which finds the timer submodule for a pair of pins // if they do not belong to the same submodule it returns a -1 int get_inverted_channel(uint8_t pin, uint8_t pin1){ - const struct pwm_pin_info_struct *info; if (pin >= CORE_NUM_DIGITAL || pin1 >= CORE_NUM_DIGITAL){ #ifdef SIMPLEFOC_TEENSY_DEBUG char s[60]; @@ -237,10 +268,8 @@ int get_inverted_channel(uint8_t pin, uint8_t pin1){ return -1; } - info = pwm_pin_info + pin; - int ch1 = info->channel; - info = pwm_pin_info + pin1; - int ch2 = info->channel; + int ch1 = get_channel(pin); + int ch2 = get_channel(pin1); if (ch1 != 1) { #ifdef SIMPLEFOC_TEENSY_DEBUG @@ -320,6 +349,70 @@ void setup_pwm_pair (IMXRT_FLEXPWM_t * flexpwm, int submodule, bool ext_sync, c } +// Helper to set up a FlexPWM submodule. +// can configure sync, prescale +// sets the desired frequency of the PWM +// sets the center-aligned pwm +void setup_pwm_timer_submodule (IMXRT_FLEXPWM_t * flexpwm, int submodule, bool ext_sync, const long frequency) +{ + int submodule_mask = 1 << submodule ; + flexpwm->MCTRL &= ~ FLEXPWM_MCTRL_RUN (submodule_mask) ; // stop it if its already running + flexpwm->MCTRL |= FLEXPWM_MCTRL_CLDOK (submodule_mask) ; // clear load OK + + // calculate the counter and prescaler for the desired pwm frequency + uint32_t newdiv = (uint32_t)((float)F_BUS_ACTUAL / frequency + 0.5f); + uint32_t prescale = 0; + //printf(" div=%lu\n", newdiv); + while (newdiv > 65535 && prescale < 7) { + newdiv = newdiv >> 1; + prescale = prescale + 1; + } + if (newdiv > 65535) { + newdiv = 65535; + } else if (newdiv < 2) { + newdiv = 2; + } + + // the halfcycle of the PWM + int half_cycle = int(newdiv/2.0f); + + // if the timer should be externally synced with the master timer + int sel = ext_sync ? 3 : 0; + + // setup the timer + // https://github.com/PaulStoffregen/cores/blob/master/teensy4/imxrt.h + flexpwm->SM[submodule].CTRL2 = FLEXPWM_SMCTRL2_INDEP | FLEXPWM_SMCTRL2_WAITEN | FLEXPWM_SMCTRL2_DBGEN | + FLEXPWM_SMCTRL2_FRCEN | FLEXPWM_SMCTRL2_INIT_SEL(sel) | FLEXPWM_SMCTRL2_FORCE_SEL(6); + flexpwm->SM[submodule].CTRL = FLEXPWM_SMCTRL_FULL | FLEXPWM_SMCTRL_HALF | FLEXPWM_SMCTRL_PRSC(prescale) ; + // https://github.com/PaulStoffregen/cores/blob/70ba01accd728abe75ebfc8dcd8b3d3a8f3e3f25/teensy4/imxrt.h#L4948 + flexpwm->SM[submodule].OCTRL = 0; //FLEXPWM_SMOCTRL_POLA | FLEXPWM_SMOCTRL_POLB;//channel_to_invert==2 ? 0 : FLEXPWM_SMOCTRL_POLA | FLEXPWM_SMOCTRL_POLB ; + if (!ext_sync) flexpwm->SM[submodule].TCTRL = FLEXPWM_SMTCTRL_OUT_TRIG_EN (0b000010) ; // sync trig out on VAL1 match if master timer + flexpwm->SM[submodule].DTCNT0 = 0 ; + flexpwm->SM[submodule].DTCNT1 = 0 ; + flexpwm->SM[submodule].INIT = -half_cycle; // count from -HALFCYCLE to +HALFCYCLE + flexpwm->SM[submodule].VAL0 = 0; + flexpwm->SM[submodule].VAL1 = half_cycle; + flexpwm->SM[submodule].VAL2 = 0 ; + flexpwm->SM[submodule].VAL3 = 0 ; + flexpwm->SM[submodule].VAL2 = 0 ; + flexpwm->SM[submodule].VAL3 = 0 ; + + flexpwm->MCTRL |= FLEXPWM_MCTRL_LDOK (submodule_mask) ; // loading reenabled + flexpwm->MCTRL |= FLEXPWM_MCTRL_RUN (submodule_mask) ; // start it running +} + + +// staring the PWM on A and B channels of the submodule +void startup_pwm_pair (IMXRT_FLEXPWM_t * flexpwm, int submodule, int channel) +{ + int submodule_mask = 1 << submodule ; + + if(channel==1) flexpwm->OUTEN |= FLEXPWM_OUTEN_PWMA_EN (submodule_mask); // enable A output + else if(channel==2) flexpwm->OUTEN |= FLEXPWM_OUTEN_PWMB_EN (submodule_mask); // enable B output +} + + + // staring the PWM on A and B channels of the submodule void startup_pwm_pair (IMXRT_FLEXPWM_t * flexpwm, int submodule) { @@ -352,6 +445,7 @@ void* _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, cons IMXRT_FLEXPWM_t *flexpwmA,*flexpwmB,*flexpwmC; int submoduleA,submoduleB,submoduleC; int inverted_channelA,inverted_channelB,inverted_channelC; + int channelA,channelB,channelC; flexpwmA = get_flexpwm(pinA_h,pinA_l); submoduleA = get_submodule(pinA_h,pinA_l); inverted_channelA = get_inverted_channel(pinA_h,pinA_l); @@ -361,6 +455,9 @@ void* _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, cons flexpwmC = get_flexpwm(pinC_h,pinC_l); submoduleC = get_submodule(pinC_h,pinC_l); inverted_channelC = get_inverted_channel(pinC_h,pinC_l); + channelA = get_channel(pinA_h); + channelB = get_channel(pinB_h); + channelC = get_channel(pinC_h); if((flexpwmA == nullptr) || (flexpwmB == nullptr) || (flexpwmC == nullptr) ){ #ifdef SIMPLEFOC_TEENSY_DEBUG @@ -381,21 +478,18 @@ void* _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, cons return SIMPLEFOC_DRIVER_INIT_FAILED; } - - Teensy4DriverParams* params = new Teensy4DriverParams { - .pins = { pinA_h, pinA_l, pinB_h, pinB_l, pinC_h, pinC_l }, - .flextimers = { flexpwmA, flexpwmB, flexpwmC}, - .submodules = { submoduleA, submoduleB, submoduleC}, - .pwm_frequency = pwm_frequency, - .dead_zone = dead_zone - }; - + #ifdef SIMPLEFOC_TEENSY_DEBUG + char buff[100]; + sprintf(buff, "TEENSY-DRV: Syncing to Master FlexPWM: %d, Submodule: %d", flexpwm_to_index(flexpwmC), submoduleC); + SIMPLEFOC_DEBUG(buff); + sprintf(buff, "TEENSY-DRV: Slave timers FlexPWM: %d, Submodule: %d and FlexPWM: %d, Submodule: %d", flexpwm_to_index(flexpwmA), submoduleA, flexpwm_to_index(flexpwmB), submoduleB); + SIMPLEFOC_DEBUG(buff); + #endif // Configure FlexPWM units, each driving A/B pair, B inverted. - // full speed about 80kHz, prescale 2 (div by 4) gives 20kHz - setup_pwm_pair (flexpwmA, submoduleA, true, pwm_frequency, dead_zone) ; // this is the master, internally synced + setup_pwm_pair (flexpwmA, submoduleA, true, pwm_frequency, dead_zone) ; // others externally synced setup_pwm_pair (flexpwmB, submoduleB, true, pwm_frequency, dead_zone) ; // others externally synced - setup_pwm_pair (flexpwmC, submoduleC, false, pwm_frequency, dead_zone) ; + setup_pwm_pair (flexpwmC, submoduleC, false, pwm_frequency, dead_zone) ; // this is the master, internally synced delayMicroseconds (100) ; // turn on XBAR1 clock for all but stop mode @@ -409,7 +503,6 @@ void* _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, cons startup_pwm_pair (flexpwmB, submoduleB) ; startup_pwm_pair (flexpwmC, submoduleC) ; - delayMicroseconds(50) ; // config the pins 2/3/6/9/8/7 as their FLEXPWM alternates. *portConfigRegister(pinA_h) = pwm_pin_info[pinA_h].muxval ; @@ -419,6 +512,17 @@ void* _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, cons *portConfigRegister(pinC_h) = pwm_pin_info[pinC_h].muxval ; *portConfigRegister(pinC_l) = pwm_pin_info[pinC_l].muxval ; + + TeensyDriverParams* params = new TeensyDriverParams { + .pins = { pinA_h, pinA_l, pinB_h, pinB_l, pinC_h, pinC_l }, + .pwm_frequency = pwm_frequency, + .additional_params = new Teensy4DriverParams { + .flextimers = { flexpwmA, flexpwmB, flexpwmC}, + .submodules = { submoduleA, submoduleB, submoduleC}, + .channels = {1,2, 1, 2, 1, 2}, + .dead_zone = dead_zone + } + }; return params; } @@ -428,11 +532,136 @@ void* _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, cons // - Stepper motor - 6PWM setting // - hardware specific void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, PhaseState *phase_state, void* params){ + Teensy4DriverParams* p = (Teensy4DriverParams*)((TeensyDriverParams*)params)->additional_params; _UNUSED(phase_state); - write_pwm_pair (((Teensy4DriverParams*)params)->flextimers[0], ((Teensy4DriverParams*)params)->submodules[0], dc_a); - write_pwm_pair (((Teensy4DriverParams*)params)->flextimers[1], ((Teensy4DriverParams*)params)->submodules[1], dc_b); - write_pwm_pair (((Teensy4DriverParams*)params)->flextimers[2], ((Teensy4DriverParams*)params)->submodules[2], dc_c); + write_pwm_pair (p->flextimers[0], p->submodules[0], dc_a); + write_pwm_pair (p->flextimers[1], p->submodules[1], dc_b); + write_pwm_pair (p->flextimers[2], p->submodules[2], dc_c); +} + +void write_pwm_on_pin(IMXRT_FLEXPWM_t *p, unsigned int submodule, uint8_t channel, float duty) +{ + uint16_t mask = 1 << submodule; + uint32_t half_cycle = p->SM[submodule].VAL1; + int mid_pwm = int((half_cycle)/2.0f); + int cval = int(mid_pwm*(duty*2-1)) + mid_pwm; + + //printf("flexpwmWrite, p=%08lX, sm=%d, ch=%c, cval=%ld\n", + //(uint32_t)p, submodule, channel == 0 ? 'X' : (channel == 1 ? 'A' : 'B'), cval); + p->MCTRL |= FLEXPWM_MCTRL_CLDOK(mask); + switch (channel) { + case 0: // X + p->SM[submodule].VAL0 = half_cycle - cval; + p->OUTEN |= FLEXPWM_OUTEN_PWMX_EN(mask); + //printf(" write channel X\n"); + break; + case 1: // A + p->SM[submodule].VAL2 = -cval; + p->SM[submodule].VAL3 = cval; + p->OUTEN |= FLEXPWM_OUTEN_PWMA_EN(mask); + //printf(" write channel A\n"); + break; + case 2: // B + p->SM[submodule].VAL4 = -cval; + p->SM[submodule].VAL5 = cval; + p->OUTEN |= FLEXPWM_OUTEN_PWMB_EN(mask); + //printf(" write channel B\n"); + } + p->MCTRL |= FLEXPWM_MCTRL_LDOK(mask); } +#ifndef AVOID_TEENSY4_CENTER_ALIGNED_3PWM + +// function setting the high pwm frequency to the supplied pins +// - BLDC motor - 3PWM setting +// - hardware speciffic +// in generic case dont do anything + void* _configureCenterAligned3PMW(long pwm_frequency,const int pinA, const int pinB, const int pinC) { + + if(!pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; // default frequency 25khz + else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 50kHz max + + IMXRT_FLEXPWM_t *flexpwmA,*flexpwmB,*flexpwmC; + int submoduleA,submoduleB,submoduleC; + flexpwmA = get_flexpwm(pinA); + submoduleA = get_submodule(pinA); + flexpwmB = get_flexpwm(pinB); + submoduleB = get_submodule(pinB); + flexpwmC = get_flexpwm(pinC); + submoduleC = get_submodule(pinC); + int channelA = get_channel(pinA); + int channelB = get_channel(pinB); + int channelC = get_channel(pinC); + + + // if pins belong to the flextimers and they only use submodules A and B + // we can configure the center-aligned pwm + if((flexpwmA != nullptr) && (flexpwmB != nullptr) && (flexpwmC != nullptr) && (channelA > 0) && (channelB > 0) && (channelC > 0) ){ + #ifdef SIMPLEFOC_TEENSY_DEBUG + SIMPLEFOC_DEBUG("TEENSY-DRV: All pins on Flexpwm A or B submodules - Configuring center-aligned pwm!"); + #endif + + // Configure FlexPWM units + setup_pwm_timer_submodule (flexpwmA, submoduleA, true, pwm_frequency) ; // others externally synced + setup_pwm_timer_submodule (flexpwmB, submoduleB, true, pwm_frequency) ; // others externally synced + setup_pwm_timer_submodule (flexpwmC, submoduleC, false, pwm_frequency) ; // this is the master, internally synced + delayMicroseconds (100) ; + + + #ifdef SIMPLEFOC_TEENSY_DEBUG + char buff[100]; + sprintf(buff, "TEENSY-CS: Syncing to Master FlexPWM: %d, Submodule: %d", flexpwm_to_index(flexpwmC), submoduleC); + SIMPLEFOC_DEBUG(buff); + sprintf(buff, "TEENSY-CS: Slave timers FlexPWM: %d, Submodule: %d and FlexPWM: %d, Submodule: %d", flexpwm_to_index(flexpwmA), submoduleA, flexpwm_to_index(flexpwmB), submoduleB); + SIMPLEFOC_DEBUG(buff); + #endif + + // // turn on XBAR1 clock for all but stop mode + xbar_init() ; + + // // Connect trigger to synchronize all timers + xbar_connect (flexpwm_submodule_to_trig(flexpwmC, submoduleC), flexpwm_submodule_to_ext_sync(flexpwmA, submoduleA)) ; + xbar_connect (flexpwm_submodule_to_trig(flexpwmC, submoduleC), flexpwm_submodule_to_ext_sync(flexpwmB, submoduleB)) ; + + TeensyDriverParams* params = new TeensyDriverParams { + .pins = { pinA, pinB, pinC }, + .pwm_frequency = pwm_frequency, + .additional_params = new Teensy4DriverParams { + .flextimers = { flexpwmA, flexpwmB, flexpwmC}, + .submodules = { submoduleA, submoduleB, submoduleC}, + .channels = {channelA, channelB, channelC}, + } + }; + + startup_pwm_pair (flexpwmA, submoduleA, channelA) ; + startup_pwm_pair (flexpwmB, submoduleB, channelB) ; + startup_pwm_pair (flexpwmC, submoduleC, channelC) ; + + // // config the pins 2/3/6/9/8/7 as their FLEXPWM alternates. + *portConfigRegister(pinA) = pwm_pin_info[pinA].muxval ; + *portConfigRegister(pinB) = pwm_pin_info[pinB].muxval ; + *portConfigRegister(pinC) = pwm_pin_info[pinC].muxval ; + + return params; + }else{ + #ifdef SIMPLEFOC_TEENSY_DEBUG + SIMPLEFOC_DEBUG("TEENSY-DRV: Not all pins on Flexpwm A and B submodules - cannot configure center-aligned pwm!"); + #endif + return SIMPLEFOC_DRIVER_INIT_FAILED; + } + +} + +// function setting the pwm duty cycle to the hardware +// - Stepper motor - 6PWM setting +// - hardware specific +void _writeCenterAligned3PMW(float dc_a, float dc_b, float dc_c, void* params){ + Teensy4DriverParams* p = (Teensy4DriverParams*)((TeensyDriverParams*)params)->additional_params; + write_pwm_on_pin (p->flextimers[0], p->submodules[0], p->channels[0], dc_a); + write_pwm_on_pin (p->flextimers[1], p->submodules[1], p->channels[1], dc_b); + write_pwm_on_pin (p->flextimers[2], p->submodules[2], p->channels[2], dc_c); +} + +#endif #endif \ No newline at end of file diff --git a/src/drivers/hardware_specific/teensy/teensy4_mcu.h b/src/drivers/hardware_specific/teensy/teensy4_mcu.h index a7d8c97a..aed64826 100644 --- a/src/drivers/hardware_specific/teensy/teensy4_mcu.h +++ b/src/drivers/hardware_specific/teensy/teensy4_mcu.h @@ -1,3 +1,6 @@ +#ifndef TEENSY4_MCU_DRIVER_H +#define TEENSY4_MCU_DRIVER_H + #include "teensy_mcu.h" // if defined @@ -7,10 +10,9 @@ // teensy 4 driver configuration parameters typedef struct Teensy4DriverParams { - int pins[6] = {(int)NOT_SET}; IMXRT_FLEXPWM_t* flextimers[3] = {NULL}; int submodules[3]; - long pwm_frequency; + int channels[6]; float dead_zone; } Teensy4DriverParams; @@ -106,7 +108,8 @@ const struct pwm_pin_info_struct pwm_pin_info[] = { #endif }; - +// function finding the flexpwm instance given the submodule +int flexpwm_to_index(IMXRT_FLEXPWM_t* flexpwm); // find the trigger TRG0 for the given timer and submodule int flexpwm_submodule_to_trig(IMXRT_FLEXPWM_t* flexpwm, int submodule); // find the external trigger for the given timer and submodule @@ -118,4 +121,5 @@ void xbar_init(); #endif +#endif #endif \ No newline at end of file diff --git a/src/drivers/hardware_specific/teensy/teensy4_mcu1.cpp.new b/src/drivers/hardware_specific/teensy/teensy4_mcu1.cpp.new new file mode 100644 index 00000000..5dcac90d --- /dev/null +++ b/src/drivers/hardware_specific/teensy/teensy4_mcu1.cpp.new @@ -0,0 +1,543 @@ +#include "teensy_mcu.h" +#include "teensy4_mcu.h" +#include "../../../communication/SimpleFOCDebug.h" + +// if defined +// - Teensy 4.0 +// - Teensy 4.1 +#if defined(__arm__) && defined(CORE_TEENSY) && ( defined(__IMXRT1062__) || defined(ARDUINO_TEENSY40) || defined(ARDUINO_TEENSY41) || defined(ARDUINO_TEENSY_MICROMOD) ) + + +#pragma message("") +#pragma message("SimpleFOC: compiling for Teensy 4.x") +#pragma message("") + + + +// function finding the TRIG event given the flexpwm timer and the submodule +// returning -1 if the submodule is not valid or no trigger is available +// allowing flexpwm1-4 and submodule 0-3 +// +// the flags are defined in the imxrt.h file +// https://github.com/PaulStoffregen/cores/blob/dd6aa8419ee173a0a6593eab669fbff54ed85f48/teensy4/imxrt.h#L9662-L9693 +int flexpwm_submodule_to_trig(IMXRT_FLEXPWM_t* flexpwm, int submodule){ + if(submodule <0 && submodule > 3) return -1; + if(flexpwm == &IMXRT_FLEXPWM1){ + return XBARA1_IN_FLEXPWM1_PWM1_OUT_TRIG0 + submodule; + }else if(flexpwm == &IMXRT_FLEXPWM2){ + return XBARA1_IN_FLEXPWM2_PWM1_OUT_TRIG0 + submodule; + }else if(flexpwm == &IMXRT_FLEXPWM3){ + return XBARA1_IN_FLEXPWM3_PWM1_OUT_TRIG0 + submodule; + }else if(flexpwm == &IMXRT_FLEXPWM4){ + return XBARA1_IN_FLEXPWM4_PWM1_OUT_TRIG0 + submodule; + } + return -1; +} + +// function finding the EXT_SYNC event given the flexpwm timer and the submodule +// returning -1 if the submodule is not valid or no trigger is available +// allowing flexpwm1-4 and submodule 0-3 +// +// the flags are defined in the imxrt.h file +// https://github.com/PaulStoffregen/cores/blob/dd6aa8419ee173a0a6593eab669fbff54ed85f48/teensy4/imxrt.h#L9757 +int flexpwm_submodule_to_ext_sync(IMXRT_FLEXPWM_t* flexpwm, int submodule){ + if(submodule <0 && submodule > 3) return -1; + if(flexpwm == &IMXRT_FLEXPWM1){ + return XBARA1_OUT_FLEXPWM1_PWM0_EXT_SYNC + submodule; + }else if(flexpwm == &IMXRT_FLEXPWM2){ + return XBARA1_OUT_FLEXPWM2_PWM0_EXT_SYNC + submodule; + }else if(flexpwm == &IMXRT_FLEXPWM3){ + return XBARA1_OUT_FLEXPWM3_EXT_SYNC0 + submodule; // TODO verify why they are not called PWM0_EXT_SYNC but EXT_SYNC0 + }else if(flexpwm == &IMXRT_FLEXPWM4){ + return XBARA1_OUT_FLEXPWM4_EXT_SYNC0 + submodule; // TODO verify why they are not called PWM0_EXT_SYNC but EXT_SYNC0 + } + return -1; +} + +// The i.MXRT1062 uses one config register per two XBAR outputs, so a helper +// function to make code more readable. +void xbar_connect(unsigned int input, unsigned int output) +{ + if (input >= 88) return; + if (output >= 132) return; + volatile uint16_t *xbar = &XBARA1_SEL0 + (output / 2); + uint16_t val = *xbar; + if (!(output & 1)) { + val = (val & 0xFF00) | input; + } else { + val = (val & 0x00FF) | (input << 8); + } + *xbar = val; +} + +void xbar_init() { + CCM_CCGR2 |= CCM_CCGR2_XBAR1(CCM_CCGR_ON); //turn clock on for xbara1 +} + +// half_cycle of the PWM variable +int half_cycle = 0; + +// function which finds the flexpwm instance for a pin +// if it does not belong to the flexpwm timer it returns a nullpointer +IMXRT_FLEXPWM_t* get_flexpwm(uint8_t pin){ + + const struct pwm_pin_info_struct *info; + if (pin >= CORE_NUM_DIGITAL) { +#ifdef SIMPLEFOC_TEENSY_DEBUG + char s[60]; + sprintf (s, "TEENSY-DRV: ERR: Pin: %d not Flextimer pin!", pin); + SIMPLEFOC_DEBUG(s); +#endif + return nullptr; + } + info = pwm_pin_info + pin; + // FlexPWM pin + IMXRT_FLEXPWM_t *flexpwm; + switch ((info->module >> 4) & 3) { + case 0: flexpwm = &IMXRT_FLEXPWM1; break; + case 1: flexpwm = &IMXRT_FLEXPWM2; break; + case 2: flexpwm = &IMXRT_FLEXPWM3; break; + default: flexpwm = &IMXRT_FLEXPWM4; + } + if(flexpwm != nullptr){ +#ifdef SIMPLEFOC_TEENSY_DEBUG + char s[60]; + sprintf (s, "TEENSY-DRV: Pin: %d on Flextimer %d.", pin, ((info->module >> 4) & 3) + 1); + SIMPLEFOC_DEBUG(s); +#endif + return flexpwm; + } + return nullptr; +} + + +// function which finds the timer submodule for a pin +// if it does not belong to the submodule it returns a -1 +int get_submodule(uint8_t pin){ + + const struct pwm_pin_info_struct *info; + if (pin >= CORE_NUM_DIGITAL){ +#ifdef SIMPLEFOC_TEENSY_DEBUG + char s[60]; + sprintf (s, "TEENSY-DRV: ERR: Pin: %d not Flextimer pin!", pin); + SIMPLEFOC_DEBUG(s); +#endif + return -1; + } + + info = pwm_pin_info + pin; + int sm1 = info->module&0x3; + + if (sm1 >= 0 && sm1 < 4) { +#ifdef SIMPLEFOC_TEENSY_DEBUG + char s[60]; + sprintf (s, "TEENSY-DRV: Pin %d on submodule %d.", pin, sm1); + SIMPLEFOC_DEBUG(s); +#endif + return sm1; + } else { +#ifdef SIMPLEFOC_TEENSY_DEBUG + char s[50]; + sprintf (s, "TEENSY-DRV: ERR: Pin: %d not in submodule!", pin); + SIMPLEFOC_DEBUG(s); +#endif + return -1; + } +} + +// function which finds the flexpwm instance for a pair of pins +// if they do not belong to the same timer it returns a nullpointer +IMXRT_FLEXPWM_t* get_flexpwm(uint8_t pin, uint8_t pin1){ + + const struct pwm_pin_info_struct *info; + if (pin >= CORE_NUM_DIGITAL || pin1 >= CORE_NUM_DIGITAL) { +#ifdef SIMPLEFOC_TEENSY_DEBUG + char s[60]; + sprintf (s, "TEENSY-DRV: ERR: Pins: %d, %d not Flextimer pins!", pin, pin1); + SIMPLEFOC_DEBUG(s); +#endif + return nullptr; + } + info = pwm_pin_info + pin; + // FlexPWM pin + IMXRT_FLEXPWM_t *flexpwm1,*flexpwm2; + switch ((info->module >> 4) & 3) { + case 0: flexpwm1 = &IMXRT_FLEXPWM1; break; + case 1: flexpwm1 = &IMXRT_FLEXPWM2; break; + case 2: flexpwm1 = &IMXRT_FLEXPWM3; break; + default: flexpwm1 = &IMXRT_FLEXPWM4; + } + + info = pwm_pin_info + pin1; + switch ((info->module >> 4) & 3) { + case 0: flexpwm2 = &IMXRT_FLEXPWM1; break; + case 1: flexpwm2 = &IMXRT_FLEXPWM2; break; + case 2: flexpwm2 = &IMXRT_FLEXPWM3; break; + default: flexpwm2 = &IMXRT_FLEXPWM4; + } + if(flexpwm1 == flexpwm2){ + char s[60]; + sprintf (s, "TEENSY-DRV: Pins: %d, %d on Flextimer %d.", pin, pin1, ((info->module >> 4) & 3) + 1); + SIMPLEFOC_DEBUG(s); + return flexpwm1; + } else { +#ifdef SIMPLEFOC_TEENSY_DEBUG + char s[60]; + sprintf (s, "TEENSY-DRV: ERR: Pins: %d, %d not in same Flextimer!", pin, pin1); + SIMPLEFOC_DEBUG(s); +#endif + return nullptr; + } +} + + +// function which finds the timer submodule for a pair of pins +// if they do not belong to the same submodule it returns a -1 +int get_submodule(uint8_t pin, uint8_t pin1){ + + const struct pwm_pin_info_struct *info; + if (pin >= CORE_NUM_DIGITAL || pin1 >= CORE_NUM_DIGITAL){ +#ifdef SIMPLEFOC_TEENSY_DEBUG + char s[60]; + sprintf (s, "TEENSY-DRV: ERR: Pins: %d, %d not Flextimer pins!", pin, pin1); + SIMPLEFOC_DEBUG(s); +#endif + return -1; + } + + info = pwm_pin_info + pin; + int sm1 = info->module&0x3; + info = pwm_pin_info + pin1; + int sm2 = info->module&0x3; + + if (sm1 == sm2) { + char s[60]; + sprintf (s, "TEENSY-DRV: Pins: %d, %d on submodule %d.", pin, pin1, sm1); + SIMPLEFOC_DEBUG(s); + return sm1; + } else { +#ifdef SIMPLEFOC_TEENSY_DEBUG + char s[50]; + sprintf (s, "TEENSY-DRV: ERR: Pins: %d, %d not in same submodule!", pin, pin1); + SIMPLEFOC_DEBUG(s); +#endif + return -1; + } +} + + +// function which finds the timer submodule for a pair of pins +// if they do not belong to the same submodule it returns a -1 +int get_inverted_channel(uint8_t pin, uint8_t pin1){ + + const struct pwm_pin_info_struct *info; + if (pin >= CORE_NUM_DIGITAL || pin1 >= CORE_NUM_DIGITAL){ +#ifdef SIMPLEFOC_TEENSY_DEBUG + char s[60]; + sprintf (s, "TEENSY-DRV: ERR: Pins: %d, %d not Flextimer pins!", pin, pin1); + SIMPLEFOC_DEBUG(s); +#endif + return -1; + } + + info = pwm_pin_info + pin; + int ch1 = info->channel; + info = pwm_pin_info + pin1; + int ch2 = info->channel; + + if (ch1 != 1) { +#ifdef SIMPLEFOC_TEENSY_DEBUG + char s[60]; + sprintf (s, "TEENSY-DRV: ERR: Pin: %d on channel %s - only A supported", pin1, ch1==2 ? "B" : "X"); + SIMPLEFOC_DEBUG(s); +#endif + return -1; + } else if (ch2 != 2) { +#ifdef SIMPLEFOC_TEENSY_DEBUG + char s[60]; + sprintf (s, "TEENSY-DRV: ERR: Inverted pin: %d on channel %s - only B supported", pin1, ch2==1 ? "A" : "X"); + SIMPLEFOC_DEBUG(s); +#endif + return -1; + } else { +#ifdef SIMPLEFOC_TEENSY_DEBUG + char s[60]; + sprintf (s, "TEENSY-DRV: Pin: %d on channel B inverted.", pin1); + SIMPLEFOC_DEBUG(s); +#endif +return ch2; + } +} + +// Helper to set up A/B pair on a FlexPWM submodule. +// can configure sync, prescale and B inversion. +// sets the desired frequency of the PWM +// sets the center-aligned pwm +void setup_pwm_pair (IMXRT_FLEXPWM_t * flexpwm, int submodule, bool ext_sync, const long frequency, float dead_zone ) +{ + int submodule_mask = 1 << submodule ; + flexpwm->MCTRL &= ~ FLEXPWM_MCTRL_RUN (submodule_mask) ; // stop it if its already running + flexpwm->MCTRL |= FLEXPWM_MCTRL_CLDOK (submodule_mask) ; // clear load OK + + + // calculate the counter and prescaler for the desired pwm frequency + uint32_t newdiv = (uint32_t)((float)F_BUS_ACTUAL / frequency + 0.5f); + uint32_t prescale = 0; + //printf(" div=%lu\n", newdiv); + while (newdiv > 65535 && prescale < 7) { + newdiv = newdiv >> 1; + prescale = prescale + 1; + } + if (newdiv > 65535) { + newdiv = 65535; + } else if (newdiv < 2) { + newdiv = 2; + } + + // the halfcycle of the PWM + half_cycle = int(newdiv/2.0f); + int dead_time = int(dead_zone*half_cycle); //default dead-time - 2% + int mid_pwm = int((half_cycle)/2.0f); + + // if the timer should be externally synced with the master timer + int sel = ext_sync ? 3 : 0; + + // setup the timer + // https://github.com/PaulStoffregen/cores/blob/master/teensy4/imxrt.h + flexpwm->SM[submodule].CTRL2 = FLEXPWM_SMCTRL2_WAITEN | FLEXPWM_SMCTRL2_DBGEN | + FLEXPWM_SMCTRL2_FRCEN | FLEXPWM_SMCTRL2_INIT_SEL(sel) | FLEXPWM_SMCTRL2_FORCE_SEL(6); + flexpwm->SM[submodule].CTRL = FLEXPWM_SMCTRL_FULL | FLEXPWM_SMCTRL_HALF | FLEXPWM_SMCTRL_PRSC(prescale) ; + // https://github.com/PaulStoffregen/cores/blob/70ba01accd728abe75ebfc8dcd8b3d3a8f3e3f25/teensy4/imxrt.h#L4948 + flexpwm->SM[submodule].OCTRL = 0; //FLEXPWM_SMOCTRL_POLA | FLEXPWM_SMOCTRL_POLB;//channel_to_invert==2 ? 0 : FLEXPWM_SMOCTRL_POLA | FLEXPWM_SMOCTRL_POLB ; + if (!ext_sync) flexpwm->SM[submodule].TCTRL = FLEXPWM_SMTCTRL_OUT_TRIG_EN (0b000010) ; // sync trig out on VAL1 match if master timer + flexpwm->SM[submodule].DTCNT0 = dead_time ; // should try this out (deadtime control) + flexpwm->SM[submodule].DTCNT1 = dead_time ; // should try this out (deadtime control) + flexpwm->SM[submodule].INIT = -half_cycle; // count from -HALFCYCLE to +HALFCYCLE + flexpwm->SM[submodule].VAL0 = 0; + flexpwm->SM[submodule].VAL1 = half_cycle ; + flexpwm->SM[submodule].VAL2 = -mid_pwm ; + flexpwm->SM[submodule].VAL3 = +mid_pwm ; + + flexpwm->MCTRL |= FLEXPWM_MCTRL_LDOK (submodule_mask) ; // loading reenabled + flexpwm->MCTRL |= FLEXPWM_MCTRL_RUN (submodule_mask) ; // start it running +} + + +// staring the PWM on A and B channels of the submodule +void startup_pwm_pair (IMXRT_FLEXPWM_t * flexpwm, int submodule) +{ + int submodule_mask = 1 << submodule ; + + flexpwm->OUTEN |= FLEXPWM_OUTEN_PWMA_EN (submodule_mask); // enable A output + flexpwm->OUTEN |= FLEXPWM_OUTEN_PWMB_EN (submodule_mask); // enable B output +} + + + +// PWM setting on the high and low pair of the PWM channels +void write_pwm_pair(IMXRT_FLEXPWM_t * flexpwm, int submodule, float duty){ + int mid_pwm = int((half_cycle)/2.0f); + int count_pwm = int(mid_pwm*(duty*2-1)) + mid_pwm; + + flexpwm->SM[submodule].VAL2 = -count_pwm; // A on + flexpwm->SM[submodule].VAL3 = count_pwm ; // A off + flexpwm->MCTRL |= FLEXPWM_MCTRL_LDOK (1<SM[submodule].VAL1; + int count_pwm = int(count*duty); + + SIMPLEFOC_DEBUG("VAL0: ",flexpwm->SM[submodule].VAL0); + SIMPLEFOC_DEBUG("VAL1: ",flexpwm->SM[submodule].VAL1); + SIMPLEFOC_DEBUG("count: ",count_pwm); + + // flexpwm->SM[submodule].VAL1 = 0; // A on + flexpwm->SM[submodule].VAL2 = count_pwm ; // A off + flexpwm->SM[submodule].VAL3 = count_pwm; // A on + flexpwm->SM[submodule].VAL4 = count_pwm ; // A off + flexpwm->SM[submodule].VAL5 = count_pwm ; // A off + flexpwm->MCTRL |= FLEXPWM_MCTRL_LDOK (1<SM[submodule].VAL1; + uint32_t cval = ((uint32_t)val * (modulo + 1)) >> analog_write_res; + if (cval > modulo) cval = modulo; // TODO: is this check correct? + + //printf("flexpwmWrite, p=%08lX, sm=%d, ch=%c, cval=%ld\n", + //(uint32_t)p, submodule, channel == 0 ? 'X' : (channel == 1 ? 'A' : 'B'), cval); + p->MCTRL |= FLEXPWM_MCTRL_CLDOK(mask); + switch (channel) { + case 0: // X + p->SM[submodule].VAL0 = modulo - cval; + p->OUTEN |= FLEXPWM_OUTEN_PWMX_EN(mask); + //printf(" write channel X\n"); + break; + case 1: // A + p->SM[submodule].VAL3 = cval; + p->OUTEN |= FLEXPWM_OUTEN_PWMA_EN(mask); + //printf(" write channel A\n"); + break; + case 2: // B + p->SM[submodule].VAL5 = cval; + p->OUTEN |= FLEXPWM_OUTEN_PWMB_EN(mask); + //printf(" write channel B\n"); + } + p->MCTRL |= FLEXPWM_MCTRL_LDOK(mask); +} + + +// function setting the high pwm frequency to the supplied pins +// - Stepper motor - 6PWM setting +// - hardware specific +void* _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) { + if(!pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; // default frequency 25khz + else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 50kHz max + + IMXRT_FLEXPWM_t *flexpwmA,*flexpwmB,*flexpwmC; + int submoduleA,submoduleB,submoduleC; + int inverted_channelA,inverted_channelB,inverted_channelC; + flexpwmA = get_flexpwm(pinA_h,pinA_l); + submoduleA = get_submodule(pinA_h,pinA_l); + inverted_channelA = get_inverted_channel(pinA_h,pinA_l); + flexpwmB = get_flexpwm(pinB_h,pinB_l); + submoduleB = get_submodule(pinB_h,pinB_l); + inverted_channelB = get_inverted_channel(pinB_h,pinB_l); + flexpwmC = get_flexpwm(pinC_h,pinC_l); + submoduleC = get_submodule(pinC_h,pinC_l); + inverted_channelC = get_inverted_channel(pinC_h,pinC_l); + + if((flexpwmA == nullptr) || (flexpwmB == nullptr) || (flexpwmC == nullptr) ){ +#ifdef SIMPLEFOC_TEENSY_DEBUG + SIMPLEFOC_DEBUG("TEENSY-DRV: ERR: Flextimer problem - failed driver config!"); +#endif + return SIMPLEFOC_DRIVER_INIT_FAILED; + } + if((submoduleA < 0) || (submoduleB < 0) || (submoduleC < 0) ){ +#ifdef SIMPLEFOC_TEENSY_DEBUG + SIMPLEFOC_DEBUG("TEENSY-DRV: ERR: Flextimer submodule problem - failed driver config!"); +#endif + return SIMPLEFOC_DRIVER_INIT_FAILED; + } + if((inverted_channelA < 0) || (inverted_channelB < 0) || (inverted_channelC < 0) ){ +#ifdef SIMPLEFOC_TEENSY_DEBUG + SIMPLEFOC_DEBUG("TEENSY-DRV: ERR: Flextimer channel problem - failed driver config!"); +#endif + return SIMPLEFOC_DRIVER_INIT_FAILED; + } + + + Teensy4DriverParams* params = new Teensy4DriverParams { + .pins = { pinA_h, pinA_l, pinB_h, pinB_l, pinC_h, pinC_l }, + .flextimers = { flexpwmA, flexpwmB, flexpwmC}, + .submodules = { submoduleA, submoduleB, submoduleC}, + .pwm_frequency = pwm_frequency, + .dead_zone = dead_zone + }; + + + // Configure FlexPWM units, each driving A/B pair, B inverted. + // full speed about 80kHz, prescale 2 (div by 4) gives 20kHz + setup_pwm_pair (flexpwmA, submoduleA, true, pwm_frequency, dead_zone) ; // this is the master, internally synced + setup_pwm_pair (flexpwmB, submoduleB, true, pwm_frequency, dead_zone) ; // others externally synced + setup_pwm_pair (flexpwmC, submoduleC, false, pwm_frequency, dead_zone) ; + delayMicroseconds (100) ; + + // turn on XBAR1 clock for all but stop mode + xbar_init() ; + + // // Connect trigger to synchronize all timers + xbar_connect (flexpwm_submodule_to_trig(flexpwmC, submoduleC), flexpwm_submodule_to_ext_sync(flexpwmA, submoduleA)) ; + xbar_connect (flexpwm_submodule_to_trig(flexpwmC, submoduleC), flexpwm_submodule_to_ext_sync(flexpwmB, submoduleB)) ; + + startup_pwm_pair (flexpwmA, submoduleA) ; + startup_pwm_pair (flexpwmB, submoduleB) ; + startup_pwm_pair (flexpwmC, submoduleC) ; + + + delayMicroseconds(50) ; + // config the pins 2/3/6/9/8/7 as their FLEXPWM alternates. + *portConfigRegister(pinA_h) = pwm_pin_info[pinA_h].muxval ; + *portConfigRegister(pinA_l) = pwm_pin_info[pinA_l].muxval ; + *portConfigRegister(pinB_h) = pwm_pin_info[pinB_h].muxval ; + *portConfigRegister(pinB_l) = pwm_pin_info[pinB_l].muxval ; + *portConfigRegister(pinC_h) = pwm_pin_info[pinC_h].muxval ; + *portConfigRegister(pinC_l) = pwm_pin_info[pinC_l].muxval ; + + return params; +} + + + +// function setting the pwm duty cycle to the hardware +// - Stepper motor - 6PWM setting +// - hardware specific +void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, PhaseState *phase_state, void* params){ + _UNUSED(phase_state); + write_pwm_pair (((Teensy4DriverParams*)params)->flextimers[0], ((Teensy4DriverParams*)params)->submodules[0], dc_a); + write_pwm_pair (((Teensy4DriverParams*)params)->flextimers[1], ((Teensy4DriverParams*)params)->submodules[1], dc_b); + write_pwm_pair (((Teensy4DriverParams*)params)->flextimers[2], ((Teensy4DriverParams*)params)->submodules[2], dc_c); +} + + +// function setting the high pwm frequency to the supplied pins +// - BLDC motor - 3PWM setting +// - hardware speciffic +// in generic case dont do anything + void* _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC) { + + if(!pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; // default frequency 25khz + else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 50kHz max + + IMXRT_FLEXPWM_t *flexpwmA,*flexpwmB,*flexpwmC; + int submoduleA,submoduleB,submoduleC; + int inverted_channelA,inverted_channelB,inverted_channelC; + flexpwmA = get_flexpwm(pinA); + submoduleA = get_submodule(pinA); + flexpwmB = get_flexpwm(pinB); + submoduleB = get_submodule(pinB); + flexpwmC = get_flexpwm(pinC); + submoduleC = get_submodule(pinC); + + Teensy4DriverParams* params = new Teensy4DriverParams { + .pins = { pinA, pinB, pinC }, + .flextimers = { flexpwmA, flexpwmB, flexpwmC}, + .submodules = { submoduleA, submoduleB, submoduleC}, + .pwm_frequency = pwm_frequency, + }; + + startup_pwm_pair (flexpwmA, submoduleA) ; + startup_pwm_pair (flexpwmB, submoduleB) ; + startup_pwm_pair (flexpwmC, submoduleC) ; + + // analogWriteFrequency(pinA, pwm_frequency); + // analogWriteFrequency(pinB, pwm_frequency); + // analogWriteFrequency(pinC, pwm_frequency); + // analogWrite(pinA, 0); + // analogWrite(pinB, 0); + // analogWrite(pinC, 0); + + // // config the pins 2/3/6/9/8/7 as their FLEXPWM alternates. + // *portConfigRegister(pinA) = pwm_pin_info[pinA].muxval ; + // *portConfigRegister(pinB) = pwm_pin_info[pinB].muxval ; + // *portConfigRegister(pinC) = pwm_pin_info[pinC].muxval ; + + return params; +} + +// function setting the pwm duty cycle to the hardware +// - Stepper motor - 6PWM setting +// - hardware specific +void _writeDutyCycle3PWM(float dc_a, float dc_b, float dc_c, void* params){ + write_pwm_on_pin (((Teensy4DriverParams*)params)->flextimers[0], ((Teensy4DriverParams*)params)->submodules[0], dc_a); + write_pwm_on_pin (((Teensy4DriverParams*)params)->flextimers[1], ((Teensy4DriverParams*)params)->submodules[1], dc_b); + write_pwm_on_pin (((Teensy4DriverParams*)params)->flextimers[2], ((Teensy4DriverParams*)params)->submodules[2], dc_c); +} + + +#endif \ No newline at end of file diff --git a/src/drivers/hardware_specific/teensy/teensy_mcu.cpp b/src/drivers/hardware_specific/teensy/teensy_mcu.cpp index dcfa3e15..2ad5f57c 100644 --- a/src/drivers/hardware_specific/teensy/teensy_mcu.cpp +++ b/src/drivers/hardware_specific/teensy/teensy_mcu.cpp @@ -2,6 +2,8 @@ #if defined(__arm__) && defined(CORE_TEENSY) +#include "../../../communication/SimpleFOCDebug.h" + // configure High PWM frequency void _setHighFrequency(const long freq, const int pin){ analogWrite(pin, 0); @@ -11,14 +13,15 @@ void _setHighFrequency(const long freq, const int pin){ // function setting the high pwm frequency to the supplied pins // - Stepper motor - 2PWM setting -// - hardware speciffic +// - hardware specific void* _configure1PWM(long pwm_frequency, const int pinA) { if(!pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; // default frequency 25khz else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 50kHz max _setHighFrequency(pwm_frequency, pinA); - GenericDriverParams* params = new GenericDriverParams { + TeensyDriverParams* params = new TeensyDriverParams { .pins = { pinA }, - .pwm_frequency = pwm_frequency + .pwm_frequency = pwm_frequency, + .additional_params = nullptr }; return params; } @@ -26,38 +29,53 @@ void* _configure1PWM(long pwm_frequency, const int pinA) { // function setting the high pwm frequency to the supplied pins // - Stepper motor - 2PWM setting -// - hardware speciffic +// - hardware specific void* _configure2PWM(long pwm_frequency, const int pinA, const int pinB) { if(!pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; // default frequency 25khz else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 50kHz max _setHighFrequency(pwm_frequency, pinA); _setHighFrequency(pwm_frequency, pinB); - GenericDriverParams* params = new GenericDriverParams { + TeensyDriverParams* params = new TeensyDriverParams { .pins = { pinA, pinB }, - .pwm_frequency = pwm_frequency + .pwm_frequency = pwm_frequency, + .additional_params = nullptr }; return params; } +// inital weak implementation of the center aligned 3pwm configuration +// teensy 4 and 3 have center aligned pwm +__attribute__((weak)) void* _configureCenterAligned3PMW(long pwm_frequency, const int pinA, const int pinB, const int pinC) { + return SIMPLEFOC_DRIVER_INIT_FAILED; +} + // function setting the high pwm frequency to the supplied pins // - BLDC motor - 3PWM setting -// - hardware speciffic +// - hardware specific void* _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC) { if(!pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; // default frequency 25khz else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 50kHz max - _setHighFrequency(pwm_frequency, pinA); - _setHighFrequency(pwm_frequency, pinB); - _setHighFrequency(pwm_frequency, pinC); - GenericDriverParams* params = new GenericDriverParams { - .pins = { pinA, pinB, pinC }, - .pwm_frequency = pwm_frequency - }; + + // try configuring center aligned pwm + void* p = _configureCenterAligned3PMW(pwm_frequency, pinA, pinB, pinC); + if(p != SIMPLEFOC_DRIVER_INIT_FAILED){ + return p; // if center aligned pwm is available return the params + }else{ // if center aligned pwm is not available use fast pwm + _setHighFrequency(pwm_frequency, pinA); + _setHighFrequency(pwm_frequency, pinB); + _setHighFrequency(pwm_frequency, pinC); + TeensyDriverParams* params = new TeensyDriverParams { + .pins = { pinA, pinB, pinC }, + .pwm_frequency = pwm_frequency, + .additional_params = nullptr + }; return params; + } } // function setting the high pwm frequency to the supplied pins // - Stepper motor - 4PWM setting -// - hardware speciffic +// - hardware specific void* _configure4PWM(long pwm_frequency, const int pinA, const int pinB, const int pinC, const int pinD) { if(!pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; // default frequency 25khz else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 50kHz max @@ -65,9 +83,10 @@ void* _configure4PWM(long pwm_frequency, const int pinA, const int pinB, const i _setHighFrequency(pwm_frequency, pinB); _setHighFrequency(pwm_frequency, pinC); _setHighFrequency(pwm_frequency, pinD); - GenericDriverParams* params = new GenericDriverParams { + TeensyDriverParams* params = new TeensyDriverParams { .pins = { pinA, pinB, pinC, pinD }, - .pwm_frequency = pwm_frequency + .pwm_frequency = pwm_frequency, + .additional_params = nullptr }; return params; } @@ -75,42 +94,57 @@ void* _configure4PWM(long pwm_frequency, const int pinA, const int pinB, const i // function setting the pwm duty cycle to the hardware // - Stepper motor - 2PWM setting -// - hardware speciffic +// - hardware specific void _writeDutyCycle1PWM(float dc_a, void* params) { // transform duty cycle from [0,1] to [0,255] - analogWrite(((GenericDriverParams*)params)->pins[0], 255.0f*dc_a); + analogWrite(((TeensyDriverParams*)params)->pins[0], 255.0f*dc_a); } // function setting the pwm duty cycle to the hardware // - Stepper motor - 2PWM setting -// - hardware speciffic +// - hardware specific void _writeDutyCycle2PWM(float dc_a, float dc_b, void* params) { // transform duty cycle from [0,1] to [0,255] - analogWrite(((GenericDriverParams*)params)->pins[0], 255.0f*dc_a); - analogWrite(((GenericDriverParams*)params)->pins[1], 255.0f*dc_b); + analogWrite(((TeensyDriverParams*)params)->pins[0], 255.0f*dc_a); + analogWrite(((TeensyDriverParams*)params)->pins[1], 255.0f*dc_b); +} + +// inital weak implementation of the center aligned 3pwm configuration +// teensy 4 and 3 have center aligned pwm implementation of this function +__attribute__((weak)) void _writeCenterAligned3PMW(float dc_a, float dc_b, float dc_c, void* params){ + _UNUSED(dc_a); + _UNUSED(dc_b); + _UNUSED(dc_c); + _UNUSED(params); } // function setting the pwm duty cycle to the hardware // - BLDC motor - 3PWM setting -// - hardware speciffic +// - hardware specific void _writeDutyCycle3PWM(float dc_a, float dc_b, float dc_c, void* params){ - // transform duty cycle from [0,1] to [0,255] - analogWrite(((GenericDriverParams*)params)->pins[0], 255.0f*dc_a); - analogWrite(((GenericDriverParams*)params)->pins[1], 255.0f*dc_b); - analogWrite(((GenericDriverParams*)params)->pins[2], 255.0f*dc_c); + + TeensyDriverParams* p = (TeensyDriverParams*)params; + if(p->additional_params != nullptr){ + _writeCenterAligned3PMW(dc_a, dc_b, dc_c, p); + }else{ + // transform duty cycle from [0,1] to [0,255] + analogWrite(p->pins[0], 255.0f*dc_a); + analogWrite(p->pins[1], 255.0f*dc_b); + analogWrite(p->pins[2], 255.0f*dc_c); + } } // function setting the pwm duty cycle to the hardware // - Stepper motor - 4PWM setting -// - hardware speciffic +// - hardware specific void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, void* params){ // transform duty cycle from [0,1] to [0,255] - analogWrite(((GenericDriverParams*)params)->pins[0], 255.0f*dc_1a); - analogWrite(((GenericDriverParams*)params)->pins[1], 255.0f*dc_1b); - analogWrite(((GenericDriverParams*)params)->pins[2], 255.0f*dc_2a); - analogWrite(((GenericDriverParams*)params)->pins[3], 255.0f*dc_2b); + analogWrite(((TeensyDriverParams*)params)->pins[0], 255.0f*dc_1a); + analogWrite(((TeensyDriverParams*)params)->pins[1], 255.0f*dc_1b); + analogWrite(((TeensyDriverParams*)params)->pins[2], 255.0f*dc_2a); + analogWrite(((TeensyDriverParams*)params)->pins[3], 255.0f*dc_2b); } #endif \ No newline at end of file diff --git a/src/drivers/hardware_specific/teensy/teensy_mcu.h b/src/drivers/hardware_specific/teensy/teensy_mcu.h index 7956ea90..266f4b69 100644 --- a/src/drivers/hardware_specific/teensy/teensy_mcu.h +++ b/src/drivers/hardware_specific/teensy/teensy_mcu.h @@ -1,3 +1,6 @@ +#ifndef TEENSY_MCU_DRIVER_H +#define TEENSY_MCU_DRIVER_H + #include "../../hardware_api.h" #if defined(__arm__) && defined(CORE_TEENSY) @@ -8,7 +11,17 @@ // debugging output #define SIMPLEFOC_TEENSY_DEBUG +typedef struct TeensyDriverParams { + int pins[6] = {(int)NOT_SET}; + long pwm_frequency; + void* additional_params; +} TeensyDriverParams; + // configure High PWM frequency void _setHighFrequency(const long freq, const int pin); +void* _configureCenterAligned3PMW(long pwm_frequency, const int pinA, const int pinB, const int pinC); +void _writeCenterAligned3PMW(float dc_a, float dc_b, float dc_c, void* params); + +#endif #endif \ No newline at end of file From 238d26b63fb88fe77042055bb410a387f18f9a49 Mon Sep 17 00:00:00 2001 From: askuric Date: Thu, 29 Feb 2024 12:54:32 +0100 Subject: [PATCH 5/9] better error messages --- .../hardware_specific/teensy/teensy4_mcu.cpp | 15 +++++++-------- .../hardware_specific/teensy/teensy_mcu.cpp | 1 + 2 files changed, 8 insertions(+), 8 deletions(-) diff --git a/src/drivers/hardware_specific/teensy/teensy4_mcu.cpp b/src/drivers/hardware_specific/teensy/teensy4_mcu.cpp index 6815c10f..0e7953ff 100644 --- a/src/drivers/hardware_specific/teensy/teensy4_mcu.cpp +++ b/src/drivers/hardware_specific/teensy/teensy4_mcu.cpp @@ -87,7 +87,8 @@ void xbar_init() { IMXRT_FLEXPWM_t* get_flexpwm(uint8_t pin){ const struct pwm_pin_info_struct *info; - if (pin >= CORE_NUM_DIGITAL) { + info = pwm_pin_info + pin; + if (pin >= CORE_NUM_DIGITAL || info->type == 2) { #ifdef SIMPLEFOC_TEENSY_DEBUG char s[60]; sprintf (s, "TEENSY-DRV: ERR: Pin: %d not Flextimer pin!", pin); @@ -95,7 +96,6 @@ IMXRT_FLEXPWM_t* get_flexpwm(uint8_t pin){ #endif return nullptr; } - info = pwm_pin_info + pin; // FlexPWM pin IMXRT_FLEXPWM_t *flexpwm; switch ((info->module >> 4) & 3) { @@ -236,17 +236,16 @@ int get_submodule(uint8_t pin, uint8_t pin1){ // 1 - A // 2 - B int get_channel(uint8_t pin){ - const struct pwm_pin_info_struct *info; - if (pin >= CORE_NUM_DIGITAL){ + info = pwm_pin_info + pin; + if (pin >= CORE_NUM_DIGITAL || info->type == 2){ #ifdef SIMPLEFOC_TEENSY_DEBUG - char s[60]; + char s[90]; sprintf (s, "TEENSY-DRV: ERR: Pin: %d not Flextimer pin!", pin); SIMPLEFOC_DEBUG(s); #endif return -1; } - info = pwm_pin_info + pin; #ifdef SIMPLEFOC_TEENSY_DEBUG char s[60]; sprintf (s, "TEENSY-DRV: Pin: %d on channel %s.", pin, info->channel==0 ? "X" : info->channel==1 ? "A" : "B"); @@ -598,7 +597,7 @@ void write_pwm_on_pin(IMXRT_FLEXPWM_t *p, unsigned int submodule, uint8_t channe // we can configure the center-aligned pwm if((flexpwmA != nullptr) && (flexpwmB != nullptr) && (flexpwmC != nullptr) && (channelA > 0) && (channelB > 0) && (channelC > 0) ){ #ifdef SIMPLEFOC_TEENSY_DEBUG - SIMPLEFOC_DEBUG("TEENSY-DRV: All pins on Flexpwm A or B submodules - Configuring center-aligned pwm!"); + SIMPLEFOC_DEBUG("TEENSY-DRV: All pins on Flexpwm A or B channels - Configuring center-aligned pwm!"); #endif // Configure FlexPWM units @@ -645,7 +644,7 @@ void write_pwm_on_pin(IMXRT_FLEXPWM_t *p, unsigned int submodule, uint8_t channe return params; }else{ #ifdef SIMPLEFOC_TEENSY_DEBUG - SIMPLEFOC_DEBUG("TEENSY-DRV: Not all pins on Flexpwm A and B submodules - cannot configure center-aligned pwm!"); + SIMPLEFOC_DEBUG("TEENSY-DRV: Not all pins on Flexpwm A and B channels - cannot configure center-aligned pwm!"); #endif return SIMPLEFOC_DRIVER_INIT_FAILED; } diff --git a/src/drivers/hardware_specific/teensy/teensy_mcu.cpp b/src/drivers/hardware_specific/teensy/teensy_mcu.cpp index 2ad5f57c..196f07fd 100644 --- a/src/drivers/hardware_specific/teensy/teensy_mcu.cpp +++ b/src/drivers/hardware_specific/teensy/teensy_mcu.cpp @@ -61,6 +61,7 @@ void* _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const in if(p != SIMPLEFOC_DRIVER_INIT_FAILED){ return p; // if center aligned pwm is available return the params }else{ // if center aligned pwm is not available use fast pwm + SIMPLEFOC_DEBUG("TEENSY-DRV: Configuring 3PWM with fast pwm. Please consider using center aligned pwm for better performance!"); _setHighFrequency(pwm_frequency, pinA); _setHighFrequency(pwm_frequency, pinB); _setHighFrequency(pwm_frequency, pinC); From d48f4d795591b688470c791509f7117bb745dd0a Mon Sep 17 00:00:00 2001 From: askuric Date: Fri, 19 Apr 2024 07:15:21 +0200 Subject: [PATCH 6/9] added support for debugging with char --- src/communication/SimpleFOCDebug.cpp | 6 ++++++ src/communication/SimpleFOCDebug.h | 1 + 2 files changed, 7 insertions(+) diff --git a/src/communication/SimpleFOCDebug.cpp b/src/communication/SimpleFOCDebug.cpp index e969d8a2..3bb62bce 100644 --- a/src/communication/SimpleFOCDebug.cpp +++ b/src/communication/SimpleFOCDebug.cpp @@ -58,6 +58,12 @@ void SimpleFOCDebug::println(const char* str, int val) { _debugPrint->println(val); } } +void SimpleFOCDebug::println(const char* str, char val) { + if (_debugPrint != NULL) { + _debugPrint->print(str); + _debugPrint->println(val); + } +} void SimpleFOCDebug::println(const __FlashStringHelper* str, int val) { if (_debugPrint != NULL) { diff --git a/src/communication/SimpleFOCDebug.h b/src/communication/SimpleFOCDebug.h index 614e6371..4fcfd538 100644 --- a/src/communication/SimpleFOCDebug.h +++ b/src/communication/SimpleFOCDebug.h @@ -45,6 +45,7 @@ class SimpleFOCDebug { static void println(const char* msg, float val); static void println(const __FlashStringHelper* msg, int val); static void println(const char* msg, int val); + static void println(const char* msg, char val); static void println(); static void println(int val); static void println(float val); From c171a019114277546f8fcb57240f67bf32d51bb3 Mon Sep 17 00:00:00 2001 From: askuric Date: Fri, 19 Apr 2024 07:27:32 +0200 Subject: [PATCH 7/9] renamed the flag to force_center_aligend_3pwm --- src/drivers/hardware_specific/teensy/teensy4_mcu.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/drivers/hardware_specific/teensy/teensy4_mcu.cpp b/src/drivers/hardware_specific/teensy/teensy4_mcu.cpp index 0e7953ff..060ff4e2 100644 --- a/src/drivers/hardware_specific/teensy/teensy4_mcu.cpp +++ b/src/drivers/hardware_specific/teensy/teensy4_mcu.cpp @@ -10,7 +10,7 @@ #pragma message("SimpleFOC: compiling for Teensy 4.x") #pragma message("") -// #define AVOID_TEENSY4_CENTER_ALIGNED_3PWM +// #define TEENSY4_FORCE_CENTER_ALIGNED_3PWM // function finding the TRIG event given the flexpwm timer and the submodule @@ -83,7 +83,7 @@ void xbar_init() { } // function which finds the flexpwm instance for a pin -// if it does not belong to the flexpwm timer it returns a nullpointer +// if it does not belong to the flexpwm timer it returns a null-pointer IMXRT_FLEXPWM_t* get_flexpwm(uint8_t pin){ const struct pwm_pin_info_struct *info; @@ -569,7 +569,7 @@ void write_pwm_on_pin(IMXRT_FLEXPWM_t *p, unsigned int submodule, uint8_t channe p->MCTRL |= FLEXPWM_MCTRL_LDOK(mask); } -#ifndef AVOID_TEENSY4_CENTER_ALIGNED_3PWM +#ifnef TEENSY4_FORCE_CENTER_ALIGNED_3PWM // function setting the high pwm frequency to the supplied pins // - BLDC motor - 3PWM setting From 82da85726a59d141a96ecebd06699a3f050aa0f3 Mon Sep 17 00:00:00 2001 From: askuric Date: Fri, 19 Apr 2024 07:28:52 +0200 Subject: [PATCH 8/9] small restructure of commented code --- src/current_sense/hardware_specific/teensy/teensy4_mcu.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/current_sense/hardware_specific/teensy/teensy4_mcu.cpp b/src/current_sense/hardware_specific/teensy/teensy4_mcu.cpp index e5934920..564f4f46 100644 --- a/src/current_sense/hardware_specific/teensy/teensy4_mcu.cpp +++ b/src/current_sense/hardware_specific/teensy/teensy4_mcu.cpp @@ -12,9 +12,9 @@ // #define TEENSY4_ADC_INTERRUPT_DEBUG -// #define _BANDWIDTH_CS 10000.0f // [Hz] bandwidth for the current sense volatile uint32_t val0, val1, val2; +// #define _BANDWIDTH_CS 10000.0f // [Hz] bandwidth for the current sense // LowPassFilter lp1 = LowPassFilter(1.0/_BANDWIDTH_CS); // LowPassFilter lp2 = LowPassFilter(1.0/_BANDWIDTH_CS); // LowPassFilter lp3 = LowPassFilter(1.0/_BANDWIDTH_CS); From d6a0b26d72158ba944a3089b511c960e9afe875f Mon Sep 17 00:00:00 2001 From: askuric Date: Fri, 19 Apr 2024 07:30:51 +0200 Subject: [PATCH 9/9] a small typo --- src/drivers/hardware_specific/teensy/teensy4_mcu.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/hardware_specific/teensy/teensy4_mcu.cpp b/src/drivers/hardware_specific/teensy/teensy4_mcu.cpp index 060ff4e2..7c4d0dbb 100644 --- a/src/drivers/hardware_specific/teensy/teensy4_mcu.cpp +++ b/src/drivers/hardware_specific/teensy/teensy4_mcu.cpp @@ -569,7 +569,7 @@ void write_pwm_on_pin(IMXRT_FLEXPWM_t *p, unsigned int submodule, uint8_t channe p->MCTRL |= FLEXPWM_MCTRL_LDOK(mask); } -#ifnef TEENSY4_FORCE_CENTER_ALIGNED_3PWM +#ifdef TEENSY4_FORCE_CENTER_ALIGNED_3PWM // function setting the high pwm frequency to the supplied pins // - BLDC motor - 3PWM setting