added teensy 4 6pwm tiemer sync and better support for low-side current sensing

Author: askuric

src/current_sense/hardware_specific/teensy/teensy4_mcu.cpp

@@ -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;
@@ -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
 
 }