Skip to content

Commit 782e719

Browse files
askuricaskuric
authored
Merge pull request #392 from simplefoc/feat_teensy4_lowside
Feat teensy4 lowside
2 parents 57ec89d + d6a0b26 commit 782e719

File tree

10 files changed

+1367
-73
lines changed

10 files changed

+1367
-73
lines changed

‎src/communication/SimpleFOCDebug.cpp‎

Lines changed: 6 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -58,6 +58,12 @@ void SimpleFOCDebug::println(const char* str, int val) {
5858
_debugPrint->println(val);
5959
}
6060
}
61+
void SimpleFOCDebug::println(const char* str, char val) {
62+
if (_debugPrint != NULL) {
63+
_debugPrint->print(str);
64+
_debugPrint->println(val);
65+
}
66+
}
6167

6268
void SimpleFOCDebug::println(const __FlashStringHelper* str, int val) {
6369
if (_debugPrint != NULL) {

‎src/communication/SimpleFOCDebug.h‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -45,6 +45,7 @@ class SimpleFOCDebug {
4545
static void println(const char* msg, float val);
4646
static void println(const __FlashStringHelper* msg, int val);
4747
static void println(const char* msg, int val);
48+
static void println(const char* msg, char val);
4849
static void println();
4950
static void println(int val);
5051
static void println(float val);

‎src/current_sense/hardware_specific/teensy/teensy4_mcu.cpp‎

Lines changed: 244 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,244 @@
1+
#include "teensy4_mcu.h"
2+
#include "../../../drivers/hardware_specific/teensy/teensy4_mcu.h"
3+
// #include "../../../common/lowpass_filter.h"
4+
#include "../../../common/foc_utils.h"
5+
#include "../../../communication/SimpleFOCDebug.h"
6+
7+
// if defined
8+
// - Teensy 4.0
9+
// - Teensy 4.1
10+
#if defined(__arm__) && defined(CORE_TEENSY) && ( defined(__IMXRT1062__) || defined(ARDUINO_TEENSY40) || defined(ARDUINO_TEENSY41) || defined(ARDUINO_TEENSY_MICROMOD) )
11+
12+
// #define TEENSY4_ADC_INTERRUPT_DEBUG
13+
14+
15+
volatile uint32_t val0, val1, val2;
16+
17+
// #define _BANDWIDTH_CS 10000.0f // [Hz] bandwidth for the current sense
18+
// LowPassFilter lp1 = LowPassFilter(1.0/_BANDWIDTH_CS);
19+
// LowPassFilter lp2 = LowPassFilter(1.0/_BANDWIDTH_CS);
20+
// LowPassFilter lp3 = LowPassFilter(1.0/_BANDWIDTH_CS);
21+
22+
void read_currents(uint32_t *a, uint32_t*b, uint32_t *c=nullptr){
23+
*a = val0;
24+
*b = val1;
25+
*c = val2;
26+
}
27+
28+
// interrupt service routine for the ADC_ETC0
29+
// reading the ADC values and clearing the interrupt
30+
void adcetc0_isr() {
31+
#ifdef TEENSY4_ADC_INTERRUPT_DEBUG
32+
digitalWrite(30,HIGH);
33+
#endif
34+
// page 3509 , section 66.5.1.3.3
35+
ADC_ETC_DONE0_1_IRQ |= 1; // clear Done0 for trg0 at 1st bit
36+
// val0 = lp1(ADC_ETC_TRIG0_RESULT_1_0 & 4095);
37+
val0 = (ADC_ETC_TRIG0_RESULT_1_0 & 4095);
38+
// val1 = lp2((ADC_ETC_TRIG0_RESULT_1_0 >> 16) & 4095);
39+
val1 = (ADC_ETC_TRIG0_RESULT_1_0 >> 16) & 4095;
40+
#ifdef TEENSY4_ADC_INTERRUPT_DEBUG
41+
digitalWrite(30,LOW);
42+
#endif
43+
}
44+
45+
46+
void adcetc1_isr() {
47+
#ifdef TEENSY4_ADC_INTERRUPT_DEBUG
48+
digitalWrite(30,HIGH);
49+
#endif
50+
// page 3509 , section 66.5.1.3.3
51+
ADC_ETC_DONE0_1_IRQ |= 1 << 16; // clear Done1 for trg0 at 16th bit
52+
val2 = ADC_ETC_TRIG0_RESULT_3_2 & 4095;
53+
// val2 = lp3( ADC_ETC_TRIG0_RESULT_3_2 & 4095);
54+
#ifdef TEENSY4_ADC_INTERRUPT_DEBUG
55+
digitalWrite(30,LOW);
56+
#endif
57+
}
58+
59+
// function initializing the ADC2
60+
// and the ADC_ETC trigger for the low side current sensing
61+
void adc1_init(int pin1, int pin2, int pin3=NOT_SET) {
62+
//Tried many configurations, but this seems to be best:
63+
ADC1_CFG = ADC_CFG_OVWREN //Allow overwriting of the next converted Data onto the existing
64+
| ADC_CFG_ADICLK(0) // input clock select - IPG clock
65+
| ADC_CFG_MODE(2) // 12-bit conversion 0 8-bit conversion 1 10-bit conversion 2 12-bit conversion
66+
| 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)
67+
| ADC_CFG_ADSTS(0) // Sample period (ADC clocks) = 3 if ADLSMP=0b
68+
| ADC_CFG_ADHSC // High speed operation
69+
| ADC_CFG_ADTRG; // Hardware trigger selected
70+
71+
72+
//Calibration of ADC1
73+
ADC1_GC |= ADC_GC_CAL; // begin cal ADC1
74+
while (ADC1_GC & ADC_GC_CAL) ;
75+
76+
ADC1_HC0 = 16; // ADC_ETC channel
77+
// use the second interrupt if necessary (for more than 2 channels)
78+
if(_isset(pin3)) {
79+
ADC1_HC1 = 16;
80+
}
81+
}
82+
83+
// function initializing the ADC2
84+
// and the ADC_ETC trigger for the low side current sensing
85+
void adc2_init(){
86+
87+
// configuring ADC2
88+
//Tried many configurations, but this seems to be best:
89+
ADC1_CFG = ADC_CFG_OVWREN //Allow overwriting of the next converted Data onto the existing
90+
| ADC_CFG_ADICLK(0) // input clock select - IPG clock
91+
| ADC_CFG_MODE(2) // 12-bit conversion 0 8-bit conversion 1 10-bit conversion 2 12-bit conversion
92+
| ADC_CFG_ADIV(2) // Input clock / 2 (0 for /1, 1 for /2 and 2 for / 4)
93+
| ADC_CFG_ADSTS(0) // Sample period (ADC clocks) = 3 if ADLSMP=0b
94+
| ADC_CFG_ADHSC // High speed operation
95+
| ADC_CFG_ADTRG; // Hardware trigger selected
96+
97+
//Calibration of ADC2
98+
ADC2_GC |= ADC_GC_CAL; // begin cal ADC2
99+
while (ADC2_GC & ADC_GC_CAL) ;
100+
101+
ADC2_HC0 = 16; // ADC_ETC channel
102+
// use the second interrupt if necessary (for more than 2 channels)
103+
// ADC2_HC1 = 16;
104+
}
105+
106+
// function initializing the ADC_ETC trigger for the low side current sensing
107+
// it uses only the ADC1
108+
// if the pin3 is not set it uses only 2 channels
109+
void adc_etc_init(int pin1, int pin2, int pin3=NOT_SET) {
110+
ADC_ETC_CTRL &= ~(1 << 31); // SOFTRST
111+
ADC_ETC_CTRL = 0x40000001; // start with trigger 0
112+
ADC_ETC_TRIG0_CTRL = ADC_ETC_TRIG_CTRL_TRIG_CHAIN( _isset(pin3) ? 2 : 1) ; // 2 if 3 channels, 1 if 2 channels
113+
114+
// ADC1 7 8, chain channel, HWTS, IE, B2B
115+
// pg 3516, section 66.5.1.8
116+
ADC_ETC_TRIG0_CHAIN_1_0 =
117+
ADC_ETC_TRIG_CHAIN_IE1(0) | // no interrupt on first or set 2 if interrupt when Done1
118+
ADC_ETC_TRIG_CHAIN_B2B1 | // Enable B2B, back to back ADC trigger
119+
ADC_ETC_TRIG_CHAIN_HWTS1(1) |
120+
ADC_ETC_TRIG_CHAIN_CSEL1(pin_to_channel[pin1]) | // ADC channel 8
121+
ADC_ETC_TRIG_CHAIN_IE0(1) | // interrupt when Done0
122+
ADC_ETC_TRIG_CHAIN_B2B1 | // Enable B2B, back to back ADC trigger
123+
ADC_ETC_TRIG_CHAIN_HWTS0(1) |
124+
ADC_ETC_TRIG_CHAIN_CSEL0(pin_to_channel[pin2]); // ADC channel 7
125+
126+
attachInterruptVector(IRQ_ADC_ETC0, adcetc0_isr);
127+
NVIC_ENABLE_IRQ(IRQ_ADC_ETC0);
128+
// use the second interrupt if necessary (for more than 2 channels)
129+
if(_isset(pin3)) {
130+
ADC_ETC_TRIG0_CHAIN_3_2 =
131+
ADC_ETC_TRIG_CHAIN_IE0(2) | // interrupt when Done1
132+
ADC_ETC_TRIG_CHAIN_B2B0 | // Enable B2B, back to back ADC trigger
133+
ADC_ETC_TRIG_CHAIN_HWTS0(1) |
134+
ADC_ETC_TRIG_CHAIN_CSEL0(pin_to_channel[pin3]);
135+
136+
attachInterruptVector(IRQ_ADC_ETC1, adcetc1_isr);
137+
NVIC_ENABLE_IRQ(IRQ_ADC_ETC1);
138+
}
139+
}
140+
141+
142+
143+
// function reading an ADC value and returning the read voltage
144+
float _readADCVoltageLowSide(const int pinA, const void* cs_params){
145+
146+
if(!_isset(pinA)) return 0.0; // if the pin is not set return 0
147+
GenericCurrentSenseParams* params = (GenericCurrentSenseParams*) cs_params;
148+
float adc_voltage_conv = params->adc_voltage_conv;
149+
if (pinA == params->pins[0]) {
150+
return val0 * adc_voltage_conv;
151+
} else if (pinA == params->pins[1]) {
152+
return val1 * adc_voltage_conv;
153+
}else if (pinA == params->pins[2]) {
154+
return val2 * adc_voltage_conv;
155+
}
156+
return 0.0;
157+
}
158+
159+
// Configure low side for generic mcu
160+
// cannot do much but
161+
void* _configureADCLowSide(const void* driver_params, const int pinA,const int pinB,const int pinC){
162+
Teensy4DriverParams* par = (Teensy4DriverParams*) ((TeensyDriverParams*)driver_params)->additional_params;
163+
if(par == nullptr){
164+
SIMPLEFOC_DEBUG("TEENSY-CS: Low side current sense failed, driver not supported!");
165+
return SIMPLEFOC_CURRENT_SENSE_INIT_FAILED;
166+
}
167+
168+
SIMPLEFOC_DEBUG("TEENSY-CS: Configuring low side current sense!");
169+
170+
#ifdef TEENSY4_ADC_INTERRUPT_DEBUG
171+
pinMode(30,OUTPUT);
172+
#endif
173+
174+
if( _isset(pinA) ) pinMode(pinA, INPUT);
175+
if( _isset(pinB) ) pinMode(pinB, INPUT);
176+
if( _isset(pinC) ) pinMode(pinC, INPUT);
177+
178+
// check if either of the pins are not set
179+
// and dont use it if it isn't
180+
int pin_count = 0;
181+
int pins[3] = {NOT_SET, NOT_SET, NOT_SET};
182+
if(_isset(pinA)) pins[pin_count++] = pinA;
183+
if(_isset(pinB)) pins[pin_count++] = pinB;
184+
if(_isset(pinC)) pins[pin_count++] = pinC;
185+
186+
187+
adc1_init(pins[0], pins[1], pins[2]);
188+
adc_etc_init(pins[0], pins[1], pins[2]);
189+
190+
xbar_init();
191+
192+
GenericCurrentSenseParams* params = new GenericCurrentSenseParams {
193+
.pins = {pins[0], pins[1], pins[2] },
194+
.adc_voltage_conv = (_ADC_VOLTAGE)/(_ADC_RESOLUTION)
195+
};
196+
return params;
197+
}
198+
199+
// sync driver and the adc
200+
void _driverSyncLowSide(void* driver_params, void* cs_params){
201+
Teensy4DriverParams* par = (Teensy4DriverParams*) ((TeensyDriverParams*)driver_params)->additional_params;
202+
IMXRT_FLEXPWM_t* flexpwm = par->flextimers[0];
203+
int submodule = par->submodules[0];
204+
205+
SIMPLEFOC_DEBUG("TEENSY-CS: Syncing low side current sense!");
206+
char buff[50];
207+
sprintf(buff, "TEENSY-CS: Syncing to FlexPWM: %d, Submodule: %d", flexpwm_to_index(flexpwm), submodule);
208+
SIMPLEFOC_DEBUG(buff);
209+
210+
// find the xbar trigger for the flexpwm
211+
int xbar_trig_pwm = flexpwm_submodule_to_trig(flexpwm, submodule);
212+
if(xbar_trig_pwm<0) return;
213+
214+
// allow theFlexPWM to trigger the ADC_ETC
215+
xbar_connect((uint32_t)xbar_trig_pwm, XBARA1_OUT_ADC_ETC_TRIG00); //FlexPWM to adc_etc
216+
217+
// setup the ADC_ETC trigger to be triggered by the FlexPWM channel 1 (val1)
218+
//This val1 interrupt on match is in the center of the PWM
219+
flexpwm->SM[submodule].TCTRL = FLEXPWM_SMTCTRL_OUT_TRIG_EN(1<<1);
220+
221+
222+
// if needed the interrupt can be moved to some other point in the PWM cycle by using an addional val register example: VAL4
223+
// setup the ADC_ETC trigger to be triggered by the FlexPWM channel 4 (val4)
224+
// flexpwm->SM[submodule].TCTRL = FLEXPWM_SMTCTRL_OUT_TRIG_EN(1<<4);
225+
// setup this val4 for interrupt on match for ADC sync
226+
// this code assumes that the val4 is not used for anything else!
227+
// reading two ADC takes about 2.5us. So put the interrupt 2.5us befor the center
228+
// flexpwm->SM[submodule].VAL4 = int(flexpwm->SM[submodule].VAL1*(1.0f - 2.5e-6*par->pwm_frequency)) ; // 2.5us before center
229+
230+
231+
#ifdef TEENSY4_ADC_INTERRUPT_DEBUG
232+
// pin 4 observes out trigger line for 'scope
233+
xbar_connect (xbar_trig_pwm, XBARA1_OUT_IOMUX_XBAR_INOUT08) ;
234+
IOMUXC_GPR_GPR6 |= IOMUXC_GPR_GPR6_IOMUXC_XBAR_DIR_SEL_8 ; // select output mode for INOUT8
235+
// Select alt 3 for EMC_06 (XBAR), rather than original 5 (GPIO)
236+
CORE_PIN4_CONFIG = 3 ; // shorthand for IOMUXC_SW_MUX_CTL_PAD_GPIO_EMC_06 = 3 ;
237+
// turn up drive & speed as very short pulse
238+
IOMUXC_SW_PAD_CTL_PAD_GPIO_EMC_06 = IOMUXC_PAD_DSE(7) | IOMUXC_PAD_SPEED(3) | IOMUXC_PAD_SRE ;
239+
#endif
240+
241+
}
242+
243+
244+
#endif

‎src/current_sense/hardware_specific/teensy/teensy4_mcu.h‎

Lines changed: 77 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,77 @@
1+
2+
#ifndef TEENSY4_CURRENTSENSE_MCU_DEF
3+
#define TEENSY4_CURRENTSENSE_MCU_DEF
4+
5+
#include "../../hardware_api.h"
6+
#include "../../../common/foc_utils.h"
7+
8+
// if defined
9+
// - Teensy 4.0
10+
// - Teensy 4.1
11+
#if defined(__arm__) && defined(CORE_TEENSY) && ( defined(__IMXRT1062__) || defined(ARDUINO_TEENSY40) || defined(ARDUINO_TEENSY41) || defined(ARDUINO_TEENSY_MICROMOD) )
12+
13+
#define _ADC_VOLTAGE 3.3f
14+
#define _ADC_RESOLUTION 4026.0f
15+
16+
// generic implementation of the hardware specific structure
17+
// containing all the necessary current sense parameters
18+
// will be returned as a void pointer from the _configureADCx functions
19+
// will be provided to the _readADCVoltageX() as a void pointer
20+
typedef struct Teensy4CurrentSenseParams {
21+
int pins[3] = {(int)NOT_SET};
22+
float adc_voltage_conv;
23+
} Teensy4CurrentSenseParams;
24+
25+
26+
27+
const uint8_t pin_to_channel[] = { // pg 482
28+
7, // 0/A0 AD_B1_02
29+
8, // 1/A1 AD_B1_03
30+
12, // 2/A2 AD_B1_07
31+
11, // 3/A3 AD_B1_06
32+
6, // 4/A4 AD_B1_01
33+
5, // 5/A5 AD_B1_00
34+
15, // 6/A6 AD_B1_10
35+
0, // 7/A7 AD_B1_11
36+
13, // 8/A8 AD_B1_08
37+
14, // 9/A9 AD_B1_09
38+
1, // 24/A10 AD_B0_12
39+
2, // 25/A11 AD_B0_13
40+
128+3, // 26/A12 AD_B1_14 - only on ADC2, 3
41+
128+4, // 27/A13 AD_B1_15 - only on ADC2, 4
42+
7, // 14/A0 AD_B1_02
43+
8, // 15/A1 AD_B1_03
44+
12, // 16/A2 AD_B1_07
45+
11, // 17/A3 AD_B1_06
46+
6, // 18/A4 AD_B1_01
47+
5, // 19/A5 AD_B1_00
48+
15, // 20/A6 AD_B1_10
49+
0, // 21/A7 AD_B1_11
50+
13, // 22/A8 AD_B1_08
51+
14, // 23/A9 AD_B1_09
52+
1, // 24/A10 AD_B0_12
53+
2, // 25/A11 AD_B0_13
54+
128+3, // 26/A12 AD_B1_14 - only on ADC2, 3
55+
128+4, // 27/A13 AD_B1_15 - only on ADC2, 4
56+
#ifdef ARDUINO_TEENSY41
57+
255, // 28
58+
255, // 29
59+
255, // 30
60+
255, // 31
61+
255, // 32
62+
255, // 33
63+
255, // 34
64+
255, // 35
65+
255, // 36
66+
255, // 37
67+
128+1, // 38/A14 AD_B1_12 - only on ADC2, 1
68+
128+2, // 39/A15 AD_B1_13 - only on ADC2, 2
69+
9, // 40/A16 AD_B1_04
70+
10, // 41/A17 AD_B1_05
71+
#endif
72+
};
73+
74+
75+
#endif
76+
77+
#endif

‎src/current_sense/hardware_specific/teensy_mcu.cpp‎ renamed to ‎src/current_sense/hardware_specific/teensy/teensy_mcu.cpp‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -1,4 +1,4 @@
1-
#include "../hardware_api.h"
1+
#include "../../hardware_api.h"
22

33
#if defined(__arm__) && defined(CORE_TEENSY)
44

0 commit comments

Comments
 (0)