current_sense/hardware_specific/teensy/teensy4_mcu.cpp

Go to the documentation of this file.

#include "teensy4_mcu.h"
#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 
// - Teensy 4.1 
#if defined(__arm__) && defined(CORE_TEENSY) && ( defined(__IMXRT1062__) || defined(ARDUINO_TEENSY40) || defined(ARDUINO_TEENSY41) || defined(ARDUINO_TEENSY_MICROMOD) )
 
// #define SIMPLEFOC_TEENSY4_ADC_INTERRUPT_DEBUG
 
 
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);
 
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() {
#ifdef SIMPLEFOC_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 = 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;
#ifdef SIMPLEFOC_TEENSY4_ADC_INTERRUPT_DEBUG
  digitalWrite(30,LOW);
#endif
}
 
 
void adcetc1_isr() {
#ifdef SIMPLEFOC_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 = lp3( ADC_ETC_TRIG0_RESULT_3_2  & 4095);
#ifdef SIMPLEFOC_TEENSY4_ADC_INTERRUPT_DEBUG
  digitalWrite(30,LOW);
#endif
}
 
// function initializing the ADC2
// and the ADC_ETC trigger for the low side current sensing
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 / 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
 
 
    //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)
    if(_isset(pin3)) {      
      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 / 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
 
    //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;
}
 
// 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 = 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
    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)
    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);
    }
}
 
 
 
// 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;
}
 
// 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){
  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 SIMPLEFOC_TEENSY4_ADC_INTERRUPT_DEBUG
  pinMode(30,OUTPUT);
#endif
 
  if( _isset(pinA) ) pinMode(pinA, INPUT);
  if( _isset(pinB) ) pinMode(pinB, INPUT);
  if( _isset(pinC) ) pinMode(pinC, INPUT);
 
  // 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]);
  adc_etc_init(pins[0], pins[1], pins[2]);
 
  xbar_init();
 
  GenericCurrentSenseParams* params = new GenericCurrentSenseParams {
    .pins = {pins[0], pins[1], pins[2] },
    .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*) ((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;
 
    // 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);
    // 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(flexpwm->SM[submodule].VAL1*(1.0f - 2.5e-6*par->pwm_frequency))  ; // 2.5us before center 
 
 
#ifdef SIMPLEFOC_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
 
  
  // return the cs parameters 
  // successfully initialized
  // TODO verify if success in future
  return cs_params;
}
 
 
#endif