Skip to content

Commit

Permalink
added teensy 4 6pwm tiemer sync and better support for low-side curre…
Browse files Browse the repository at this point in the history 
…nt sensing
  • Loading branch information
@askuric
askuric committed Feb 26, 2024
1 parent 81db3f5 commit 0588d38
Show file tree
Hide file tree
Showing 3 changed files with 207 additions and 62 deletions.
94 changes: 45 additions & 49 deletions src/current_sense/hardware_specific/teensy/teensy4_mcu.cpp
View file
Original file line number Diff line number Diff line change
@@ -1,34 +1,23 @@
#include "teensy4_mcu.h"
#include "../../../drivers/hardware_specific/teensy/teensy4_mcu.h"
// #include "../../../common/lowpass_filter.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) )

// 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;
Expand All @@ -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
Expand All @@ -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
Expand All @@ -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
Expand Down Expand Up @@ -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
Expand All @@ -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);
Expand All @@ -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();
Expand All @@ -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

}

Expand Down
Loading

0 comments on commit 0588d38

Please sign in to comment.