Merge pull request #392 from simplefoc/feat_teensy4_lowside

Feat teensy4 lowside

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) {

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);

src/current_sense/hardware_specific/teensy/teensy4_mcu.cpp

@@ -0,0 +1,244 @@
+#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 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 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 TEENSY4_ADC_INTERRUPT_DEBUG
+  digitalWrite(30,LOW);
+#endif
+}
+
+
+void adcetc1_isr() {
+#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 = lp3( ADC_ETC_TRIG0_RESULT_3_2  & 4095);
+#ifdef 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 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 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
+
+}
+
+
+#endif

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

src/current_sense/hardware_specific/teensy_mcu.cpp → 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)