From 0b21e0b9f4f32031858126fb034c8fd6227bc2bb Mon Sep 17 00:00:00 2001 From: maxime Date: Fri, 21 May 2021 17:18:56 -0600 Subject: [PATCH 01/15] FEAT samd21 low side current sense, sync with pwm use of EVSYS to trigger ADC conversion at each TCC OVF removed DMA code as I couldn't figure out how to combine both Problem1: Currently I only get straight 1.71V +/- 0.01 no matter what happens with the motor /control loop. Something is off with the timing Problem2: all OVF signal triggers the ADC, which is configured in INPUTSCAN mode and iterate over the 3 channel at each event trigger. while events come from different channels, they all sink into the same USER channel and there is now way of correctly mapping e.g. TCC0 to ADC pinA --- .../hardware_specific/samd21_mcu.cpp | 265 ++++++++++-------- .../hardware_specific/samd21_mcu.h | 4 +- src/drivers/hardware_specific/samd21_mcu.cpp | 77 ++++- src/drivers/hardware_specific/samd51_mcu.cpp | 2 +- src/drivers/hardware_specific/samd_mcu.h | 2 +- 5 files changed, 232 insertions(+), 118 deletions(-) diff --git a/src/current_sense/hardware_specific/samd21_mcu.cpp b/src/current_sense/hardware_specific/samd21_mcu.cpp index 5c5de779..24989782 100644 --- a/src/current_sense/hardware_specific/samd21_mcu.cpp +++ b/src/current_sense/hardware_specific/samd21_mcu.cpp @@ -25,7 +25,7 @@ void _configureADCLowSide(const int pinA,const int pinB,const int pinC) } void _startADC3PinConversionLowSide() { - instance.startADCScan(); + // instance.startADCScan(); } /** * function reading an ADC value and returning the read voltage @@ -91,7 +91,9 @@ SAMDCurrentSenseADCDMA::SAMDCurrentSenseADCDMA() { } -void SAMDCurrentSenseADCDMA::init(int pinA, int pinB, int pinC, int pinAREF, float voltageAREF, uint32_t adcBits, uint32_t channelDMA) +void SAMDCurrentSenseADCDMA::init(int pinA, int pinB, int pinC +, int pinAREF, float voltageAREF +, uint32_t adcBits, uint32_t channelDMA) { this->pinA = pinA; this->pinB = pinB; @@ -102,29 +104,33 @@ void SAMDCurrentSenseADCDMA::init(int pinA, int pinB, int pinC, int pinAREF, flo this->maxCountsADC = 1 << adcBits; countsToVolts = ( voltageAREF / maxCountsADC ); + for(static int i = 0; i < 20; i++) + adcBuffer[i] = 0; initPins(); initADC(); - initDMA(); - startADCScan(); //so we have something to read next time we call readResults() + // initDMA(); + // startADCScan(); //so we have something to read next time we call readResults() + + } void SAMDCurrentSenseADCDMA::startADCScan(){ - adcToDMATransfer(adcBuffer + oneBeforeFirstAIN, BufferSize); - adcStartWithDMA(); + // adcToDMATransfer(adcBuffer + oneBeforeFirstAIN, BufferSize); + // adcStartWithDMA(); } bool SAMDCurrentSenseADCDMA::readResults(uint16_t & a, uint16_t & b, uint16_t & c){ - if(ADC->CTRLA.bit.ENABLE) - return false; + // if(ADC->CTRLA.bit.ENABLE) + // return false; uint32_t ainA = g_APinDescription[pinA].ulADCChannelNumber; uint32_t ainB = g_APinDescription[pinB].ulADCChannelNumber; - a = adcBuffer[ainA]; - b = adcBuffer[ainB]; + a = adcBuffer[1]; + b = adcBuffer[2]; if(_isset(pinC)) { uint32_t ainC = g_APinDescription[pinC].ulADCChannelNumber; - c = adcBuffer[ainC]; + c = adcBuffer[3]; } return true; } @@ -163,14 +169,30 @@ void SAMDCurrentSenseADCDMA::initADC(){ analogRead(pinB); // do some pin init pinPeripheral() analogRead(pinC); // do some pin init pinPeripheral() - ADC->CTRLA.bit.ENABLE = 0x00; // Disable ADC + ADC->CTRLA.bit.ENABLE = 0x00; // Disable ADC + + /* Turn on the digital interface clock */ + PM->APBCMASK.reg |= PM_APBCMASK_ADC; + + /* Turn on the analog interface clock and use GCLK Generator + */ + GCLK_CLKCTRL_Type adc_clkctrl{.reg = 0}; + adc_clkctrl.bit.WRTLOCK = 0; + adc_clkctrl.bit.CLKEN = 1; /* enable clock */ + adc_clkctrl.bit.ID = ADC_GCLK_ID; + adc_clkctrl.bit.GEN = 0; /* GCLK_GENERATOR_0 */ + GCLK->CLKCTRL.reg = adc_clkctrl.reg; + + /* reset the ADC to its initial settings */ + ADC->CTRLA.reg = ADC_CTRLA_SWRST; ADCsync(); - //ADC->REFCTRL.bit.REFSEL = ADC_REFCTRL_REFSEL_INTVCC0_Val; // 2.2297 V Supply VDDANA - ADC->INPUTCTRL.bit.GAIN = ADC_INPUTCTRL_GAIN_1X_Val; // Gain select as 1X - // ADC->INPUTCTRL.bit.GAIN = ADC_INPUTCTRL_GAIN_DIV2_Val; // default - ADC->REFCTRL.bit.REFSEL = ADC_REFCTRL_REFSEL_AREFA; - // ADC->REFCTRL.bit.REFSEL = ADC_REFCTRL_REFSEL_INTVCC0; - ADCsync(); // ref 31.6.16 + + /* Configure reference */ + ADC_REFCTRL_Type refctrl{.reg = 0}; + // refctrl.bit.REFCOMP = 1; /* enable reference compensation */ + refctrl.bit.REFSEL = this->pinAREF == 42 ? ADC_REFCTRL_REFSEL_AREFA_Val : ADC_REFCTRL_REFSEL_INTVCC1_Val; + + ADC->REFCTRL.reg = refctrl.reg; /* Bits 19:16 – INPUTSCAN[3:0]: Number of Input Channels Included in Scan @@ -185,24 +207,7 @@ void SAMDCurrentSenseADCDMA::initADC(){ Table 32-14. Positive Mux Input Selection MUXPOS[4:0] Group configuration Description 0x00 PIN0 ADC AIN0 pin - 0x01 PIN1 ADC AIN1 pin - 0x02 PIN2 ADC AIN2 pin - 0x03 PIN3 ADC AIN3 pin - 0x04 PIN4 ADC AIN4 pin - 0x05 PIN5 ADC AIN5 pin - 0x06 PIN6 ADC AIN6 pin - 0x07 PIN7 ADC AIN7 pin - 0x08 PIN8 ADC AIN8 pin - 0x09 PIN9 ADC AIN9 pin - 0x0A PIN10 ADC AIN10 pin - 0x0B PIN11 ADC AIN11 pin - 0x0C PIN12 ADC AIN12 pin - 0x0D PIN13 ADC AIN13 pin - 0x0E PIN14 ADC AIN14 pin - 0x0F PIN15 ADC AIN15 pin - 0x10 PIN16 ADC AIN16 pin - 0x11 PIN17 ADC AIN17 pin - 0x12 PIN18 ADC AIN18 pin + [...] 0x13 PIN19 ADC AIN19 pin 0x14-0x17 Reserved 0x18 TEMP Temperature reference @@ -212,93 +217,131 @@ void SAMDCurrentSenseADCDMA::initADC(){ 0x1C DAC DAC output 0x1D-0x1F Reserved */ - ADC->INPUTCTRL.bit.MUXPOS = oneBeforeFirstAIN; - ADCsync(); - ADC->INPUTCTRL.bit.INPUTSCAN = lastAIN; // so the adc will scan from oneBeforeFirstAIN to lastAIN (inclusive) + ADC_INPUTCTRL_Type inputctrl{.reg = 0}; + inputctrl.bit.GAIN = refctrl.bit.REFSEL == ADC_REFCTRL_REFSEL_AREFA_Val ? ADC_INPUTCTRL_GAIN_1X_Val : ADC_INPUTCTRL_GAIN_DIV2_Val; + inputctrl.bit.MUXPOS = firstAIN; + inputctrl.bit.MUXNEG = ADC_INPUTCTRL_MUXNEG_GND_Val; + inputctrl.bit.INPUTSCAN = 3; // so the adc will scan from oneBeforeFirstAIN to lastAIN (inclusive) + inputctrl.bit.INPUTOFFSET = 0; //input scan cursor + ADC->INPUTCTRL.reg = inputctrl.reg; ADCsync(); - ADC->INPUTCTRL.bit.INPUTOFFSET = 0; //input scan cursor - ADCsync(); - ADC->AVGCTRL.reg = 0x00 ; //no averaging - ADC->SAMPCTRL.reg = 0x05; ; //sample length in 1/2 CLK_ADC cycles, see GCLK_ADC and ADC_CTRLB_PRESCALER_DIV16 - // according to the specsheet: f_GCLK_ADC ADC input clock frequency 48 MHz, so same as fCPU + + /* Set up the average and samples */ + ADC_AVGCTRL_Type avgctrl{.reg = 0}; + avgctrl.bit.ADJRES = 0, + avgctrl.bit.SAMPLENUM = ADC_AVGCTRL_SAMPLENUM_1_Val, + ADC->AVGCTRL.reg = avgctrl.reg; + /* Configure sample length */ + ADC->SAMPCTRL.reg = ADC_SAMPCTRL_SAMPLEN(5); //sample length in 1/2 CLK_ADC cycles, see GCLK_ADC and ADC_CTRLB_PRESCALER_DIV16 + // according to the specsheet: fGCLK_ADC ADC input clock frequency 48 MHz, so same as fCPU ADCsync(); - ADC->CTRLB.reg = ADC_CTRLB_PRESCALER_DIV16 | ADC_CTRLB_FREERUN | ADC_CTRLB_RESSEL_12BIT; + + ADC_CTRLB_Type ctrlb{.reg = 0}; + /* ADC clock is 8MHz / 4 = 2MHz */ + ctrlb.bit.PRESCALER = ADC_CTRLB_PRESCALER_DIV16_Val; + ctrlb.bit.RESSEL = ADC_CTRLB_RESSEL_12BIT_Val; + ctrlb.bit.CORREN = 0; + ctrlb.bit.FREERUN = 0; + ctrlb.bit.LEFTADJ = 0; + ctrlb.bit.DIFFMODE = 0; + + ADC->CTRLB.reg = ctrlb.reg; ADCsync(); -} -volatile dmacdescriptor wrb[12] __attribute__ ((aligned (16))); + ADC_EVCTRL_Type adc_evctrl{.reg = 0}; + adc_evctrl.bit.WINMONEO = 0; + adc_evctrl.bit.RESRDYEO = 0; + adc_evctrl.bit.SYNCEI = 1; + adc_evctrl.bit.STARTEI = 1; + ADC->EVCTRL.reg = adc_evctrl.reg; -void SAMDCurrentSenseADCDMA::initDMA() { - // probably on by default - PM->AHBMASK.reg |= PM_AHBMASK_DMAC ; - PM->APBBMASK.reg |= PM_APBBMASK_DMAC ; - NVIC_EnableIRQ( DMAC_IRQn ) ; - DMAC->BASEADDR.reg = (uint32_t)descriptor_section; - DMAC->WRBADDR.reg = (uint32_t)wrb; - DMAC->CTRL.reg = DMAC_CTRL_DMAENABLE | DMAC_CTRL_LVLEN(0xf); -} + ADC->INTENSET.bit.RESRDY = 1; + NVIC_EnableIRQ( ADC_IRQn ); + ADC->CTRLA.bit.ENABLE = 0x01; - -void SAMDCurrentSenseADCDMA::adcToDMATransfer(void *rxdata, uint32_t hwords) { - - DMAC->CHID.reg = DMAC_CHID_ID(channelDMA); - DMAC->CHCTRLA.reg &= ~DMAC_CHCTRLA_ENABLE; - DMAC->CHCTRLA.reg = DMAC_CHCTRLA_SWRST; - DMAC->SWTRIGCTRL.reg &= (uint32_t)(~(1 << channelDMA)); - - DMAC->CHCTRLB.reg = DMAC_CHCTRLB_LVL(0) - | DMAC_CHCTRLB_TRIGSRC(ADC_DMAC_ID_RESRDY) - | DMAC_CHCTRLB_TRIGACT_BEAT; - DMAC->CHINTENSET.reg = DMAC_CHINTENSET_MASK ; // enable all 3 interrupts - descriptor.descaddr = 0; - descriptor.srcaddr = (uint32_t) &ADC->RESULT.reg; - descriptor.btcnt = hwords; - descriptor.dstaddr = (uint32_t)rxdata + hwords*2; // end address - descriptor.btctrl = DMAC_BTCTRL_BEATSIZE_HWORD | DMAC_BTCTRL_DSTINC | DMAC_BTCTRL_VALID; - memcpy(&descriptor_section[channelDMA],&descriptor, sizeof(dmacdescriptor)); - - // start channel - DMAC->CHID.reg = DMAC_CHID_ID(channelDMA); - DMAC->CHCTRLA.reg |= DMAC_CHCTRLA_ENABLE; } +// volatile dmacdescriptor wrb[12] __attribute__ ((aligned (16))); -int iii = 0; +// void SAMDCurrentSenseADCDMA::initDMA() { +// // probably on by default +// PM->AHBMASK.reg |= PM_AHBMASK_DMAC ; +// PM->APBBMASK.reg |= PM_APBBMASK_DMAC ; +// NVIC_EnableIRQ( DMAC_IRQn ) ; +// DMAC->BASEADDR.reg = (uint32_t)descriptor_section; +// DMAC->WRBADDR.reg = (uint32_t)wrb; +// DMAC->CTRL.reg = DMAC_CTRL_DMAENABLE | DMAC_CTRL_LVLEN(0xf); +// } -void adcStopWithDMA(void){ - ADCsync(); - ADC->CTRLA.bit.ENABLE = 0x00; - // ADCsync(); - // if(iii++ % 1000 == 0) - // { - // SIMPLEFOC_SAMD_DEBUG_SERIAL.print(a); - // SIMPLEFOC_SAMD_DEBUG_SERIAL.print(" :: "); - // SIMPLEFOC_SAMD_DEBUG_SERIAL.print(b); - // SIMPLEFOC_SAMD_DEBUG_SERIAL.print(" :: "); - // SIMPLEFOC_SAMD_DEBUG_SERIAL.print(c); - // SIMPLEFOC_SAMD_DEBUG_SERIAL.println("yo!"); - // } +// void SAMDCurrentSenseADCDMA::adcToDMATransfer(void *rxdata, uint32_t hwords) { -} +// DMAC->CHID.reg = DMAC_CHID_ID(channelDMA); +// DMAC->CHCTRLA.reg &= ~DMAC_CHCTRLA_ENABLE; +// DMAC->CHCTRLA.reg = DMAC_CHCTRLA_SWRST; +// DMAC->SWTRIGCTRL.reg &= (uint32_t)(~(1 << channelDMA)); + +// DMAC->CHCTRLB.reg = DMAC_CHCTRLB_LVL(0) +// | DMAC_CHCTRLB_TRIGSRC(ADC_DMAC_ID_RESRDY) +// | DMAC_CHCTRLB_TRIGACT_BEAT; +// DMAC->CHINTENSET.reg = DMAC_CHINTENSET_MASK ; // enable all 3 interrupts +// descriptor.descaddr = 0; +// descriptor.srcaddr = (uint32_t) &ADC->RESULT.reg; +// descriptor.btcnt = hwords; +// descriptor.dstaddr = (uint32_t)rxdata + hwords*2; // end address +// descriptor.btctrl = DMAC_BTCTRL_BEATSIZE_HWORD | DMAC_BTCTRL_DSTINC | DMAC_BTCTRL_VALID; +// memcpy(&descriptor_section[channelDMA],&descriptor, sizeof(dmacdescriptor)); + +// // start channel +// DMAC->CHID.reg = DMAC_CHID_ID(channelDMA); +// DMAC->CHCTRLA.reg |= DMAC_CHCTRLA_ENABLE; +// } + + +// int iii = 0; + +// void adcStopWithDMA(void){ +// ADCsync(); +// ADC->CTRLA.bit.ENABLE = 0x00; +// // ADCsync(); +// // if(iii++ % 1000 == 0) +// // { +// // SIMPLEFOC_SAMD_DEBUG_SERIAL.print(a); +// // SIMPLEFOC_SAMD_DEBUG_SERIAL.print(" :: "); +// // SIMPLEFOC_SAMD_DEBUG_SERIAL.print(b); +// // SIMPLEFOC_SAMD_DEBUG_SERIAL.print(" :: "); +// // SIMPLEFOC_SAMD_DEBUG_SERIAL.print(c); +// // SIMPLEFOC_SAMD_DEBUG_SERIAL.println("yo!"); +// // } + + +// } + +void ADC_Handler() +{ -void adcStartWithDMA(void){ - ADCsync(); - ADC->INPUTCTRL.bit.INPUTOFFSET = 0; - ADCsync(); - ADC->SWTRIG.bit.FLUSH = 1; - ADCsync(); - ADC->CTRLA.bit.ENABLE = 0x01; - ADCsync(); -} + instance.adcBuffer[ADC->INPUTCTRL.bit.INPUTOFFSET] = ADC->RESULT.reg; -void DMAC_Handler() { - uint8_t active_channel; - active_channel = DMAC->INTPEND.reg & DMAC_INTPEND_ID_Msk; // get channel number - DMAC->CHID.reg = DMAC_CHID_ID(active_channel); - adcStopWithDMA(); - DMAC->CHINTFLAG.reg = DMAC_CHINTENCLR_TCMPL; // clear - DMAC->CHINTFLAG.reg = DMAC_CHINTENCLR_TERR; - DMAC->CHINTFLAG.reg = DMAC_CHINTENCLR_SUSP; + // instance.adcBuffer[i++%4] = ADC->RESULT.reg; +} -} \ No newline at end of file +// void adcStartWithDMA(void){ +// ADCsync(); +// ADC->INPUTCTRL.bit.INPUTOFFSET = 0; +// ADCsync(); +// ADC->SWTRIG.bit.FLUSH = 1; +// ADCsync(); +// ADC->CTRLA.bit.ENABLE = 0x01; +// ADCsync(); +// } + +// void DMAC_Handler() { +// uint8_t active_channel; +// active_channel = DMAC->INTPEND.reg & DMAC_INTPEND_ID_Msk; // get channel number +// DMAC->CHID.reg = DMAC_CHID_ID(active_channel); +// // adcStopWithDMA(); no need to stop it anymore it's not freerunning +// DMAC->CHINTFLAG.reg = DMAC_CHINTENCLR_TCMPL; // clear +// DMAC->CHINTFLAG.reg = DMAC_CHINTENCLR_TERR; +// DMAC->CHINTFLAG.reg = DMAC_CHINTENCLR_SUSP; + +// } \ No newline at end of file diff --git a/src/current_sense/hardware_specific/samd21_mcu.h b/src/current_sense/hardware_specific/samd21_mcu.h index a01c72e3..b773f6e0 100644 --- a/src/current_sense/hardware_specific/samd21_mcu.h +++ b/src/current_sense/hardware_specific/samd21_mcu.h @@ -32,13 +32,13 @@ class SAMDCurrentSenseADCDMA void initPins(); void initADC(); - void initDMA(); + // void initDMA(); + uint32_t oneBeforeFirstAIN; // hack to discard first noisy readout uint32_t firstAIN; uint32_t lastAIN; uint32_t BufferSize = 0; - uint16_t adcBuffer[20]; diff --git a/src/drivers/hardware_specific/samd21_mcu.cpp b/src/drivers/hardware_specific/samd21_mcu.cpp index ab41b745..3fd745ca 100644 --- a/src/drivers/hardware_specific/samd21_mcu.cpp +++ b/src/drivers/hardware_specific/samd21_mcu.cpp @@ -162,6 +162,19 @@ void configureSAMDClock() { GCLK_GENCTRL_ID(4); // Select GCLK4 while (GCLK->STATUS.bit.SYNCBUSY); // Wait for synchronization + //EVSYS config + /* Turn on the digital interface clock */ + PM->APBCMASK.reg |= PM_APBCMASK_EVSYS; + + /* Turn on the peripheral interface clock and select GCLK */ + GCLK_CLKCTRL_Type clkctrl; + clkctrl.bit.WRTLOCK = 0; + clkctrl.bit.CLKEN = 1; + clkctrl.bit.ID = EVSYS_GCLK_ID_0; + clkctrl.bit.GEN = 0; /* GCLK_GENERATOR_0 */ + GCLK->CLKCTRL.reg = clkctrl.reg; + while (GCLK->STATUS.bit.SYNCBUSY); // Wait for synchronization + #ifdef SIMPLEFOC_SAMD_DEBUG SIMPLEFOC_SAMD_DEBUG_SERIAL.println("Configured clock..."); #endif @@ -176,7 +189,7 @@ void configureSAMDClock() { * pwm_frequency is fixed at 24kHz for now. We could go slower, but going * faster won't be possible without sacrificing resolution. */ -void configureTCC(tccConfiguration& tccConfig, long pwm_frequency, bool negate, float hw6pwm) { +void configureTCC(tccConfiguration& tccConfig, long pwm_frequency, bool negate, float hw6pwm, bool enableOVFEO) { // TODO for the moment we ignore the frequency... if (!tccConfigured[tccConfig.tcc.tccn]) { uint32_t GCLK_CLKCTRL_ID_ofthistcc = -1; @@ -194,8 +207,46 @@ void configureTCC(tccConfiguration& tccConfig, long pwm_frequency, bool negate, GCLK_CLKCTRL_ID_ofthistcc; // Feed GCLK4 to tcc while (GCLK->STATUS.bit.SYNCBUSY); // Wait for synchronization + + if(enableOVFEO) + { + #ifdef SIMPLEFOC_SAMD_DEBUG + SIMPLEFOC_SAMD_DEBUG_SERIAL.print("Configuring EVSYS for TCC channel "); + SIMPLEFOC_SAMD_DEBUG_SERIAL.println(tccConfig.tcc.tccn); + #endif + + + EVSYS_USER_Type user; + user.bit.CHANNEL = tccConfig.tcc.tccn + 1; /* use channel tccn p421: "Note that to select channel n, the value (n+1) must be written to the USER.CHANNEL bit group." */ + user.bit.USER = EVSYS_ID_USER_ADC_SYNC; /* ADC Start*/ + EVSYS->USER.reg = user.reg; + // configuration of the event system so that it triggers an "EVSYS_ID_USER_ADC_SYNC" at each timer overflow (underflow) + EVSYS_CHANNEL_Type channel; + switch (tccConfig.tcc.tccn){ + case 0: channel.bit.EVGEN = EVSYS_ID_GEN_TCC0_OVF; break; + case 1: channel.bit.EVGEN = EVSYS_ID_GEN_TCC1_OVF; break; + case 2: channel.bit.EVGEN = EVSYS_ID_GEN_TCC2_OVF; break; + // case 3: channel.bit.EVGEN = EVSYS_ID_GEN_TC3_OVF; break; // TODO: we need the TC equivalent of TCC_WAVEB_WAVEGENB_DSBOTTOM + // case 4: channel.bit.EVGEN = EVSYS_ID_GEN_TC4_OVF; break; // TODO: we need the TC equivalent of TCC_WAVEB_WAVEGENB_DSBOTTOM + // case 5: channel.bit.EVGEN = EVSYS_ID_GEN_TC5_OVF; break; // TODO: we need the TC equivalent of TCC_WAVEB_WAVEGENB_DSBOTTOM + default: + #ifdef SIMPLEFOC_SAMD_DEBUG + SIMPLEFOC_SAMD_DEBUG_SERIAL.print("Error in configureTCC() [EVSYS] Bad tccn number: "); + SIMPLEFOC_SAMD_DEBUG_SERIAL.println(tccConfig.tcc.tccn); + #endif + return; + } + + channel.bit.EDGSEL = EVSYS_CHANNEL_EDGSEL_NO_EVT_OUTPUT_Val; + channel.bit.PATH = EVSYS_CHANNEL_PATH_ASYNCHRONOUS_Val; + channel.bit.SWEVT = 0; /* no software trigger */ + channel.bit.CHANNEL = tccConfig.tcc.tccn; /* use channel tccn */ + EVSYS->CHANNEL.reg = channel.reg; + } + tccConfigured[tccConfig.tcc.tccn] = true; + if (tccConfig.tcc.tccn>=TCC_INST_NUM) { Tc* tc = (Tc*)GetTC(tccConfig.tcc.chaninfo); // disable @@ -229,7 +280,17 @@ void configureTCC(tccConfiguration& tccConfig, long pwm_frequency, bool negate, tcc->DRVCTRL.vec.INVEN = (tcc->DRVCTRL.vec.INVEN&invenMask)|invenVal; syncTCC(tcc); // wait for sync - tcc->WAVE.reg |= TCC_WAVE_POL(0xF)|TCC_WAVEB_WAVEGENB_DSBOTH; // Set wave form configuration + // Set wave form configuration + // We do phase-correct PWM (up-down counting) - so the TOP value represents exactly the middle of the PWM on-period for the high-side, + // and the BOTTOM value represents the middle of the PWM on-period for the low-side (which has inverted output). + // Personally, I would just attach to the high side, then the code can probably remain the same for 3-PWM and 6-PWM. + // So basically for low side sensing I think you always want to generate events when the counter reaches the bottom value (turns around). + // You do this by setting PWM mode “DSBOTTOM” + if(enableOVFEO) + tcc->WAVE.reg |= TCC_WAVE_POL(0xF)|TCC_WAVEB_WAVEGENB_DSBOTTOM; + else + tcc->WAVE.reg |= TCC_WAVE_POL(0xF)|TCC_WAVEB_WAVEGENB_DSBOTH; + while ( tcc->SYNCBUSY.bit.WAVE == 1 ); // wait for sync if (hw6pwm>0.0) { @@ -250,10 +311,15 @@ void configureTCC(tccConfiguration& tccConfig, long pwm_frequency, bool negate, while ( (tcc->SYNCBUSY.reg & chanbit) > 0 ); } + if(enableOVFEO) { + tcc->EVCTRL.bit.OVFEO = 1; + while ( tcc->SYNCBUSY.bit.ENABLE == 1 ); // wait for sync + } + // Enable TC tcc->CTRLA.reg |= TCC_CTRLA_ENABLE | TCC_CTRLA_PRESCALER_DIV1; //48Mhz/1=48Mhz/2(up/down)=24MHz/1024=24KHz while ( tcc->SYNCBUSY.bit.ENABLE == 1 ); // wait for sync - + #ifdef SIMPLEFOC_SAMD_DEBUG SIMPLEFOC_SAMD_DEBUG_SERIAL.print(" Initialized TCC "); SIMPLEFOC_SAMD_DEBUG_SERIAL.print(tccConfig.tcc.tccn); @@ -301,9 +367,14 @@ void configureTCC(tccConfiguration& tccConfig, long pwm_frequency, bool negate, } +void EVSYS_Init(int chaninfo) +{ +} + + void writeSAMDDutyCycle(int chaninfo, float dc) { uint8_t tccn = GetTCNumber(chaninfo); diff --git a/src/drivers/hardware_specific/samd51_mcu.cpp b/src/drivers/hardware_specific/samd51_mcu.cpp index 08201b97..4cfb9fc2 100644 --- a/src/drivers/hardware_specific/samd51_mcu.cpp +++ b/src/drivers/hardware_specific/samd51_mcu.cpp @@ -202,7 +202,7 @@ void configureSAMDClock() { * pwm_frequency is fixed at 24kHz for now. We could go slower, but going * faster won't be possible without sacrificing resolution. */ -void configureTCC(tccConfiguration& tccConfig, long pwm_frequency, bool negate, float hw6pwm) { +void configureTCC(tccConfiguration& tccConfig, long pwm_frequency, bool negate, float hw6pwm, bool enableOVFEO) { // TODO for the moment we ignore the frequency... if (!tccConfigured[tccConfig.tcc.tccn]) { uint32_t GCLK_CLKCTRL_ID_ofthistcc = GCLK_CLKCTRL_IDs[tccConfig.tcc.tccn]; diff --git a/src/drivers/hardware_specific/samd_mcu.h b/src/drivers/hardware_specific/samd_mcu.h index 7faf7442..928a3ad3 100644 --- a/src/drivers/hardware_specific/samd_mcu.h +++ b/src/drivers/hardware_specific/samd_mcu.h @@ -94,7 +94,7 @@ extern bool tccConfigured[TCC_INST_NUM+TC_INST_NUM]; struct wo_association& getWOAssociation(EPortType port, uint32_t pin); void writeSAMDDutyCycle(int chaninfo, float dc); void configureSAMDClock(); -void configureTCC(tccConfiguration& tccConfig, long pwm_frequency, bool negate=false, float hw6pwm=-1); +void configureTCC(tccConfiguration& tccConfig, long pwm_frequency, bool negate=false, float hw6pwm=-1, bool enableOVFEO = true); __inline__ void syncTCC(Tcc* TCCx) __attribute__((always_inline, unused)); EPioType getPeripheralOfPermutation(int permutation, int pin_position); From 9f03964491bf1896992f02b1341a6e9b7f8a5fe2 Mon Sep 17 00:00:00 2001 From: maxime Date: Fri, 21 May 2021 17:45:43 -0600 Subject: [PATCH 02/15] bugfix: forgot that my pins were not consecutive --- src/current_sense/hardware_specific/samd21_mcu.cpp | 10 +++++----- src/current_sense/hardware_specific/samd21_mcu.h | 2 +- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/src/current_sense/hardware_specific/samd21_mcu.cpp b/src/current_sense/hardware_specific/samd21_mcu.cpp index 24989782..df1b3e8b 100644 --- a/src/current_sense/hardware_specific/samd21_mcu.cpp +++ b/src/current_sense/hardware_specific/samd21_mcu.cpp @@ -125,12 +125,12 @@ bool SAMDCurrentSenseADCDMA::readResults(uint16_t & a, uint16_t & b, uint16_t & // return false; uint32_t ainA = g_APinDescription[pinA].ulADCChannelNumber; uint32_t ainB = g_APinDescription[pinB].ulADCChannelNumber; - a = adcBuffer[1]; - b = adcBuffer[2]; + a = adcBuffer[ainA]; + b = adcBuffer[ainB]; if(_isset(pinC)) { uint32_t ainC = g_APinDescription[pinC].ulADCChannelNumber; - c = adcBuffer[3]; + c = adcBuffer[ainC]; } return true; } @@ -221,7 +221,7 @@ void SAMDCurrentSenseADCDMA::initADC(){ inputctrl.bit.GAIN = refctrl.bit.REFSEL == ADC_REFCTRL_REFSEL_AREFA_Val ? ADC_INPUTCTRL_GAIN_1X_Val : ADC_INPUTCTRL_GAIN_DIV2_Val; inputctrl.bit.MUXPOS = firstAIN; inputctrl.bit.MUXNEG = ADC_INPUTCTRL_MUXNEG_GND_Val; - inputctrl.bit.INPUTSCAN = 3; // so the adc will scan from oneBeforeFirstAIN to lastAIN (inclusive) + inputctrl.bit.INPUTSCAN = lastAIN - inputctrl.bit.MUXPOS + 1; // so the adc will scan from oneBeforeFirstAIN to lastAIN (inclusive) inputctrl.bit.INPUTOFFSET = 0; //input scan cursor ADC->INPUTCTRL.reg = inputctrl.reg; ADCsync(); @@ -320,7 +320,7 @@ void SAMDCurrentSenseADCDMA::initADC(){ void ADC_Handler() { - instance.adcBuffer[ADC->INPUTCTRL.bit.INPUTOFFSET] = ADC->RESULT.reg; + instance.adcBuffer[ADC->INPUTCTRL.bit.MUXPOS + ADC->INPUTCTRL.bit.INPUTOFFSET - 1] = ADC->RESULT.reg; // instance.adcBuffer[i++%4] = ADC->RESULT.reg; } diff --git a/src/current_sense/hardware_specific/samd21_mcu.h b/src/current_sense/hardware_specific/samd21_mcu.h index b773f6e0..04242bdb 100644 --- a/src/current_sense/hardware_specific/samd21_mcu.h +++ b/src/current_sense/hardware_specific/samd21_mcu.h @@ -26,6 +26,7 @@ class SAMDCurrentSenseADCDMA void startADCScan(); bool readResults(uint16_t & a, uint16_t & b, uint16_t & c); float toVolts(uint16_t counts); + uint16_t adcBuffer[20]; private: void adcToDMATransfer(void *rxdata, uint32_t hwords); @@ -39,7 +40,6 @@ class SAMDCurrentSenseADCDMA uint32_t firstAIN; uint32_t lastAIN; uint32_t BufferSize = 0; - uint16_t adcBuffer[20]; uint32_t pinA; From f7e8a121ac9126194ed8180ba87d0c7f895b6749 Mon Sep 17 00:00:00 2001 From: maxime Date: Sat, 22 May 2021 14:56:35 -0600 Subject: [PATCH 03/15] bugfix: not reading during dead times anymore --- src/current_sense/hardware_specific/samd21_mcu.cpp | 13 ++++++++++++- src/drivers/hardware_specific/samd21_mcu.cpp | 2 +- 2 files changed, 13 insertions(+), 2 deletions(-) diff --git a/src/current_sense/hardware_specific/samd21_mcu.cpp b/src/current_sense/hardware_specific/samd21_mcu.cpp index df1b3e8b..d2eb6f6b 100644 --- a/src/current_sense/hardware_specific/samd21_mcu.cpp +++ b/src/current_sense/hardware_specific/samd21_mcu.cpp @@ -187,6 +187,17 @@ void SAMDCurrentSenseADCDMA::initADC(){ ADC->CTRLA.reg = ADC_CTRLA_SWRST; ADCsync(); + + uint32_t bias = (*((uint32_t *) ADC_FUSES_BIASCAL_ADDR) & ADC_FUSES_BIASCAL_Msk) >> ADC_FUSES_BIASCAL_Pos; + uint32_t linearity = (*((uint32_t *) ADC_FUSES_LINEARITY_0_ADDR) & ADC_FUSES_LINEARITY_0_Msk) >> ADC_FUSES_LINEARITY_0_Pos; + linearity |= ((*((uint32_t *) ADC_FUSES_LINEARITY_1_ADDR) & ADC_FUSES_LINEARITY_1_Msk) >> ADC_FUSES_LINEARITY_1_Pos) << 5; + + /* Wait for bus synchronization. */ + while (ADC->STATUS.bit.SYNCBUSY) {}; + + /* Write the calibration data. */ + ADC->CALIB.reg = ADC_CALIB_BIAS_CAL(bias) | ADC_CALIB_LINEARITY_CAL(linearity); + /* Configure reference */ ADC_REFCTRL_Type refctrl{.reg = 0}; // refctrl.bit.REFCOMP = 1; /* enable reference compensation */ @@ -316,7 +327,7 @@ void SAMDCurrentSenseADCDMA::initADC(){ // } - +void ADC_Handler() __attribute__((weak)); void ADC_Handler() { diff --git a/src/drivers/hardware_specific/samd21_mcu.cpp b/src/drivers/hardware_specific/samd21_mcu.cpp index 3fd745ca..db282c93 100644 --- a/src/drivers/hardware_specific/samd21_mcu.cpp +++ b/src/drivers/hardware_specific/samd21_mcu.cpp @@ -287,7 +287,7 @@ void configureTCC(tccConfiguration& tccConfig, long pwm_frequency, bool negate, // So basically for low side sensing I think you always want to generate events when the counter reaches the bottom value (turns around). // You do this by setting PWM mode “DSBOTTOM” if(enableOVFEO) - tcc->WAVE.reg |= TCC_WAVE_POL(0xF)|TCC_WAVEB_WAVEGENB_DSBOTTOM; + tcc->WAVE.reg |= TCC_WAVE_POL(0xF)|TCC_WAVEB_WAVEGENB_DSTOP; else tcc->WAVE.reg |= TCC_WAVE_POL(0xF)|TCC_WAVEB_WAVEGENB_DSBOTH; From d290fa9721655aaf1197dd48bb30bb6121d322f3 Mon Sep 17 00:00:00 2001 From: maxime Date: Fri, 28 May 2021 00:00:59 -0600 Subject: [PATCH 04/15] FEAT ADC + DMA + EVSYS with TCC trigs --- src/current_sense/LowsideCurrentSense.cpp | 2 - src/current_sense/hardware_api.h | 2 - .../hardware_specific/samd21_mcu.cpp | 307 ++++++++++++------ .../hardware_specific/samd21_mcu.h | 28 +- src/drivers/hardware_specific/samd21_mcu.cpp | 19 +- src/drivers/hardware_specific/samd_mcu.cpp | 8 +- src/drivers/hardware_specific/samd_mcu.h | 6 +- 7 files changed, 224 insertions(+), 148 deletions(-) diff --git a/src/current_sense/LowsideCurrentSense.cpp b/src/current_sense/LowsideCurrentSense.cpp index e0df940a..abebf03d 100644 --- a/src/current_sense/LowsideCurrentSense.cpp +++ b/src/current_sense/LowsideCurrentSense.cpp @@ -34,7 +34,6 @@ void LowsideCurrentSense::calibrateOffsets(){ offset_ic= 0; // read the adc voltage 1000 times ( arbitrary number ) for (int i = 0; i < 1000; i++) { - _startADC3PinConversionLowSide(); offset_ia += _readADCVoltageLowSide(pinA); offset_ib += _readADCVoltageLowSide(pinB); if(_isset(pinC)) offset_ic += _readADCVoltageLowSide(pinC); @@ -49,7 +48,6 @@ void LowsideCurrentSense::calibrateOffsets(){ // read all three phase currents (if possible 2 or 3) PhaseCurrent_s LowsideCurrentSense::getPhaseCurrents(){ PhaseCurrent_s current; - _startADC3PinConversionLowSide(); current.a = (_readADCVoltageLowSide(pinA) - offset_ia)*gain_a;// amps current.b = (_readADCVoltageLowSide(pinB) - offset_ib)*gain_b;// amps current.c = (!_isset(pinC)) ? 0 : (_readADCVoltageLowSide(pinC) - offset_ic)*gain_c; // amps diff --git a/src/current_sense/hardware_api.h b/src/current_sense/hardware_api.h index 7b648cde..ad831218 100644 --- a/src/current_sense/hardware_api.h +++ b/src/current_sense/hardware_api.h @@ -29,8 +29,6 @@ void _configureADCInline(const int pinA,const int pinB,const int pinC = NOT_SET) */ void _configureADCLowSide(const int pinA,const int pinB,const int pinC = NOT_SET); -void _startADC3PinConversionLowSide(); - /** * function reading an ADC value and returning the read voltage * diff --git a/src/current_sense/hardware_specific/samd21_mcu.cpp b/src/current_sense/hardware_specific/samd21_mcu.cpp index d2eb6f6b..efa0a21e 100644 --- a/src/current_sense/hardware_specific/samd21_mcu.cpp +++ b/src/current_sense/hardware_specific/samd21_mcu.cpp @@ -3,7 +3,6 @@ #include "../hardware_api.h" -static bool freeRunning = false; static int _pinA, _pinB, _pinC; static uint16_t a = 0xFFFF, b = 0xFFFF, c = 0xFFFF; // updated by adcStopWithDMA when configured in freerunning mode static SAMDCurrentSenseADCDMA instance; @@ -19,14 +18,9 @@ void _configureADCLowSide(const int pinA,const int pinB,const int pinC) _pinA = pinA; _pinB = pinB; _pinC = pinC; - freeRunning = true; - instance.init(pinA, pinB, pinC); + instance.init(pinA, pinB, pinC, EVSYS_ID_GEN_TCC0_OVF); } -void _startADC3PinConversionLowSide() -{ - // instance.startADCScan(); -} /** * function reading an ADC value and returning the read voltage * @@ -51,8 +45,7 @@ float _readADCVoltageLowSide(const int pinA) */ void _driverSyncLowSide() { - SIMPLEFOC_SAMD_DEBUG_SERIAL.println(F("TODO! _driverSyncLowSide() is not implemented")); - instance.startADCScan(); + SIMPLEFOC_SAMD_DEBUG_SERIAL.println(F("TODO! _driverSyncLowSide() is not untested, but if you use EVSYS_ID_GEN_TCC_OVF != -1 you are in sync")); //TODO: hook with PWM interrupts } @@ -67,8 +60,6 @@ void _driverSyncLowSide() // Credit: significant portions of this code were pulled from Paul Gould's git https://github.com/gouldpa/FOC-Arduino-Brushless -static void adcStopWithDMA(void); -static void adcStartWithDMA(void); /** * @brief ADC sync wait @@ -91,8 +82,8 @@ SAMDCurrentSenseADCDMA::SAMDCurrentSenseADCDMA() { } -void SAMDCurrentSenseADCDMA::init(int pinA, int pinB, int pinC -, int pinAREF, float voltageAREF +void SAMDCurrentSenseADCDMA::init(int pinA, int pinB, int pinC, +int EVSYS_ID_GEN_TCC_OVF, int pinAREF, float voltageAREF , uint32_t adcBits, uint32_t channelDMA) { this->pinA = pinA; @@ -102,24 +93,28 @@ void SAMDCurrentSenseADCDMA::init(int pinA, int pinB, int pinC this->channelDMA = channelDMA; this->voltageAREF = voltageAREF; this->maxCountsADC = 1 << adcBits; + this->EVSYS_ID_GEN_TCC_OVF = EVSYS_ID_GEN_TCC_OVF; countsToVolts = ( voltageAREF / maxCountsADC ); for(static int i = 0; i < 20; i++) - adcBuffer[i] = 0; + adcBuffer[i] = 42; initPins(); + if(this->EVSYS_ID_GEN_TCC_OVF != -1) + { + #ifdef SIMPLEFOC_SAMD_DEBUG + SIMPLEFOC_SAMD_DEBUG_SERIAL.print("Configuring EVSYS for ADC with EVSYS_ID_GEN_TCC_OVF"); + SIMPLEFOC_SAMD_DEBUG_SERIAL.println(this->EVSYS_ID_GEN_TCC_OVF); + #endif + + initEVSYS(); + initDMA(); + initDMAChannel(); + } initADC(); - // initDMA(); - // startADCScan(); //so we have something to read next time we call readResults() - } -void SAMDCurrentSenseADCDMA::startADCScan(){ - // adcToDMATransfer(adcBuffer + oneBeforeFirstAIN, BufferSize); - // adcStartWithDMA(); -} - bool SAMDCurrentSenseADCDMA::readResults(uint16_t & a, uint16_t & b, uint16_t & c){ // if(ADC->CTRLA.bit.ENABLE) // return false; @@ -142,7 +137,13 @@ float SAMDCurrentSenseADCDMA::toVolts(uint16_t counts) { void SAMDCurrentSenseADCDMA::initPins(){ - pinMode(pinAREF, INPUT); + if(pinAREF != -1) + { + SIMPLEFOC_SAMD_DEBUG_SERIAL.print("wtf"); + if(g_APinDescription[pinAREF].ulPort == EPortType::PORTA && g_APinDescription[pinAREF].ulPin == 3) + pinMode(pinAREF, INPUT); + } + pinMode(pinA, INPUT); pinMode(pinB, INPUT); @@ -159,7 +160,7 @@ void SAMDCurrentSenseADCDMA::initPins(){ } oneBeforeFirstAIN = firstAIN - 1; //hack to discard noisy first readout - BufferSize = lastAIN - oneBeforeFirstAIN + 1; + bufferSize = lastAIN - oneBeforeFirstAIN + 1; } @@ -188,21 +189,17 @@ void SAMDCurrentSenseADCDMA::initADC(){ ADCsync(); + /* Load the factory calibration data. */ + uint32_t bias = (*((uint32_t *) ADC_FUSES_BIASCAL_ADDR) & ADC_FUSES_BIASCAL_Msk) >> ADC_FUSES_BIASCAL_Pos; uint32_t linearity = (*((uint32_t *) ADC_FUSES_LINEARITY_0_ADDR) & ADC_FUSES_LINEARITY_0_Msk) >> ADC_FUSES_LINEARITY_0_Pos; linearity |= ((*((uint32_t *) ADC_FUSES_LINEARITY_1_ADDR) & ADC_FUSES_LINEARITY_1_Msk) >> ADC_FUSES_LINEARITY_1_Pos) << 5; - - /* Wait for bus synchronization. */ - while (ADC->STATUS.bit.SYNCBUSY) {}; - - /* Write the calibration data. */ ADC->CALIB.reg = ADC_CALIB_BIAS_CAL(bias) | ADC_CALIB_LINEARITY_CAL(linearity); /* Configure reference */ ADC_REFCTRL_Type refctrl{.reg = 0}; // refctrl.bit.REFCOMP = 1; /* enable reference compensation */ - refctrl.bit.REFSEL = this->pinAREF == 42 ? ADC_REFCTRL_REFSEL_AREFA_Val : ADC_REFCTRL_REFSEL_INTVCC1_Val; - + refctrl.bit.REFSEL = this->pinAREF != -1 ? ADC_REFCTRL_REFSEL_AREFA_Val : ADC_REFCTRL_REFSEL_INTVCC1_Val; ADC->REFCTRL.reg = refctrl.reg; /* @@ -266,93 +263,191 @@ void SAMDCurrentSenseADCDMA::initADC(){ adc_evctrl.bit.STARTEI = 1; ADC->EVCTRL.reg = adc_evctrl.reg; - ADC->INTENSET.bit.RESRDY = 1; - NVIC_EnableIRQ( ADC_IRQn ); + // if(this->EVSYS_ID_GEN_TCC_OVF == -1) + // { + //not evsys + dma driven + ADC->INTENSET.bit.RESRDY = 1; + NVIC_EnableIRQ( ADC_IRQn ); + // } ADC->CTRLA.bit.ENABLE = 0x01; } -// volatile dmacdescriptor wrb[12] __attribute__ ((aligned (16))); +DmacDescriptor descriptor_section[12] __attribute__ ((aligned (16))); +volatile DmacDescriptor write_back[12] __attribute__ ((aligned (16))); + +void SAMDCurrentSenseADCDMA::initDMA() { + // probably on by default + PM->AHBMASK.reg |= PM_AHBMASK_DMAC ; + PM->APBBMASK.reg |= PM_APBBMASK_DMAC ; + + DMAC->BASEADDR.reg = (uint32_t)descriptor_section; // Descriptor Base Memory Address + DMAC->WRBADDR.reg = (uint32_t)write_back; //Write-Back Memory Base Address + + + DMAC_PRICTRL0_Type prictrl0{.reg = 0}; + + prictrl0.bit.RRLVLEN0 = 0b1; //enable round-robin + prictrl0.bit.RRLVLEN1 = 0b1; //enable round-robin + prictrl0.bit.RRLVLEN2 = 0b1; //enable round-robin + prictrl0.bit.RRLVLEN3 = 0b1; //enable round-robin + + DMAC->PRICTRL0.reg = prictrl0.reg; + + DMAC_CTRL_Type ctrl{.reg = 0}; + ctrl.bit.DMAENABLE = 0b1; + ctrl.bit.LVLEN0 = 0b1; + ctrl.bit.LVLEN1 = 0b1; + ctrl.bit.LVLEN2 = 0b1; + ctrl.bit.LVLEN3 = 0b1; + ctrl.bit.CRCENABLE = 0b0; + + DMAC->CTRL.reg = ctrl.reg; + + NVIC_EnableIRQ( DMAC_IRQn ) ; +} + +DmacDescriptor descriptors[20] __attribute__ ((aligned (16))); + +void SAMDCurrentSenseADCDMA::initDMAChannel() { + + //select the channel + DMAC->CHID.bit.ID = channelDMA; -// void SAMDCurrentSenseADCDMA::initDMA() { -// // probably on by default -// PM->AHBMASK.reg |= PM_AHBMASK_DMAC ; -// PM->APBBMASK.reg |= PM_APBBMASK_DMAC ; -// NVIC_EnableIRQ( DMAC_IRQn ) ; -// DMAC->BASEADDR.reg = (uint32_t)descriptor_section; -// DMAC->WRBADDR.reg = (uint32_t)wrb; -// DMAC->CTRL.reg = DMAC_CTRL_DMAENABLE | DMAC_CTRL_LVLEN(0xf); -// } + // disable and reset the channel + DMAC->CHCTRLA.bit.ENABLE = 0b0; //must be done **before** SWRST + DMAC->CHCTRLA.bit.SWRST = 0b1; + DMAC->CHINTENSET.bit.TCMPL = DMAC_CHINTENSET_MASK ; // enable all 3 interrupts -// void SAMDCurrentSenseADCDMA::adcToDMATransfer(void *rxdata, uint32_t hwords) { + // configure the channel + DMAC_CHCTRLB_Type chctrlb{.reg = 0}; + chctrlb.bit.LVL = DMAC_CHCTRLB_LVL_LVL0_Val; + chctrlb.bit.EVIE = 0b0; //input (USER) event enabled? + chctrlb.bit.EVOE = 0b1; //output (GEN) event enabled? + chctrlb.bit.EVACT = DMAC_CHCTRLB_EVACT_NOACT_Val; //only used if EVIE is set + chctrlb.bit.TRIGSRC = ADC_DMAC_ID_RESRDY; + chctrlb.bit.TRIGACT = DMAC_CHCTRLB_TRIGACT_BEAT_Val; //block, beat or transaction + DMAC->CHCTRLB.reg = chctrlb.reg; + + for(uint32_t i = firstAIN; i < lastAIN + 1; i++) + { + bool isLast = (i == lastAIN); + + descriptors[i].DESCADDR.reg = isLast ? 0 : (uint32_t)&descriptors[i+1]; + descriptors[i].SRCADDR.reg = (uint32_t) &ADC->RESULT.reg; + descriptors[i].BTCNT.reg = 1; + + volatile DMAC_BTCTRL_Type & btctrl = descriptors[i].BTCTRL; + btctrl.bit.VALID = 0b1; /*!< bit: 0 Descriptor Valid */ + + //we want no events after the last adc read + btctrl.bit.EVOSEL = isLast ? DMAC_BTCTRL_EVOSEL_DISABLE_Val : DMAC_BTCTRL_EVOSEL_BLOCK_Val; /*!< bit: 1.. 2 Event Output Selection */ + + btctrl.bit.BLOCKACT = DMAC_BTCTRL_BLOCKACT_NOACT_Val; //isLast ? DMAC_BTCTRL_BLOCKACT_NOACT_Val : DMAC_BTCTRL_BLOCKACT_INT_Val; /*!< bit: 3.. 4 Block Action */ + btctrl.bit.BEATSIZE = DMAC_BTCTRL_BEATSIZE_HWORD_Val; /*!< bit: 8.. 9 Beat Size (byte, half-words, words) */ + btctrl.bit.SRCINC = 0b0; /*!< bit: 10 Source Address Increment Enable */ + btctrl.bit.DSTINC = 0b1; /*!< bit: 11 Destination Address Increment Enable */ + btctrl.bit.STEPSEL = DMAC_BTCTRL_STEPSEL_DST_Val; /*!< bit: 12 Step Selection */ + btctrl.bit.STEPSIZE = DMAC_BTCTRL_STEPSIZE_X1_Val; /*!< bit: 13..15 Address Increment Step Size */ + /* + p.283 When destination address incrementation is configured (BTCTRL.DSTINC is one), SRCADDR must be set to the + destination address of the last beat transfer in the block transfer. The destination address should be calculated as + follows: + DSTADDR = DSTADDRSTART + BTCNT ⋅ ( BEATSIZE + 1 ) ⋅ 2 STEPSIZE , where BTCTRL.STEPSEL is zero + DSTADDR = DSTADDRSTART + BTCNT ⋅ ( BEATSIZE + 1 ) , where BTCTRL.STEPSEL is one + - DSTADDRSTART is the destination address of the first beat transfer in the block transfer + - BTCNT is the initial number of beats remaining in the block transfer + - BEATSIZE is the configured number of bytes in a beat + - STEPSIZE is the configured number of beats for each incrementation + */ + uint32_t factor = btctrl.bit.STEPSEL == 0 ? (1 << btctrl.bit.STEPSIZE) /*2^STEPSIZE*/: 1; + + descriptors[i].DSTADDR.reg = (uint32_t)(((uint8_t*)&adcBuffer[i]) + descriptors[i].BTCNT.reg * (btctrl.bit.BEATSIZE + 1) * factor); // end address -// DMAC->CHID.reg = DMAC_CHID_ID(channelDMA); -// DMAC->CHCTRLA.reg &= ~DMAC_CHCTRLA_ENABLE; -// DMAC->CHCTRLA.reg = DMAC_CHCTRLA_SWRST; -// DMAC->SWTRIGCTRL.reg &= (uint32_t)(~(1 << channelDMA)); + } + + memcpy(&descriptor_section[channelDMA], &descriptors[firstAIN], sizeof(DmacDescriptor)); -// DMAC->CHCTRLB.reg = DMAC_CHCTRLB_LVL(0) -// | DMAC_CHCTRLB_TRIGSRC(ADC_DMAC_ID_RESRDY) -// | DMAC_CHCTRLB_TRIGACT_BEAT; -// DMAC->CHINTENSET.reg = DMAC_CHINTENSET_MASK ; // enable all 3 interrupts -// descriptor.descaddr = 0; -// descriptor.srcaddr = (uint32_t) &ADC->RESULT.reg; -// descriptor.btcnt = hwords; -// descriptor.dstaddr = (uint32_t)rxdata + hwords*2; // end address -// descriptor.btctrl = DMAC_BTCTRL_BEATSIZE_HWORD | DMAC_BTCTRL_DSTINC | DMAC_BTCTRL_VALID; -// memcpy(&descriptor_section[channelDMA],&descriptor, sizeof(dmacdescriptor)); - -// // start channel -// DMAC->CHID.reg = DMAC_CHID_ID(channelDMA); -// DMAC->CHCTRLA.reg |= DMAC_CHCTRLA_ENABLE; -// } - - -// int iii = 0; - -// void adcStopWithDMA(void){ -// ADCsync(); -// ADC->CTRLA.bit.ENABLE = 0x00; -// // ADCsync(); -// // if(iii++ % 1000 == 0) -// // { -// // SIMPLEFOC_SAMD_DEBUG_SERIAL.print(a); -// // SIMPLEFOC_SAMD_DEBUG_SERIAL.print(" :: "); -// // SIMPLEFOC_SAMD_DEBUG_SERIAL.print(b); -// // SIMPLEFOC_SAMD_DEBUG_SERIAL.print(" :: "); -// // SIMPLEFOC_SAMD_DEBUG_SERIAL.print(c); -// // SIMPLEFOC_SAMD_DEBUG_SERIAL.println("yo!"); -// // } - - -// } + // start channel + DMAC->CHCTRLA.bit.ENABLE = 0b1; +} + +void SAMDCurrentSenseADCDMA::initEVSYS() +{ + /* Turn on the digital interface clock */ + PM->APBCMASK.reg |= PM_APBCMASK_EVSYS; + + /* Turn on the peripheral interface clock and select GCLK */ + GCLK_CLKCTRL_Type clkctrl0; + clkctrl0.bit.WRTLOCK = 0; + clkctrl0.bit.CLKEN = 1; + clkctrl0.bit.ID = EVSYS_GCLK_ID_0; //enable clock for channel 0 + clkctrl0.bit.GEN = GCLK_CLKCTRL_GEN_GCLK0_Val; /* GCLK_GENERATOR_0 */ + GCLK->CLKCTRL.reg = clkctrl0.reg; + + GCLK_CLKCTRL_Type clkctrl1; + clkctrl1.bit.WRTLOCK = 0; + clkctrl1.bit.CLKEN = 1; + clkctrl1.bit.ID = EVSYS_GCLK_ID_1; //enable clock for channel 1 + clkctrl1.bit.GEN = GCLK_CLKCTRL_GEN_GCLK0_Val; /* GCLK_GENERATOR_0 */ + GCLK->CLKCTRL.reg = clkctrl1.reg; + + // event user (destination) + EVSYS_USER_Type user_0; + user_0.bit.CHANNEL = 0 + 1; /* use channel 0 p421: "Note that to select channel n, the value (n+1) must be written to the USER.CHANNEL bit group." */ + user_0.bit.USER = EVSYS_ID_USER_ADC_SYNC; /* ADC Sync*/ + EVSYS->USER.reg = user_0.reg; + + // event user (destination) + EVSYS_USER_Type user_1; + user_1.bit.CHANNEL = 1 + 1; /* p421: "Note that to select channel n, the value (n+1) must be written to the USER.CHANNEL bit group." */ + // user_1.bit.USER = EVSYS_ID_USER_DMAC_CH_0; /* ADC Start*/ + user_1.bit.USER = EVSYS_ID_USER_ADC_START; /* ADC Start*/ + EVSYS->USER.reg = user_1.reg; + + // event generator (source) + EVSYS_CHANNEL_Type channel_0; + channel_0.bit.EDGSEL = EVSYS_CHANNEL_EDGSEL_NO_EVT_OUTPUT_Val; + channel_0.bit.PATH = EVSYS_CHANNEL_PATH_ASYNCHRONOUS_Val; + channel_0.bit.EVGEN = this->EVSYS_ID_GEN_TCC_OVF; /* TCCO Timer OVF */ + channel_0.bit.SWEVT = 0b0; /* no software trigger */ + channel_0.bit.CHANNEL = user_0.bit.CHANNEL - 1; /* use channel 0 */ + EVSYS->CHANNEL.reg = channel_0.reg; + + // event generator (source) + EVSYS_CHANNEL_Type channel_1{.reg = 0}; + channel_1.bit.EDGSEL = EVSYS_CHANNEL_EDGSEL_NO_EVT_OUTPUT_Val; + channel_1.bit.PATH = EVSYS_CHANNEL_PATH_ASYNCHRONOUS_Val; + channel_1.bit.EVGEN = EVSYS_ID_GEN_DMAC_CH_0; + channel_1.bit.SWEVT = 0b0; + channel_1.bit.CHANNEL = user_1.bit.CHANNEL - 1; + EVSYS->CHANNEL.reg = channel_1.reg; + + + // EVSYS does not emit interrups on asynchronous path + // EVSYS->INTENSET.reg = EVSYS_INTENSET_MASK; + // NVIC_EnableIRQ( EVSYS_IRQn ) ; +} + void ADC_Handler() __attribute__((weak)); void ADC_Handler() { + instance.adc_i++; + // only used if event system is not used + if(instance.EVSYS_ID_GEN_TCC_OVF == -1) + instance.adcBuffer[ADC->INPUTCTRL.bit.MUXPOS + ADC->INPUTCTRL.bit.INPUTOFFSET - 1] = ADC->RESULT.reg; + ADC->INTFLAG.bit.RESRDY = 0b1; + ADC->INTFLAG.bit.SYNCRDY = 0b1; +} - instance.adcBuffer[ADC->INPUTCTRL.bit.MUXPOS + ADC->INPUTCTRL.bit.INPUTOFFSET - 1] = ADC->RESULT.reg; +void DMAC_Handler() __attribute__((weak)); +void DMAC_Handler() { - // instance.adcBuffer[i++%4] = ADC->RESULT.reg; -} + instance.dma_i++; + DMAC->CHID.bit.ID = DMAC->INTPEND.bit.ID; + DMAC->CHINTFLAG.bit.TCMPL = 0b1; // clear + DMAC->CHCTRLA.bit.ENABLE = 0b1; + ADC->INPUTCTRL.bit.INPUTOFFSET = 0; -// void adcStartWithDMA(void){ -// ADCsync(); -// ADC->INPUTCTRL.bit.INPUTOFFSET = 0; -// ADCsync(); -// ADC->SWTRIG.bit.FLUSH = 1; -// ADCsync(); -// ADC->CTRLA.bit.ENABLE = 0x01; -// ADCsync(); -// } - -// void DMAC_Handler() { -// uint8_t active_channel; -// active_channel = DMAC->INTPEND.reg & DMAC_INTPEND_ID_Msk; // get channel number -// DMAC->CHID.reg = DMAC_CHID_ID(active_channel); -// // adcStopWithDMA(); no need to stop it anymore it's not freerunning -// DMAC->CHINTFLAG.reg = DMAC_CHINTENCLR_TCMPL; // clear -// DMAC->CHINTFLAG.reg = DMAC_CHINTENCLR_TERR; -// DMAC->CHINTFLAG.reg = DMAC_CHINTENCLR_SUSP; - -// } \ No newline at end of file +} \ No newline at end of file diff --git a/src/current_sense/hardware_specific/samd21_mcu.h b/src/current_sense/hardware_specific/samd21_mcu.h index 04242bdb..a124aefc 100644 --- a/src/current_sense/hardware_specific/samd21_mcu.h +++ b/src/current_sense/hardware_specific/samd21_mcu.h @@ -1,20 +1,14 @@ #ifndef CURRENT_SENSE_SAMD21_H #define CURRENT_SENSE_SAMD21_H + +#include + // #define SIMPLEFOC_SAMD_DEBUG #if !defined(SIMPLEFOC_SAMD_DEBUG_SERIAL) #define SIMPLEFOC_SAMD_DEBUG_SERIAL Serial #endif -#include - typedef struct { - uint16_t btctrl; - uint16_t btcnt; - uint32_t srcaddr; - uint32_t dstaddr; - uint32_t descaddr; - } dmacdescriptor ; - class SAMDCurrentSenseADCDMA { @@ -22,24 +16,27 @@ class SAMDCurrentSenseADCDMA public: static SAMDCurrentSenseADCDMA * getHardwareAPIInstance(); SAMDCurrentSenseADCDMA(); - void init(int pinA, int pinB, int pinC, int pinAREF = 42, float voltageAREF = 3.3, uint32_t adcBits = 12, uint32_t channelDMA = 3); - void startADCScan(); + void init(int pinA, int pinB, int pinC, int EVSYS_ID_GEN_TCC_OVF = -1, int pinAREF = -1, float voltageAREF = 3.3, uint32_t adcBits = 12, uint32_t channelDMA = 3); bool readResults(uint16_t & a, uint16_t & b, uint16_t & c); float toVolts(uint16_t counts); uint16_t adcBuffer[20]; + int adc_i = 0; + int dma_i = 0; + int EVSYS_ID_GEN_TCC_OVF; private: - void adcToDMATransfer(void *rxdata, uint32_t hwords); void initPins(); void initADC(); - // void initDMA(); + void initDMA(); + void initDMAChannel(); + void initEVSYS(); uint32_t oneBeforeFirstAIN; // hack to discard first noisy readout uint32_t firstAIN; uint32_t lastAIN; - uint32_t BufferSize = 0; + uint32_t bufferSize = 0; uint32_t pinA; @@ -52,9 +49,6 @@ class SAMDCurrentSenseADCDMA float voltageAREF; float maxCountsADC; float countsToVolts; - - dmacdescriptor descriptor_section[12] __attribute__ ((aligned (16))); - dmacdescriptor descriptor __attribute__ ((aligned (16))); }; diff --git a/src/drivers/hardware_specific/samd21_mcu.cpp b/src/drivers/hardware_specific/samd21_mcu.cpp index db282c93..6586d1a5 100644 --- a/src/drivers/hardware_specific/samd21_mcu.cpp +++ b/src/drivers/hardware_specific/samd21_mcu.cpp @@ -162,19 +162,6 @@ void configureSAMDClock() { GCLK_GENCTRL_ID(4); // Select GCLK4 while (GCLK->STATUS.bit.SYNCBUSY); // Wait for synchronization - //EVSYS config - /* Turn on the digital interface clock */ - PM->APBCMASK.reg |= PM_APBCMASK_EVSYS; - - /* Turn on the peripheral interface clock and select GCLK */ - GCLK_CLKCTRL_Type clkctrl; - clkctrl.bit.WRTLOCK = 0; - clkctrl.bit.CLKEN = 1; - clkctrl.bit.ID = EVSYS_GCLK_ID_0; - clkctrl.bit.GEN = 0; /* GCLK_GENERATOR_0 */ - GCLK->CLKCTRL.reg = clkctrl.reg; - while (GCLK->STATUS.bit.SYNCBUSY); // Wait for synchronization - #ifdef SIMPLEFOC_SAMD_DEBUG SIMPLEFOC_SAMD_DEBUG_SERIAL.println("Configured clock..."); #endif @@ -217,7 +204,7 @@ void configureTCC(tccConfiguration& tccConfig, long pwm_frequency, bool negate, EVSYS_USER_Type user; - user.bit.CHANNEL = tccConfig.tcc.tccn + 1; /* use channel tccn p421: "Note that to select channel n, the value (n+1) must be written to the USER.CHANNEL bit group." */ + user.bit.CHANNEL = 0 + 1; /* use channel tccn p421: "Note that to select channel n, the value (n+1) must be written to the USER.CHANNEL bit group." */ user.bit.USER = EVSYS_ID_USER_ADC_SYNC; /* ADC Start*/ EVSYS->USER.reg = user.reg; // configuration of the event system so that it triggers an "EVSYS_ID_USER_ADC_SYNC" at each timer overflow (underflow) @@ -238,9 +225,9 @@ void configureTCC(tccConfiguration& tccConfig, long pwm_frequency, bool negate, } channel.bit.EDGSEL = EVSYS_CHANNEL_EDGSEL_NO_EVT_OUTPUT_Val; - channel.bit.PATH = EVSYS_CHANNEL_PATH_ASYNCHRONOUS_Val; + channel.bit.PATH = EVSYS_CHANNEL_PATH_ASYNCHRONOUS_Val; //EVSYS_ID_USER_ADC_SYNC is asynchronous only channel.bit.SWEVT = 0; /* no software trigger */ - channel.bit.CHANNEL = tccConfig.tcc.tccn; /* use channel tccn */ + channel.bit.CHANNEL = user.bit.CHANNEL - 1; /* use channel tccn */ EVSYS->CHANNEL.reg = channel.reg; } diff --git a/src/drivers/hardware_specific/samd_mcu.cpp b/src/drivers/hardware_specific/samd_mcu.cpp index 1b035dfd..5345aa70 100644 --- a/src/drivers/hardware_specific/samd_mcu.cpp +++ b/src/drivers/hardware_specific/samd_mcu.cpp @@ -316,7 +316,7 @@ void _configure2PWM(long pwm_frequency, const int pinA, const int pinB) { configureSAMDClock(); // configure the TCC (waveform, top-value, pre-scaler = frequency) - configureTCC(tccConfs[0], pwm_frequency); + configureTCC(tccConfs[0], pwm_frequency, false, -1, true); configureTCC(tccConfs[1], pwm_frequency); #ifdef SIMPLEFOC_SAMD_DEBUG SIMPLEFOC_SAMD_DEBUG_SERIAL.println("Configured TCCs..."); @@ -409,7 +409,7 @@ void _configure3PWM(long pwm_frequency, const int pinA, const int pinB, const in configureSAMDClock(); // configure the TCC (waveform, top-value, pre-scaler = frequency) - configureTCC(tccConfs[0], pwm_frequency); + configureTCC(tccConfs[0], pwm_frequency, false, -1, true); configureTCC(tccConfs[1], pwm_frequency); configureTCC(tccConfs[2], pwm_frequency); #ifdef SIMPLEFOC_SAMD_DEBUG @@ -480,7 +480,7 @@ void _configure4PWM(long pwm_frequency, const int pin1A, const int pin1B, const configureSAMDClock(); // configure the TCC (waveform, top-value, pre-scaler = frequency) - configureTCC(tccConfs[0], pwm_frequency); + configureTCC(tccConfs[0], pwm_frequency, false, -1, true); configureTCC(tccConfs[1], pwm_frequency); configureTCC(tccConfs[2], pwm_frequency); configureTCC(tccConfs[3], pwm_frequency); @@ -580,7 +580,7 @@ int _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const configureSAMDClock(); // configure the TCC(s) - configureTCC(pinAh, pwm_frequency, false, (pinAh.tcc.chaninfo==pinAl.tcc.chaninfo)?dead_zone:-1); + configureTCC(pinAh, pwm_frequency, false, (pinAh.tcc.chaninfo==pinAl.tcc.chaninfo)?dead_zone:-1, true); if ((pinAh.tcc.chaninfo!=pinAl.tcc.chaninfo)) configureTCC(pinAl, pwm_frequency, true, -1.0); configureTCC(pinBh, pwm_frequency, false, (pinBh.tcc.chaninfo==pinBl.tcc.chaninfo)?dead_zone:-1); diff --git a/src/drivers/hardware_specific/samd_mcu.h b/src/drivers/hardware_specific/samd_mcu.h index 928a3ad3..c73e9f04 100644 --- a/src/drivers/hardware_specific/samd_mcu.h +++ b/src/drivers/hardware_specific/samd_mcu.h @@ -94,7 +94,11 @@ extern bool tccConfigured[TCC_INST_NUM+TC_INST_NUM]; struct wo_association& getWOAssociation(EPortType port, uint32_t pin); void writeSAMDDutyCycle(int chaninfo, float dc); void configureSAMDClock(); -void configureTCC(tccConfiguration& tccConfig, long pwm_frequency, bool negate=false, float hw6pwm=-1, bool enableOVFEO = true); + +/* + @param enableOVFEO : This will trigger ADC + DMA input scan on each counter TOP, I recommand only enabling OVFO for one TCC, and assume all TCC reach their TOP at the same time +*/ +void configureTCC(tccConfiguration& tccConfig, long pwm_frequency, bool negate=false, float hw6pwm=-1, bool enableOVFEO = false); __inline__ void syncTCC(Tcc* TCCx) __attribute__((always_inline, unused)); EPioType getPeripheralOfPermutation(int permutation, int pin_position); From 8a29fcd643d148d7e57c748ff9d1c19a4d0577a9 Mon Sep 17 00:00:00 2001 From: maxime Date: Fri, 28 May 2021 08:31:50 -0600 Subject: [PATCH 05/15] FEAT fix when evsys-dma is not used --- src/current_sense/hardware_specific/samd21_mcu.cpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/src/current_sense/hardware_specific/samd21_mcu.cpp b/src/current_sense/hardware_specific/samd21_mcu.cpp index efa0a21e..a6710b09 100644 --- a/src/current_sense/hardware_specific/samd21_mcu.cpp +++ b/src/current_sense/hardware_specific/samd21_mcu.cpp @@ -436,7 +436,10 @@ void ADC_Handler() instance.adc_i++; // only used if event system is not used if(instance.EVSYS_ID_GEN_TCC_OVF == -1) - instance.adcBuffer[ADC->INPUTCTRL.bit.MUXPOS + ADC->INPUTCTRL.bit.INPUTOFFSET - 1] = ADC->RESULT.reg; + { + uint16_t offset = ADC->INPUTCTRL.bit.INPUTOFFSET; + instance.adcBuffer[ADC->INPUTCTRL.bit.MUXPOS + (offset > 0 ? offset - 1 : ADC->INPUTCTRL.bit.INPUTSCAN)] = ADC->RESULT.reg; + } ADC->INTFLAG.bit.RESRDY = 0b1; ADC->INTFLAG.bit.SYNCRDY = 0b1; } From ae43b15aa15c3d5ab55ad4bad7a58bd605c53478 Mon Sep 17 00:00:00 2001 From: maxime Date: Fri, 28 May 2021 09:41:38 -0600 Subject: [PATCH 06/15] FEAT change _readADCVoltagesLowSide to read all 3 voltages in one shot --- src/current_sense/LowsideCurrentSense.cpp | 22 ++++---- src/current_sense/hardware_api.h | 2 +- .../hardware_specific/esp32_mcu.cpp | 15 ++---- .../hardware_specific/samd21_mcu.cpp | 50 ++++++++++++------- 4 files changed, 51 insertions(+), 38 deletions(-) diff --git a/src/current_sense/LowsideCurrentSense.cpp b/src/current_sense/LowsideCurrentSense.cpp index abebf03d..d7cc7168 100644 --- a/src/current_sense/LowsideCurrentSense.cpp +++ b/src/current_sense/LowsideCurrentSense.cpp @@ -32,25 +32,29 @@ void LowsideCurrentSense::calibrateOffsets(){ offset_ia =0; offset_ib= 0; offset_ic= 0; + float a, b, c; // read the adc voltage 1000 times ( arbitrary number ) for (int i = 0; i < 1000; i++) { - offset_ia += _readADCVoltageLowSide(pinA); - offset_ib += _readADCVoltageLowSide(pinB); - if(_isset(pinC)) offset_ic += _readADCVoltageLowSide(pinC); + _readADCVoltagesLowSide(a, b, c); + offset_ia += a; + offset_ib += b; + if(_isset(pinC)) offset_ic += c; _delay(1); } // calculate the mean offsets - offset_ia = offset_ia / 1000.0; - offset_ib = offset_ib / 1000.0; - if(_isset(pinC)) offset_ic = offset_ic / 1000.0; + offset_ia = offset_ia / 1000.0f; + offset_ib = offset_ib / 1000.0f; + if(_isset(pinC)) offset_ic = offset_ic / 1000.0f; } // read all three phase currents (if possible 2 or 3) PhaseCurrent_s LowsideCurrentSense::getPhaseCurrents(){ PhaseCurrent_s current; - current.a = (_readADCVoltageLowSide(pinA) - offset_ia)*gain_a;// amps - current.b = (_readADCVoltageLowSide(pinB) - offset_ib)*gain_b;// amps - current.c = (!_isset(pinC)) ? 0 : (_readADCVoltageLowSide(pinC) - offset_ic)*gain_c; // amps + + _readADCVoltagesLowSide(current.a, current.b, current.c); + current.a = (current.a - offset_ia)*gain_a;// amps + current.b = (current.b - offset_ib)*gain_b;// amps + current.c = (!_isset(pinC)) ? 0 : (current.c - offset_ic)*gain_c; // amps return current; } // Function synchronizing current sense with motor driver. diff --git a/src/current_sense/hardware_api.h b/src/current_sense/hardware_api.h index ad831218..b08325f4 100644 --- a/src/current_sense/hardware_api.h +++ b/src/current_sense/hardware_api.h @@ -34,7 +34,7 @@ void _configureADCLowSide(const int pinA,const int pinB,const int pinC = NOT_SET * * @param pinA - the arduino pin to be read (it has to be ADC pin) */ -float _readADCVoltageLowSide(const int pinA); +void _readADCVoltagesLowSide(float & a, float & b, float & c); /** * function syncing the Driver with the ADC for the LowSide Sensing diff --git a/src/current_sense/hardware_specific/esp32_mcu.cpp b/src/current_sense/hardware_specific/esp32_mcu.cpp index 3f7e83b3..b84204f1 100644 --- a/src/current_sense/hardware_specific/esp32_mcu.cpp +++ b/src/current_sense/hardware_specific/esp32_mcu.cpp @@ -20,19 +20,14 @@ byte currentState = 1; #define _ADC_CONV ( (_ADC_VOLTAGE) / (_ADC_RESOLUTION) ) // function reading an ADC value and returning the read voltage -float _readADCVoltageLowSide(const int pin){ - uint32_t raw_adc; +void _readADCVoltagesLowSide(float & a, float & b, float & c){ - if (pin == _pinA) raw_adc = a1; - else if (pin == _pinB) raw_adc = a2; - else if (pin == _pinC) raw_adc = a3; - - return raw_adc * _ADC_CONV; + a = _ADC_CONV * a1; + b = _ADC_CONV * a2; + if(_isset(_pinC)) + c = _ADC_CONV * a3; } -void _startADC3PinConversionLowSide(){ - -} // function reading an ADC value and returning the read voltage void _configureADCLowSide(const int pinA,const int pinB,const int pinC){ diff --git a/src/current_sense/hardware_specific/samd21_mcu.cpp b/src/current_sense/hardware_specific/samd21_mcu.cpp index a6710b09..0cada041 100644 --- a/src/current_sense/hardware_specific/samd21_mcu.cpp +++ b/src/current_sense/hardware_specific/samd21_mcu.cpp @@ -4,7 +4,7 @@ static int _pinA, _pinB, _pinC; -static uint16_t a = 0xFFFF, b = 0xFFFF, c = 0xFFFF; // updated by adcStopWithDMA when configured in freerunning mode +static uint16_t a_raw = 0xFFFF, b_raw = 0xFFFF, c_raw = 0xFFFF; // updated by adcStopWithDMA when configured in freerunning mode static SAMDCurrentSenseADCDMA instance; /** * function reading an ADC value and returning the read voltage @@ -26,18 +26,21 @@ void _configureADCLowSide(const int pinA,const int pinB,const int pinC) * * @param pinA - the arduino pin to be read (it has to be ADC pin) */ -float _readADCVoltageLowSide(const int pinA) +void _readADCVoltagesLowSide(float & a, float & b, float & c) { - instance.readResults(a, b, c); + static int last_read = -1; + + while(last_read == instance.dma_i); + + last_read = instance.dma_i; + + instance.readResults(a_raw, b_raw, c_raw); - if(pinA == _pinA) - return instance.toVolts(a); - if(pinA == _pinB) - return instance.toVolts(b); - if(pinA == _pinC) - return instance.toVolts(c); - - return NAN; + a = instance.toVolts(a_raw); + b = instance.toVolts(b_raw); + if(_isset(_pinC)) + c = instance.toVolts(c_raw); + } /** @@ -263,12 +266,10 @@ void SAMDCurrentSenseADCDMA::initADC(){ adc_evctrl.bit.STARTEI = 1; ADC->EVCTRL.reg = adc_evctrl.reg; - // if(this->EVSYS_ID_GEN_TCC_OVF == -1) - // { - //not evsys + dma driven - ADC->INTENSET.bit.RESRDY = 1; - NVIC_EnableIRQ( ADC_IRQn ); - // } + //not evsys + dma driven + // ADC->INTENSET.bit.RESRDY = 1; + // NVIC_EnableIRQ( ADC_IRQn ); + ADC->CTRLA.bit.ENABLE = 0x01; } @@ -419,7 +420,20 @@ void SAMDCurrentSenseADCDMA::initEVSYS() EVSYS_CHANNEL_Type channel_1{.reg = 0}; channel_1.bit.EDGSEL = EVSYS_CHANNEL_EDGSEL_NO_EVT_OUTPUT_Val; channel_1.bit.PATH = EVSYS_CHANNEL_PATH_ASYNCHRONOUS_Val; - channel_1.bit.EVGEN = EVSYS_ID_GEN_DMAC_CH_0; + switch(channelDMA) + { + case 0: channel_1.bit.EVGEN = EVSYS_ID_GEN_DMAC_CH_0; break; + case 1: channel_1.bit.EVGEN = EVSYS_ID_GEN_DMAC_CH_1; break; + case 2: channel_1.bit.EVGEN = EVSYS_ID_GEN_DMAC_CH_2; break; + case 3: channel_1.bit.EVGEN = EVSYS_ID_GEN_DMAC_CH_3; break; + default: + #ifdef SIMPLEFOC_SAMD_DEBUG + SIMPLEFOC_SAMD_DEBUG_SERIAL.print("Bad dma channel, only 0,1,2 or 3 are supported : "); + SIMPLEFOC_SAMD_DEBUG_SERIAL.println(channelDMA); + #endif + break; + } + channel_1.bit.SWEVT = 0b0; channel_1.bit.CHANNEL = user_1.bit.CHANNEL - 1; EVSYS->CHANNEL.reg = channel_1.reg; From 4afcabfcc5ff862ad16beea2cbdb0d1685974395 Mon Sep 17 00:00:00 2001 From: maxime Date: Wed, 2 Jun 2021 11:50:50 -0600 Subject: [PATCH 07/15] AdvancedSPI for samd --- src/common/hardware_specific/samd21.cpp | 126 +++++++++ .../hardware_specific/samd21_AdvancedSPI.cpp | 245 ++++++++++++++++++ .../hardware_specific/samd21_AdvancedSPI.h | 45 ++++ src/common/hardware_specific/samd_mcu.cpp | 63 +++++ src/common/hardware_specific/samd_mcu.h | 180 +++++++++++++ 5 files changed, 659 insertions(+) create mode 100644 src/common/hardware_specific/samd21.cpp create mode 100644 src/common/hardware_specific/samd21_AdvancedSPI.cpp create mode 100644 src/common/hardware_specific/samd21_AdvancedSPI.h create mode 100644 src/common/hardware_specific/samd_mcu.cpp create mode 100644 src/common/hardware_specific/samd_mcu.h diff --git a/src/common/hardware_specific/samd21.cpp b/src/common/hardware_specific/samd21.cpp new file mode 100644 index 00000000..8488c856 --- /dev/null +++ b/src/common/hardware_specific/samd21.cpp @@ -0,0 +1,126 @@ +#include "samd_mcu.h" + +#ifdef _SAMD21_ + +#if defined __SAMD21J18A__ +#define D_TC(TCC_VALUE) TCC_VALUE +#define D_AIN(AIN_VALUE) AIN_VALUE +#else +#define TC6_CH0 = (6<<8)|(0) +#define TC6_CH1 = (6<<8)|(1) +#define TC7_CH0 = (7<<8)|(0) +#define TC7_CH1 = (7<<8)|(1) +#define D_TC(TCC_VALUE) NOT_ON_TIMER + +#define ADC_Channel8=8 +#define ADC_Channel9=9 +#define ADC_Channel10=10 +#define ADC_Channel11=11 +#define ADC_Channel12=12 +#define ADC_Channel13=13 +#define ADC_Channel14=14 +#define ADC_Channel15=15 +#define D_AIN(AIN_VALUE) No_ADC_Channel +#endif + +#if defined __SAMD21J18A__ +#define D_TC(TCC_VALUE) TCC_VALUE +#define D_AIN(AIN_VALUE) AIN_VALUE +#else +#define TC6_CH0 = (6<<8)|(0) +#define TC6_CH1 = (6<<8)|(1) +#define TC7_CH0 = (7<<8)|(0) +#define TC7_CH1 = (7<<8)|(1) +#define D_TC(TCC_VALUE) NOT_ON_TIMER + +#define ADC_Channel8=8 +#define ADC_Channel9=9 +#define ADC_Channel10=10 +#define ADC_Channel11=11 +#define ADC_Channel12=12 +#define ADC_Channel13=13 +#define ADC_Channel14=14 +#define ADC_Channel15=15 +#define D_AIN(AIN_VALUE) No_ADC_Channel +#endif + +const int8_t g_SamdMapPortPin[2][32] = +{ +// 0 , 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19, 20, 21, 22, 23, 24, 25, 26, 27, 28, 29, 30, 31 +/*PORTA*/{0 , 1, 2, 3, 10, 11, 12, 13, 14, 15, 16, 17, 24, 25, 26, 27, 28, 29, 30, 31, 34, 35, 36, 37, 38, 39, -1, 42, 43, -1, 44, 45}, +/*PORTB*/{48, 49, 50, 51, 4, 5, 6, 7, 8, 9, 18, 19, 20, 21, 22, 23, 32, 33, -1, -1, -1, -1, 40, 41, -1, -1, -1, -1, -1, -1, 46, 47} +}; + + +const SamdPinDefinition g_SamdPinDefinitions[] = { +{ 1, 1 , 1 , PORTA, 0 , VDDANA, NO_Type, EXTERNAL_INT_0 , No_VREF , No_ADC_Channel , No_ADC_Channel , No_PTC_Channel , NO_SERCOM , NO_PAD, SERCOM_1 , PAD_0 , TCC2_CH0 , NOT_ON_TIMER , NO_COM , NO_GCLK}, +{ 2, 2 , 2 , PORTA, 1 , VDDANA, NO_Type, EXTERNAL_INT_1 , No_VREF , No_ADC_Channel , No_ADC_Channel , No_PTC_Channel , NO_SERCOM , NO_PAD, SERCOM_1 , PAD_1 , TCC2_CH1 , NOT_ON_TIMER , NO_COM , NO_GCLK}, +{ 3, 3 , 3 , PORTA, 2 , VDDANA, NO_Type, EXTERNAL_INT_2 , VOUT , ADC_Channel0 , No_ADC_Channel , PTC_Channel_Y0 , NO_SERCOM , NO_PAD, NO_SERCOM , NO_PAD , NOT_ON_TIMER , NOT_ON_TIMER , NO_COM , NO_GCLK}, +{ 4, 4 , 4 , PORTA, 3 , VDDANA, NO_Type, EXTERNAL_INT_3 , VREFA , ADC_Channel1 , No_ADC_Channel , PTC_Channel_Y1 , NO_SERCOM , NO_PAD, NO_SERCOM , NO_PAD , NOT_ON_TIMER , NOT_ON_TIMER , NO_COM , NO_GCLK}, +{ -1, -1 , 5 , PORTB, 4 , VDDANA, NO_Type, EXTERNAL_INT_4 , No_VREF , D_AIN(ADC_Channel12), No_ADC_Channel , PTC_Channel_Y10 , NO_SERCOM , NO_PAD, NO_SERCOM , NO_PAD , NOT_ON_TIMER , NOT_ON_TIMER , NO_COM , NO_GCLK}, +{ -1, -1 , 6 , PORTB, 5 , VDDANA, NO_Type, EXTERNAL_INT_5 , No_VREF , D_AIN(ADC_Channel13), No_ADC_Channel , PTC_Channel_Y11 , NO_SERCOM , NO_PAD, NO_SERCOM , NO_PAD , NOT_ON_TIMER , NOT_ON_TIMER , NO_COM , NO_GCLK}, +{ -1, -1 , 9 , PORTB, 6 , VDDANA, NO_Type, EXTERNAL_INT_6 , No_VREF , D_AIN(ADC_Channel14), No_ADC_Channel , PTC_Channel_Y12 , NO_SERCOM , NO_PAD, NO_SERCOM , NO_PAD , NOT_ON_TIMER , NOT_ON_TIMER , NO_COM , NO_GCLK}, +{ -1, -1 , 10, PORTB, 7 , VDDANA, NO_Type, EXTERNAL_INT_7 , No_VREF , D_AIN(ADC_Channel15), No_ADC_Channel , PTC_Channel_Y13 , NO_SERCOM , NO_PAD, NO_SERCOM , NO_PAD , NOT_ON_TIMER , NOT_ON_TIMER , NO_COM , NO_GCLK}, +{ -1, 7 , 11, PORTB, 8 , VDDANA, NO_Type, EXTERNAL_INT_8 , No_VREF , ADC_Channel2 , No_ADC_Channel , PTC_Channel_Y14 , NO_SERCOM , NO_PAD, SERCOM_4 , PAD_0 , TC4_CH0 , NOT_ON_TIMER , NO_COM , NO_GCLK}, +{ -1, 8 , 12, PORTB, 9 , VDDANA, NO_Type, EXTERNAL_INT_9 , No_VREF , ADC_Channel3 , No_ADC_Channel , PTC_Channel_Y15 , NO_SERCOM , NO_PAD, SERCOM_4 , PAD_1 , TC4_CH1 , NOT_ON_TIMER , NO_COM , NO_GCLK}, +{ 5, 9 , 13, PORTA, 4 , VDDANA, NO_Type, EXTERNAL_INT_4 , VREFB , ADC_Channel4 , ADC_Channel0 , PTC_Channel_Y2 , NO_SERCOM , NO_PAD, SERCOM_0 , PAD_0 , TCC0_CH0 , NOT_ON_TIMER , NO_COM , NO_GCLK}, +{ 6, 10, 14, PORTA, 5 , VDDANA, NO_Type, EXTERNAL_INT_5 , No_VREF , ADC_Channel5 , ADC_Channel1 , PTC_Channel_Y3 , NO_SERCOM , NO_PAD, SERCOM_0 , PAD_1 , TCC0_CH1 , NOT_ON_TIMER , NO_COM , NO_GCLK}, +{ 7, 11, 15, PORTA, 6 , VDDANA, NO_Type, EXTERNAL_INT_6 , No_VREF , ADC_Channel6 , ADC_Channel2 , PTC_Channel_Y4 , NO_SERCOM , NO_PAD, SERCOM_0 , PAD_2 , TCC1_CH0 , NOT_ON_TIMER , NO_COM , NO_GCLK}, +{ 8, 12, 16, PORTA, 7 , VDDANA, NO_Type, EXTERNAL_INT_7 , No_VREF , ADC_Channel7 , ADC_Channel3 , PTC_Channel_Y5 , NO_SERCOM , NO_PAD, SERCOM_0 , PAD_3 , TCC1_CH1 , NOT_ON_TIMER , I2S_SD0 , NO_GCLK}, +{ 11, 13, 17, PORTA, 8 , VDDIO , NO_Type, EXTERNAL_INT_NMI , No_VREF , ADC_Channel16 , No_ADC_Channel , PTC_Channel_X0 , SERCOM_0 , PAD_0 , SERCOM_2 , PAD_0 , TCC0_CH0 , TCC1_CH2 , I2S_SD1 , NO_GCLK}, +{ 12, 14, 18, PORTA, 9 , VDDIO , NO_Type, EXTERNAL_INT_9 , No_VREF , ADC_Channel17 , No_ADC_Channel , PTC_Channel_X1 , SERCOM_0 , PAD_1 , SERCOM_2 , PAD_1 , TCC1_CH3 , TCC0_CH1 , I2S_MCK0 , NO_GCLK}, +{ 13, 15, 19, PORTA, 10, VDDIO , NO_Type, EXTERNAL_INT_10 , No_VREF , ADC_Channel18 , No_ADC_Channel , PTC_Channel_X2 , SERCOM_0 , PAD_2 , SERCOM_2 , PAD_2 , TCC0_CH2 , TCC1_CH0 , I2S_SCK0 , GCLK_IO_4}, +{ 14, 16, 20, PORTA, 11, VDDIO , NO_Type, EXTERNAL_INT_11 , No_VREF , ADC_Channel19 , No_ADC_Channel , PTC_Channel_X3 , SERCOM_0 , PAD_3 , SERCOM_2 , PAD_3 , TCC1_CH1 , NOT_ON_TIMER , I2S_FS0 , GCLK_IO_5}, +{ -1, 19, 23, PORTB, 10, VDDIO , NO_Type, EXTERNAL_INT_10 , No_VREF , No_ADC_Channel , No_ADC_Channel , No_PTC_Channel , NO_SERCOM , NO_PAD, SERCOM_4 , PAD_2 , TC5_CH0 , TCC0_CH4 , I2S_MCK1 , GCLK_IO_4}, +{ -1, 20, 24, PORTB, 11, VDDIO , NO_Type, EXTERNAL_INT_11 , No_VREF , No_ADC_Channel , No_ADC_Channel , No_PTC_Channel , NO_SERCOM , NO_PAD, SERCOM_4 , PAD_3 , TC5_CH1 , TCC0_CH5 , I2S_SCK1 , GCLK_IO_5}, +{ -1, -1, 25, PORTB, 12, VDDIO , I2C , EXTERNAL_INT_12 , No_VREF , No_ADC_Channel , No_ADC_Channel , PTC_Channel_X12 , SERCOM_4 , PAD_0 , NO_SERCOM , NO_PAD , TC4_CH0 , TCC0_CH6 , I2S_FS1 , GCLK_IO_6}, +{ -1, -1, 26, PORTB, 13, VDDIO , I2C , EXTERNAL_INT_13 , No_VREF , No_ADC_Channel , No_ADC_Channel , PTC_Channel_X13 , SERCOM_4 , PAD_1 , NO_SERCOM , NO_PAD , TC4_CH1 , TCC0_CH7 , NO_COM , GCLK_IO_7}, +{ -1, -1, 27, PORTB, 14, VDDIO , NO_Type, EXTERNAL_INT_14 , No_VREF , No_ADC_Channel , No_ADC_Channel , PTC_Channel_X14 , SERCOM_4 , PAD_2 , NO_SERCOM , NO_PAD , TC5_CH0 , NOT_ON_TIMER , NO_COM , GCLK_IO_0}, +{ -1, -1, 28, PORTB, 15, VDDIO , NO_Type, EXTERNAL_INT_15 , No_VREF , No_ADC_Channel , No_ADC_Channel , PTC_Channel_X15 , SERCOM_4 , PAD_3 , NO_SERCOM , NO_PAD , TC5_CH1 , NOT_ON_TIMER , NO_COM , GCLK_IO_1}, +{ -1, 21, 29, PORTA, 12, VDDIO , I2C , EXTERNAL_INT_12 , No_VREF , No_ADC_Channel , No_ADC_Channel , No_PTC_Channel , SERCOM_2 , PAD_0 , SERCOM_4 , PAD_0 , TCC2_CH0 , TCC0_CH6 , NO_COM , AC_CMP_0}, +{ -1, 22, 30, PORTA, 13, VDDIO , I2C , EXTERNAL_INT_13 , No_VREF , No_ADC_Channel , No_ADC_Channel , No_PTC_Channel , SERCOM_2 , PAD_1 , SERCOM_4 , PAD_1 , TCC2_CH1 , TCC0_CH7 , NO_COM , AC_CMP_1}, +{ 15, 23, 31, PORTA, 14, VDDIO , NO_Type, EXTERNAL_INT_14 , No_VREF , No_ADC_Channel , No_ADC_Channel , No_PTC_Channel , SERCOM_2 , PAD_2 , SERCOM_4 , PAD_2 , TC3_CH0 , TCC0_CH4 , NO_COM , GCLK_IO_0}, +{ 16, 24, 32, PORTA, 15, VDDIO , NO_Type, EXTERNAL_INT_15 , No_VREF , No_ADC_Channel , No_ADC_Channel , No_PTC_Channel , SERCOM_2 , PAD_3 , SERCOM_4 , PAD_3 , TC3_CH1 , TCC0_CH5 , NO_COM , GCLK_IO_1}, +{ 17, 25, 35, PORTA, 16, VDDIO , I2C , EXTERNAL_INT_0 , No_VREF , No_ADC_Channel , No_ADC_Channel , PTC_Channel_X4 , SERCOM_1 , PAD_0 , SERCOM_3 , PAD_0 , TCC2_CH0 , TCC0_CH6 , NO_COM , GCLK_IO_2}, +{ 18, 26, 36, PORTA, 17, VDDIO , I2C , EXTERNAL_INT_1 , No_VREF , No_ADC_Channel , No_ADC_Channel , PTC_Channel_X5 , SERCOM_1 , PAD_1 , SERCOM_3 , PAD_1 , TCC2_CH1 , TCC0_CH7 , NO_COM , GCLK_IO_3}, +{ 19, 27, 37, PORTA, 18, VDDIO , NO_Type, EXTERNAL_INT_2 , No_VREF , No_ADC_Channel , No_ADC_Channel , PTC_Channel_X6 , SERCOM_1 , PAD_2 , SERCOM_3 , PAD_2 , TC3_CH0 , TCC0_CH2 , NO_COM , AC_CMP_0}, +{ 20, 28, 38, PORTA, 19, VDDIO , NO_Type, EXTERNAL_INT_3 , No_VREF , No_ADC_Channel , No_ADC_Channel , PTC_Channel_X7 , SERCOM_1 , PAD_3 , SERCOM_3 , PAD_3 , TC3_CH1 , TCC0_CH3 , I2S_SD0 , AC_CMP_1}, +{ -1, -1, 39, PORTB, 16, VDDIO , I2C , EXTERNAL_INT_0 , No_VREF , No_ADC_Channel , No_ADC_Channel , No_PTC_Channel , SERCOM_5 , PAD_0 , NO_SERCOM , NO_PAD , D_TC(TC6_CH0), TCC0_CH4 , I2S_SD1 , GCLK_IO_2}, +{ -1, -1, 40, PORTB, 17, VDDIO , I2C , EXTERNAL_INT_1 , No_VREF , No_ADC_Channel , No_ADC_Channel , No_PTC_Channel , SERCOM_5 , PAD_1 , NO_SERCOM , NO_PAD , D_TC(TC6_CH1), TCC0_CH5 , I2S_MCK0 , GCLK_IO_3}, +{ -1, 29, 41, PORTA, 20, VDDIO , NO_Type, EXTERNAL_INT_4 , No_VREF , No_ADC_Channel , No_ADC_Channel , PTC_Channel_X8 , SERCOM_5 , PAD_2 , SERCOM_3 , PAD_2 , D_TC(TC7_CH0), TCC0_CH6 , I2S_SCK0 , GCLK_IO_4}, +{ -1, 30, 42, PORTA, 21, VDDIO , NO_Type, EXTERNAL_INT_5 , No_VREF , No_ADC_Channel , No_ADC_Channel , PTC_Channel_X9 , SERCOM_5 , PAD_3 , SERCOM_3 , PAD_3 , D_TC(TC7_CH1), TCC0_CH7 , I2S_FS0 , GCLK_IO_5}, +{ 21, 31, 43, PORTA, 22, VDDIO , I2C , EXTERNAL_INT_6 , No_VREF , No_ADC_Channel , No_ADC_Channel , PTC_Channel_X10 , SERCOM_3 , PAD_0 , SERCOM_5 , PAD_0 , TC4_CH0 , TCC0_CH4 , NO_COM , GCLK_IO_6}, +{ 22, 32, 44, PORTA, 23, VDDIO , I2C , EXTERNAL_INT_7 , No_VREF , No_ADC_Channel , No_ADC_Channel , PTC_Channel_X11 , SERCOM_3 , PAD_1 , SERCOM_5 , PAD_1 , TC4_CH1 , TCC0_CH5 , USB_SOF1kHz, GCLK_IO_7}, +{ 23, 33, 45, PORTA, 24, VDDIO , NO_Type, EXTERNAL_INT_12 , No_VREF , No_ADC_Channel , No_ADC_Channel , No_PTC_Channel , SERCOM_3 , PAD_2 , SERCOM_5 , PAD_2 , TC5_CH0 , TCC1_CH2 , USB_DM , NO_GCLK}, +{ 24, 34, 46, PORTA, 25, VDDIO , NO_Type, EXTERNAL_INT_13 , No_VREF , No_ADC_Channel , No_ADC_Channel , No_PTC_Channel , SERCOM_3 , PAD_3 , SERCOM_5 , PAD_3 , TC5_CH1 , TCC1_CH3 , USB_DP , NO_GCLK}, +{ -1, 37, 49, PORTB, 22, VDDIO , NO_Type, EXTERNAL_INT_6 , No_VREF , No_ADC_Channel , No_ADC_Channel , No_PTC_Channel , NO_SERCOM , NO_PAD, SERCOM_5 , PAD_2 , D_TC(TC7_CH0), NOT_ON_TIMER , NO_COM , GCLK_IO_0}, +{ -1, 38, 50, PORTB, 23, VDDIO , NO_Type, EXTERNAL_INT_7 , No_VREF , No_ADC_Channel , No_ADC_Channel , No_PTC_Channel , NO_SERCOM , NO_PAD, SERCOM_5 , PAD_3 , D_TC(TC7_CH1), NOT_ON_TIMER , NO_COM , GCLK_IO_1}, +{ 25, 39, 51, PORTA, 27, VDDIO , NO_Type, EXTERNAL_INT_15 , No_VREF , No_ADC_Channel , No_ADC_Channel , No_PTC_Channel , NO_SERCOM , NO_PAD, NO_SERCOM , NO_PAD , NOT_ON_TIMER , NOT_ON_TIMER , NO_COM , GCLK_IO_0}, +{ 27, 41, 53, PORTA, 28, VDDIO , NO_Type, EXTERNAL_INT_8 , No_VREF , No_ADC_Channel , No_ADC_Channel , No_PTC_Channel , NO_SERCOM , NO_PAD, NO_SERCOM , NO_PAD , NOT_ON_TIMER , NOT_ON_TIMER , NO_COM , GCLK_IO_0}, +{ 31, 45, 57, PORTA, 30, VDDIO , NO_Type, EXTERNAL_INT_10 , No_VREF , No_ADC_Channel , No_ADC_Channel , No_PTC_Channel , NO_SERCOM , NO_PAD, SERCOM_1 , PAD_2 , TCC1_CH0 , NOT_ON_TIMER , SWCLK , GCLK_IO_0}, +{ 32, 46, 58, PORTA, 31, VDDIO , NO_Type, EXTERNAL_INT_11 , No_VREF , No_ADC_Channel , No_ADC_Channel , No_PTC_Channel , NO_SERCOM , NO_PAD, SERCOM_1 , PAD_3 , TCC1_CH1 , NOT_ON_TIMER , SWDIO , NO_GCLK}, +{ -1, -1, 59, PORTB, 30, VDDIO , I2C , EXTERNAL_INT_14 , No_VREF , No_ADC_Channel , No_ADC_Channel , No_PTC_Channel , NO_SERCOM , NO_PAD, SERCOM_5 , PAD_0 , TCC0_CH0 , TCC1_CH2 , NO_COM , NO_GCLK}, +{ -1, -1, 60, PORTB, 31, VDDIO , I2C , EXTERNAL_INT_15 , No_VREF , No_ADC_Channel , No_ADC_Channel , No_PTC_Channel , NO_SERCOM , NO_PAD, SERCOM_5 , PAD_1 , TCC0_CH1 , TCC1_CH3 , NO_COM , NO_GCLK}, +{ -1, -1, 61, PORTB, 0, VDDANA, NO_Type , EXTERNAL_INT_0 , No_VREF , D_AIN(ADC_Channel8) , No_ADC_Channel , PTC_Channel_Y6 , NO_SERCOM , NO_PAD, SERCOM_5 , PAD_2 , D_TC(TC7_CH0), NOT_ON_TIMER , NO_COM , NO_GCLK}, +{ -1, -1, 62, PORTB, 1, VDDANA, NO_Type , EXTERNAL_INT_1 , No_VREF , D_AIN(ADC_Channel9) , No_ADC_Channel , PTC_Channel_Y7 , NO_SERCOM , NO_PAD, SERCOM_5 , PAD_3 , D_TC(TC7_CH1), NOT_ON_TIMER , NO_COM , NO_GCLK}, +{ -1, 47, 63, PORTB, 2, VDDANA, NO_Type , EXTERNAL_INT_2 , No_VREF , D_AIN(ADC_Channel10), No_ADC_Channel , PTC_Channel_Y8 , NO_SERCOM , NO_PAD, SERCOM_5 , PAD_0 , D_TC(TC6_CH0), NOT_ON_TIMER , NO_COM , NO_GCLK}, +{ -1, 48, 64, PORTB, 3, VDDANA, NO_Type , EXTERNAL_INT_3 , No_VREF , D_AIN(ADC_Channel11), No_ADC_Channel , PTC_Channel_Y9 , NO_SERCOM , NO_PAD, SERCOM_5 , PAD_1 , D_TC(TC6_CH1), NOT_ON_TIMER , NO_COM , NO_GCLK}, +}; + + +SercomConfig getSercom(SercomChannel channel) +{ + switch(channel) + { + case SERCOM_0: return {.sercom =SERCOM0, .irq = SERCOM0_IRQn, .clockId = GCM_SERCOM0_CORE}; + case SERCOM_1: return {.sercom =SERCOM1, .irq = SERCOM1_IRQn, .clockId = GCM_SERCOM1_CORE}; + case SERCOM_2: return {.sercom =SERCOM2, .irq = SERCOM2_IRQn, .clockId = GCM_SERCOM2_CORE}; + case SERCOM_3: return {.sercom =SERCOM3, .irq = SERCOM3_IRQn, .clockId = GCM_SERCOM3_CORE}; + case SERCOM_4: return {.sercom =SERCOM4, .irq = SERCOM4_IRQn, .clockId = GCM_SERCOM4_CORE}; + case SERCOM_5: return {.sercom =SERCOM5, .irq = SERCOM5_IRQn, .clockId = GCM_SERCOM5_CORE}; + default: + return {.sercom = nullptr, .irq = PendSV_IRQn, .clockId = 0}; + } +} + +#endif //_SAMD21_ diff --git a/src/common/hardware_specific/samd21_AdvancedSPI.cpp b/src/common/hardware_specific/samd21_AdvancedSPI.cpp new file mode 100644 index 00000000..bc0cd111 --- /dev/null +++ b/src/common/hardware_specific/samd21_AdvancedSPI.cpp @@ -0,0 +1,245 @@ +#include "samd21_AdvancedSPI.h" + +#ifdef _SAMD21_ + +#include + + +class SSGuard +{ + public: + SSGuard(SAMDAdvancedSPI & spi) : spi(spi), isHoldingGuard(false) + { + if(!spi.hardwareSS && !spi.isSoftwareSSLow) + { + digitalWrite(spi.pinSS, LOW); + spi.isSoftwareSSLow = true; + isHoldingGuard = true; + } + } + ~SSGuard() + { + if(isHoldingGuard) + { + digitalWrite(spi.pinSS, HIGH); + spi.isSoftwareSSLow = false; + } + } + + SAMDAdvancedSPI & spi; + bool isHoldingGuard; + +}; + + +SAMDAdvancedSPI::SAMDAdvancedSPI(SercomChannel sercomChannel, uint8_t pinMOSI, uint8_t pinMISO, uint8_t pinSCK, uint8_t pinSS, bool hardwareSS) : + sercomChannel(sercomChannel), pinMOSI(pinMOSI), pinMISO(pinMISO), pinSCK(pinSCK), pinSS(pinSS), hardwareSS(hardwareSS) +{ + +} + +int SAMDAdvancedSPI::init(SercomSpiClockMode spi_mode, uint32_t clock_speed) +{ + debugPrintf("MOSI: "); + defMOSI = getSamdPinDefinition(pinMOSI); + debugPrintf("MISO: "); + defMISO = getSamdPinDefinition(pinMISO); + debugPrintf("SCK : "); + defSCK = getSamdPinDefinition(pinSCK); + debugPrintf("SS : "); + defSS = getSamdPinDefinition(pinSS); + + if(defMOSI == nullptr || defMISO == nullptr || defSCK == nullptr || (hardwareSS && defSS == nullptr)) + { + return -1; + } + + sercomCfg = getSercom(sercomChannel); + + sercomCfg.sercom->SPI.CTRLA.bit.ENABLE = 0; + while(sercomCfg.sercom->SPI.SYNCBUSY.bit.ENABLE); + + // Setting NVIC + NVIC_EnableIRQ(sercomCfg.irq); + NVIC_SetPriority (sercomCfg.irq, SERCOM_NVIC_PRIORITY); /* set Priority */ + + //Setting clock + GCLK_CLKCTRL_Type clckctrl{.reg = 0}; + clckctrl.bit.ID = sercomCfg.clockId ; + clckctrl.bit.GEN = GCLK_CLKCTRL_GEN_GCLK0_Val; + clckctrl.bit.CLKEN = 0b1; + GCLK->CLKCTRL.reg = clckctrl.reg; + + while ( GCLK->STATUS.reg & GCLK_STATUS_SYNCBUSY ); + + if(!hardwareSS) + { + pinMode(pinSS, OUTPUT); + digitalWrite(pinSS, HIGH); + isSoftwareSSLow = false; + } + + //Setting the CTRLB register + SERCOM_SPI_CTRLB_Type ctrlb{.reg = 0}; + ctrlb.bit.CHSIZE = SPI_CHAR_SIZE_8_BITS; + ctrlb.bit.RXEN = 0b1; + ctrlb.bit.MSSEN = hardwareSS ? 0b1 : 0b0; + sercomCfg.sercom->SPI.CTRLB.reg = ctrlb.reg; + while(sercomCfg.sercom->SPI.SYNCBUSY.bit.CTRLB); + + + //Synchronous arithmetic + sercomCfg.sercom->SPI.BAUD.reg = SERCOM_FREQ_REF / (2 * clock_speed) - 1; + + //Setting the CTRLA register + SERCOM_SPI_CTRLA_Type ctrla{.reg = 0}; + switch (spi_mode) + { + case SERCOM_SPI_MODE_0: + ctrla.bit.CPOL = 0b0; + ctrla.bit.CPHA = 0b0; + break; + case SERCOM_SPI_MODE_1: + ctrla.bit.CPOL = 0b0; + ctrla.bit.CPHA = 0b1; + break; + case SERCOM_SPI_MODE_2: + ctrla.bit.CPOL = 0b1; + ctrla.bit.CPHA = 0b0; + break; + case SERCOM_SPI_MODE_3: + ctrla.bit.CPOL = 0b1; + ctrla.bit.CPHA = 0b1; + break; + default: + debugPrint("SPI sercom CLOCK configuration error!"); + return -1; + } + + SercomPad MOSI_pad, MISO_pad, SCK_pad, SS_pad; + EPioType peripheral; + + if(defMOSI->sercom == sercomChannel && defMISO->sercom == sercomChannel && defSCK->sercom == sercomChannel && (ctrlb.bit.MSSEN == 0 || defSS->sercom == sercomChannel)) + { + MOSI_pad = defMOSI->sercom_pad; + MISO_pad = defMISO->sercom_pad; + SCK_pad = defSCK->sercom_pad; + SS_pad = defSS->sercom_pad; + peripheral = EPioType::PIO_SERCOM; + + debugPrintf("Using sercom %d\n\r", defMOSI->sercom); + } + else if(defMOSI->sercom_alt == sercomChannel && defMISO->sercom_alt == sercomChannel && defSCK->sercom_alt == sercomChannel && (ctrlb.bit.MSSEN == 0 || defSS->sercom_alt == sercomChannel)) + { + MOSI_pad = defMOSI->sercom_pad_alt; + MISO_pad = defMISO->sercom_pad_alt; + SCK_pad = defSCK->sercom_pad_alt; + SS_pad = defSS->sercom_pad_alt; + peripheral = EPioType::PIO_SERCOM_ALT; + + debugPrintf("Using sercom alt %d\n\r", defMOSI->sercom_alt); + } + else + { + debugPrint("SPI sercom CHANNEL configuration error!"); + return -1; + } + + pinPeripheral(pinMOSI, peripheral); + pinPeripheral(pinMISO, peripheral); + pinPeripheral(pinSCK, peripheral); + pinPeripheral(pinSS, ctrlb.bit.MSSEN ? peripheral : EPioType::PIO_OUTPUT); + + debugPrintf("Using sercom pads MOSI[%d], MISO[%d], SCK[%d], SS[%d]\n\r", MOSI_pad, MISO_pad, SCK_pad, ctrlb.bit.MSSEN ? SS_pad : -1); + + if(MOSI_pad == PAD_0 && SCK_pad == PAD_1 + && (ctrlb.bit.MSSEN ? (SS_pad == PAD_2 && MISO_pad == PAD_3) : (MISO_pad == PAD_3 || MISO_pad == PAD_2))) + { + ctrla.bit.DOPO = 0x0; + ctrla.bit.DIPO = MISO_pad; + } + else if(MOSI_pad == PAD_2 && SCK_pad == PAD_3 + && (ctrlb.bit.MSSEN ? (SS_pad == PAD_1 && MISO_pad == PAD_0) : (MISO_pad == PAD_0 || MISO_pad == PAD_1))) + { + ctrla.bit.DOPO = 0x1; + ctrla.bit.DIPO = MISO_pad; + } + else if(MOSI_pad == PAD_3 && SCK_pad == PAD_1 + && (ctrlb.bit.MSSEN ? (SS_pad == PAD_2 && MISO_pad == PAD_0) : (MISO_pad == PAD_0 || MISO_pad == PAD_2))) + { + ctrla.bit.DOPO = 0x2; + ctrla.bit.DIPO = MISO_pad; + } + else if(MOSI_pad == PAD_0 && SCK_pad == PAD_3 + && (ctrlb.bit.MSSEN ? (SS_pad == PAD_1 && MISO_pad == PAD_2) : (MISO_pad == PAD_2 || MISO_pad == PAD_1))) + { + ctrla.bit.DOPO = 0x3; + ctrla.bit.DIPO = MISO_pad; + } + else + { + debugPrint("SPI sercom PADS configuration invalid!"); + return -1; + } + + debugPrintf("Using sercom DOPO %d and DIPO %d\n\r", ctrla.bit.DOPO, ctrla.bit.DIPO); + + ctrla.bit.MODE = SERCOM_SPI_CTRLA_MODE_SPI_MASTER_Val; + ctrla.bit.DORD = SercomDataOrder::MSB_FIRST; + + + ctrla.bit.ENABLE = 0b1; //***enables the SPI*** + + sercomCfg.sercom->SPI.CTRLA.reg = ctrla.reg; + while(sercomCfg.sercom->SPI.SYNCBUSY.bit.ENABLE); + + + return 0; + +} + + +uint8_t SAMDAdvancedSPI::transfer(uint8_t data) +{ + SSGuard g(*this); + sercomCfg.sercom->SPI.DATA.bit.DATA = data; // Writing data into Data register + while( sercomCfg.sercom->SPI.INTFLAG.bit.RXC == 0 ); + return sercomCfg.sercom->SPI.DATA.bit.DATA; // Reading data +} + + + + +uint16_t SAMDAdvancedSPI::transfer16(uint16_t data) +{ + union { uint16_t val; struct { uint8_t lsb; uint8_t msb; }; } t; + + + SSGuard g(*this); + + t.val = data; + + if (sercomCfg.sercom->SPI.CTRLA.bit.DORD == SercomDataOrder::LSB_FIRST) { + t.lsb = transfer(t.lsb); + t.msb = transfer(t.msb); + } else { + t.msb = transfer(t.msb); + t.lsb = transfer(t.lsb); + } + + + return t.val; +} + +/** + * Closes the SPI connection + * SPI has an internal SPI-device counter, for each init()-call the close() function must be called exactly 1 time + */ +void SAMDAdvancedSPI::close(){ + //Setting the Software Reset bit to 1 + sercomCfg.sercom->SPI.CTRLA.bit.SWRST = 1; + //Wait both bits Software Reset from CTRLA and SYNCBUSY are equal to 0 + while(sercomCfg.sercom->SPI.CTRLA.bit.SWRST || sercomCfg.sercom->SPI.SYNCBUSY.bit.SWRST); +} + +#endif \ No newline at end of file diff --git a/src/common/hardware_specific/samd21_AdvancedSPI.h b/src/common/hardware_specific/samd21_AdvancedSPI.h new file mode 100644 index 00000000..3e95d5e9 --- /dev/null +++ b/src/common/hardware_specific/samd21_AdvancedSPI.h @@ -0,0 +1,45 @@ +#pragma once + +#include + +#ifdef _SAMD21_ + +#include "samd_mcu.h" + +class SAMDAdvancedSPI +{ + public: + + SAMDAdvancedSPI(SercomChannel sercomChannel, uint8_t pinMOSI, uint8_t pinMISO, uint8_t pinSCK, uint8_t pinSS, bool hardwareSS); + + int init(SercomSpiClockMode spi_mode, uint32_t clock_speed); + + byte spiCalcEvenParity(word value); + + uint8_t transfer(uint8_t data); + + uint16_t transfer16(uint16_t data); + + void close(); + + private: + + + SercomChannel sercomChannel; + const uint8_t pinMOSI; + const uint8_t pinMISO; + const uint8_t pinSCK; + const uint8_t pinSS; + const bool hardwareSS; + bool isSoftwareSSLow; + + const SamdPinDefinition * defMOSI; + const SamdPinDefinition * defMISO; + const SamdPinDefinition * defSCK; + const SamdPinDefinition * defSS; + SercomConfig sercomCfg; + + friend class SSGuard; +}; + +#endif \ No newline at end of file diff --git a/src/common/hardware_specific/samd_mcu.cpp b/src/common/hardware_specific/samd_mcu.cpp new file mode 100644 index 00000000..35c378e3 --- /dev/null +++ b/src/common/hardware_specific/samd_mcu.cpp @@ -0,0 +1,63 @@ +#include "samd_mcu.h" + +#include + +SercomSpiClockMode from_SPI_MODE(int spi_mode) +{ + switch (spi_mode) + { + case SPI_MODE0: + return SERCOM_SPI_MODE_0; + case SPI_MODE1: + return SERCOM_SPI_MODE_1; + case SPI_MODE2: + return SERCOM_SPI_MODE_2; + case SPI_MODE3: + return SERCOM_SPI_MODE_3; + default: + return SERCOM_SPI_MODE_0; + } +} + +const SamdPinDefinition * getSamdPinDefinition(int arduinoPin) +{ + if(arduinoPin < 0) + { + debugPrint(F("getSamdPinDefinition() : pin < 0")); + return nullptr; + } + if(arduinoPin > PINS_COUNT-1) + { + debugPrintf("getSamdPinDefinition() : arduino pin %d above %d", arduinoPin, PINS_COUNT-1); + return nullptr; + } + + EPortType port = g_APinDescription[arduinoPin].ulPort; + uint8_t port_pin = g_APinDescription[arduinoPin].ulPin; + + if(port_pin > 31) + { + debugPrint(F("getSamdPinDefinition() : port_pin > 31")); + return nullptr; + } + + if(port_pin > 31) + { + debugPrint(F("getSamdPinDefinition() : port_pin > 31")); + return nullptr; + } + + int8_t index = g_SamdMapPortPin[port][port_pin]; + + if(index < 0) + { + debugPrint(F("getSamdPinDefinition() : index < 0, the pin doesn't exist!")); + return nullptr; + } + + const SamdPinDefinition * rv = &g_SamdPinDefinitions[index]; + + debugPrintf("getSamdPinDefinition(): Arduino pin: %d, PORT%c%d, SERCOM%d:PAD[%d], SERCOM_ALT%d:PAD[%d] \n\r", arduinoPin, 'A' + port, port_pin, rv->sercom, rv->sercom_pad, rv->sercom_alt, rv->sercom_pad_alt); + + return rv; +} \ No newline at end of file diff --git a/src/common/hardware_specific/samd_mcu.h b/src/common/hardware_specific/samd_mcu.h new file mode 100644 index 00000000..e570f5f1 --- /dev/null +++ b/src/common/hardware_specific/samd_mcu.h @@ -0,0 +1,180 @@ +#pragma once +#include + + +typedef enum +{ + SPI_PAD_0_SCK_1_SS_2 = 0, + SPI_PAD_2_SCK_3_SS_1, + SPI_PAD_3_SCK_1_SS_2, + SPI_PAD_0_SCK_3_SS_1 +} SercomSpiDOPO; + +typedef enum +{ + NO_SERCOM = -1, + SERCOM_0 = 0, + SERCOM_1, + SERCOM_2, + SERCOM_3, + SERCOM_4, + SERCOM_5, +} SercomChannel; + +typedef struct _SercomConfig +{ + Sercom * sercom; + IRQn_Type irq; + uint8_t clockId; +} SercomConfig; + +SercomConfig getSercom(SercomChannel channel); + +typedef enum +{ + NO_PAD = -1, + PAD_0 = 0, + PAD_1, + PAD_2, + PAD_3, +} SercomPad; + +typedef enum +{ + No_VREF = -1, + VREFA = 0, + VREFB, + VOUT, +} VRef; + +typedef enum +{ + No_PTC_Channel=-1, + PTC_Channel_X0 =0x00 + 0, + PTC_Channel_X1 =0x00 + 1, + PTC_Channel_X2 =0x00 + 2, + PTC_Channel_X3 =0x00 + 3, + PTC_Channel_X4 =0x00 + 4, + PTC_Channel_X5 =0x00 + 5, + PTC_Channel_X6 =0x00 + 6, + PTC_Channel_X7 =0x00 + 7, + PTC_Channel_X8 =0x00 + 8, + PTC_Channel_X9 =0x00 + 9, + PTC_Channel_X10=0x00 + 10, + PTC_Channel_X11=0x00 + 11, + PTC_Channel_X12=0x00 + 12, + PTC_Channel_X13=0x00 + 13, + PTC_Channel_X14=0x00 + 14, + PTC_Channel_X15=0x00 + 15, + PTC_Channel_Y0 =0x10 + 0, + PTC_Channel_Y1 =0x10 + 1, + PTC_Channel_Y2 =0x10 + 2, + PTC_Channel_Y3 =0x10 + 3, + PTC_Channel_Y4 =0x10 + 4, + PTC_Channel_Y5 =0x10 + 5, + PTC_Channel_Y6 =0x10 + 6, + PTC_Channel_Y7 =0x10 + 7, + PTC_Channel_Y8 =0x10 + 8, + PTC_Channel_Y9 =0x10 + 9, + PTC_Channel_Y10=0x10 + 10, + PTC_Channel_Y11=0x10 + 11, + PTC_Channel_Y12=0x10 + 12, + PTC_Channel_Y13=0x10 + 13, + PTC_Channel_Y14=0x10 + 14, + PTC_Channel_Y15=0x10 + 15, +} PeripheralTouchChannel; + +typedef enum +{ + NO_COM = -1, + I2S_SD0 = 0, + I2S_SD1, + I2S_MCK0, + I2S_MCK1, + I2S_SCK0, + I2S_SCK1, + I2S_FS0, + I2S_FS1, + USB_SOF1kHz, + USB_DM, + USB_DP, + SWCLK, + SWDIO, + +} ComChannel; + +typedef enum +{ + VDDANA, + VDDIO +} VDDType; + +typedef enum +{ + NO_Type, + I2C +} COMType; + +typedef enum +{ + NO_GCLK, + GCLK_IO_0 = 0, + GCLK_IO_1 = 1, + GCLK_IO_2 = 2, + GCLK_IO_3 = 3, + GCLK_IO_4 = 4, + GCLK_IO_5 = 5, + GCLK_IO_6 = 6, + GCLK_IO_7 = 7, + AC_CMP_0 = 0x10 + 0, + AC_CMP_1 = 0x10 + 1, + AC_CMP_2 = 0x10 + 2, + AC_CMP_3 = 0x10 + 3, +} GCLKChannel; + +typedef struct _SamdPinDefinition +{ + int8_t SAMD21E_chip_pin; + int8_t SAMD21G_chip_pin; + int8_t SAMD21J_chip_pin; + EPortType port; + uint8_t port_pin; + VDDType vdd; + COMType is_i2c; + EExt_Interrupts extrernal_interrupt; + VRef vref; + EAnalogChannel adc_channel; + EAnalogChannel dac_channel; + PeripheralTouchChannel ptc; + SercomChannel sercom; + SercomPad sercom_pad; + SercomChannel sercom_alt; + SercomPad sercom_pad_alt; + ETCChannel tc_tcc; + ETCChannel tcc; + ComChannel com_channel; + GCLKChannel gclk_channel; +} SamdPinDefinition; + + + +extern const int8_t g_SamdMapPortPin[2][32]; +extern const SamdPinDefinition g_SamdPinDefinitions[]; + + +#ifdef SIMPLEFOC_SAMD_DEBUG +template +void debugPrint(T message) +{ + SIMPLEFOC_SAMD_DEBUG_SERIAL.print(message); +} +static char buffer[1000]; +#define debugPrintf(args...) buffer[999] = '\0'; sprintf(buffer, args); debugPrint(buffer); +#else +#define debugPrintf(args...) ; +#define debugPrint(arg) ; +#endif + +SercomSpiClockMode from_SPI_MODE(int spi_mode); + +const SamdPinDefinition * getSamdPinDefinition(int arduinoPin); From e3c1a4b2ad7642ff09621f0b152074446eea1bc1 Mon Sep 17 00:00:00 2001 From: maxime Date: Thu, 3 Jun 2021 15:19:53 -0600 Subject: [PATCH 08/15] AdvancedSPI for samd --- .../hardware_specific/samd21_AdvancedSPI.h | 2 +- src/common/hardware_specific/samd_mcu.h | 10 +-- .../hardware_specific/samd21_mcu.cpp | 4 +- src/drivers/hardware_specific/samd21_mcu.cpp | 80 ++++++++++++------- 4 files changed, 57 insertions(+), 39 deletions(-) diff --git a/src/common/hardware_specific/samd21_AdvancedSPI.h b/src/common/hardware_specific/samd21_AdvancedSPI.h index 3e95d5e9..8acb7f96 100644 --- a/src/common/hardware_specific/samd21_AdvancedSPI.h +++ b/src/common/hardware_specific/samd21_AdvancedSPI.h @@ -22,7 +22,7 @@ class SAMDAdvancedSPI void close(); - private: + // private: SercomChannel sercomChannel; diff --git a/src/common/hardware_specific/samd_mcu.h b/src/common/hardware_specific/samd_mcu.h index e570f5f1..104ad6ee 100644 --- a/src/common/hardware_specific/samd_mcu.h +++ b/src/common/hardware_specific/samd_mcu.h @@ -164,15 +164,15 @@ extern const SamdPinDefinition g_SamdPinDefinitions[]; #ifdef SIMPLEFOC_SAMD_DEBUG template -void debugPrint(T message) -{ - SIMPLEFOC_SAMD_DEBUG_SERIAL.print(message); -} +void debugPrint(T message){ SIMPLEFOC_SAMD_DEBUG_SERIAL.print(message);} +template +void debugPrintln(T message){ SIMPLEFOC_SAMD_DEBUG_SERIAL.println(message);} static char buffer[1000]; -#define debugPrintf(args...) buffer[999] = '\0'; sprintf(buffer, args); debugPrint(buffer); +#define debugPrintf(args...) sprintf(buffer, args); debugPrint(buffer); #else #define debugPrintf(args...) ; #define debugPrint(arg) ; +#define debugPrintln(arg) ; #endif SercomSpiClockMode from_SPI_MODE(int spi_mode); diff --git a/src/current_sense/hardware_specific/samd21_mcu.cpp b/src/current_sense/hardware_specific/samd21_mcu.cpp index 0cada041..6ad63a0d 100644 --- a/src/current_sense/hardware_specific/samd21_mcu.cpp +++ b/src/current_sense/hardware_specific/samd21_mcu.cpp @@ -410,7 +410,7 @@ void SAMDCurrentSenseADCDMA::initEVSYS() // event generator (source) EVSYS_CHANNEL_Type channel_0; channel_0.bit.EDGSEL = EVSYS_CHANNEL_EDGSEL_NO_EVT_OUTPUT_Val; - channel_0.bit.PATH = EVSYS_CHANNEL_PATH_ASYNCHRONOUS_Val; + channel_0.bit.PATH = EVSYS_CHANNEL_PATH_ASYNCHRONOUS_Val; //--> EVSYS_ID_USER_ADC_SYNC: Asynchronous path only channel_0.bit.EVGEN = this->EVSYS_ID_GEN_TCC_OVF; /* TCCO Timer OVF */ channel_0.bit.SWEVT = 0b0; /* no software trigger */ channel_0.bit.CHANNEL = user_0.bit.CHANNEL - 1; /* use channel 0 */ @@ -419,7 +419,7 @@ void SAMDCurrentSenseADCDMA::initEVSYS() // event generator (source) EVSYS_CHANNEL_Type channel_1{.reg = 0}; channel_1.bit.EDGSEL = EVSYS_CHANNEL_EDGSEL_NO_EVT_OUTPUT_Val; - channel_1.bit.PATH = EVSYS_CHANNEL_PATH_ASYNCHRONOUS_Val; + channel_1.bit.PATH = EVSYS_CHANNEL_PATH_ASYNCHRONOUS_Val; //--> EVSYS_ID_USER_ADC_START: Asynchronous path only switch(channelDMA) { case 0: channel_1.bit.EVGEN = EVSYS_ID_GEN_DMAC_CH_0; break; diff --git a/src/drivers/hardware_specific/samd21_mcu.cpp b/src/drivers/hardware_specific/samd21_mcu.cpp index 6586d1a5..0c28a7d4 100644 --- a/src/drivers/hardware_specific/samd21_mcu.cpp +++ b/src/drivers/hardware_specific/samd21_mcu.cpp @@ -2,6 +2,7 @@ #include "./samd_mcu.h" +#include "../../common/hardware_specific/samd_mcu.h" #ifdef _SAMD21_ @@ -195,40 +196,57 @@ void configureTCC(tccConfiguration& tccConfig, long pwm_frequency, bool negate, while (GCLK->STATUS.bit.SYNCBUSY); // Wait for synchronization + static bool isOVFEOConfigured = false; if(enableOVFEO) { - #ifdef SIMPLEFOC_SAMD_DEBUG - SIMPLEFOC_SAMD_DEBUG_SERIAL.print("Configuring EVSYS for TCC channel "); - SIMPLEFOC_SAMD_DEBUG_SERIAL.println(tccConfig.tcc.tccn); - #endif - - - EVSYS_USER_Type user; - user.bit.CHANNEL = 0 + 1; /* use channel tccn p421: "Note that to select channel n, the value (n+1) must be written to the USER.CHANNEL bit group." */ - user.bit.USER = EVSYS_ID_USER_ADC_SYNC; /* ADC Start*/ - EVSYS->USER.reg = user.reg; - // configuration of the event system so that it triggers an "EVSYS_ID_USER_ADC_SYNC" at each timer overflow (underflow) - EVSYS_CHANNEL_Type channel; - switch (tccConfig.tcc.tccn){ - case 0: channel.bit.EVGEN = EVSYS_ID_GEN_TCC0_OVF; break; - case 1: channel.bit.EVGEN = EVSYS_ID_GEN_TCC1_OVF; break; - case 2: channel.bit.EVGEN = EVSYS_ID_GEN_TCC2_OVF; break; - // case 3: channel.bit.EVGEN = EVSYS_ID_GEN_TC3_OVF; break; // TODO: we need the TC equivalent of TCC_WAVEB_WAVEGENB_DSBOTTOM - // case 4: channel.bit.EVGEN = EVSYS_ID_GEN_TC4_OVF; break; // TODO: we need the TC equivalent of TCC_WAVEB_WAVEGENB_DSBOTTOM - // case 5: channel.bit.EVGEN = EVSYS_ID_GEN_TC5_OVF; break; // TODO: we need the TC equivalent of TCC_WAVEB_WAVEGENB_DSBOTTOM - default: - #ifdef SIMPLEFOC_SAMD_DEBUG - SIMPLEFOC_SAMD_DEBUG_SERIAL.print("Error in configureTCC() [EVSYS] Bad tccn number: "); - SIMPLEFOC_SAMD_DEBUG_SERIAL.println(tccConfig.tcc.tccn); - #endif - return; + if(isOVFEOConfigured) + { + debugPrintln("Fatal error! OVFEO already configured, you can only configure it once."); + } + else + { + + debugPrintf("Configuring EVSYS for TCC channel %d\n\r", tccConfig.tcc.tccn); + + uint8_t evgen_tcc = 0; + switch (tccConfig.tcc.tccn){ + case 0: evgen_tcc = EVSYS_ID_GEN_TCC0_OVF; break; + case 1: evgen_tcc = EVSYS_ID_GEN_TCC1_OVF; break; + case 2: evgen_tcc = EVSYS_ID_GEN_TCC2_OVF; break; + // case 3: channel.bit.EVGEN = EVSYS_ID_GEN_TC3_OVF; break; // TODO: we need the TC equivalent of TCC_WAVEB_WAVEGENB_DSBOTTOM + // case 4: channel.bit.EVGEN = EVSYS_ID_GEN_TC4_OVF; break; // TODO: we need the TC equivalent of TCC_WAVEB_WAVEGENB_DSBOTTOM + // case 5: channel.bit.EVGEN = EVSYS_ID_GEN_TC5_OVF; break; // TODO: we need the TC equivalent of TCC_WAVEB_WAVEGENB_DSBOTTOM + default: + debugPrintf("Error in configureTCC() [EVSYS] Bad tccn number: %d\n\r", tccConfig.tcc.tccn); + return; + } + + EVSYS_USER_Type userADC; + userADC.bit.CHANNEL = tccConfig.tcc.tccn + 1; /* use channel tccn p421: "Note that to select channel n, the value (n+1) must be written to the USER.CHANNEL bit group." */ + userADC.bit.USER = EVSYS_ID_USER_ADC_SYNC; /* ADC Start*/ + EVSYS->USER.reg = userADC.reg; + // configuration of the event system so that it triggers an "EVSYS_ID_USER_ADC_SYNC" at each timer overflow (underflow) + EVSYS_CHANNEL_Type channelTCC_ADC; + channelTCC_ADC.bit.EVGEN = evgen_tcc; + channelTCC_ADC.bit.EDGSEL = EVSYS_CHANNEL_EDGSEL_NO_EVT_OUTPUT_Val; + channelTCC_ADC.bit.PATH = EVSYS_CHANNEL_PATH_ASYNCHRONOUS_Val; //EVSYS_ID_USER_ADC_SYNC is asynchronous only + channelTCC_ADC.bit.SWEVT = 0; /* no software trigger */ + channelTCC_ADC.bit.CHANNEL = userADC.bit.CHANNEL - 1; + EVSYS->CHANNEL.reg = channelTCC_ADC.reg; + + EVSYS_USER_Type userDMA3; + userDMA3.bit.CHANNEL = tccConfig.tcc.tccn + 6 + 1; /* use channel tccn p421: "Note that to select channel n, the value (n+1) must be written to the USER.CHANNEL bit group." */ + userDMA3.bit.USER = EVSYS_ID_USER_DMAC_CH_3; /* DMA_Start*/ + EVSYS->USER.reg = userDMA3.reg; + + EVSYS_CHANNEL_Type channelTCC_DMA3; + channelTCC_DMA3.bit.EVGEN = evgen_tcc; + channelTCC_DMA3.bit.EDGSEL = EVSYS_CHANNEL_EDGSEL_NO_EVT_OUTPUT_Val; + channelTCC_DMA3.bit.PATH = EVSYS_CHANNEL_PATH_RESYNCHRONIZED_Val; //DMA and TCC use different clocks (I think?) + channelTCC_DMA3.bit.SWEVT = 0; /* no software trigger */ + channelTCC_DMA3.bit.CHANNEL = userDMA3.bit.CHANNEL - 1; + EVSYS->CHANNEL.reg = channelTCC_DMA3.reg; } - - channel.bit.EDGSEL = EVSYS_CHANNEL_EDGSEL_NO_EVT_OUTPUT_Val; - channel.bit.PATH = EVSYS_CHANNEL_PATH_ASYNCHRONOUS_Val; //EVSYS_ID_USER_ADC_SYNC is asynchronous only - channel.bit.SWEVT = 0; /* no software trigger */ - channel.bit.CHANNEL = user.bit.CHANNEL - 1; /* use channel tccn */ - EVSYS->CHANNEL.reg = channel.reg; } tccConfigured[tccConfig.tcc.tccn] = true; From 0ca9f296ef4b2dea3c021d6a6de33c4a63903c9b Mon Sep 17 00:00:00 2001 From: maxime Date: Fri, 4 Jun 2021 09:47:55 -0600 Subject: [PATCH 09/15] begining to move things around in order to globalize evsys and dmac api --- src/common/hardware_specific/samd21.cpp | 186 +++++++++++++++ src/common/hardware_specific/samd_mcu.cpp | 24 +- src/common/hardware_specific/samd_mcu.h | 15 +- .../hardware_specific/samd21_mcu.cpp | 221 ++++++------------ .../hardware_specific/samd21_mcu.h | 16 +- src/drivers/hardware_specific/samd21_mcu.cpp | 60 ----- 6 files changed, 296 insertions(+), 226 deletions(-) diff --git a/src/common/hardware_specific/samd21.cpp b/src/common/hardware_specific/samd21.cpp index 8488c856..5ad408bd 100644 --- a/src/common/hardware_specific/samd21.cpp +++ b/src/common/hardware_specific/samd21.cpp @@ -123,4 +123,190 @@ SercomConfig getSercom(SercomChannel channel) } } + +bool g_EVSYSChannelInitialized[] = {false, false, false, false, false, false, false, false, false, false, false, false, false, false, false, false}; + +int initEVSYS(uint8_t evsysChannel, uint16_t EVSYS_ID_USER, uint16_t EVSYS_ID_GEN, uint16_t EVSYS_CHANNEL_PATH, uint16_t EVSYS_CHANNEL_EDGSEL, bool force) +{ + /* Turn on the digital interface clock */ + + if(g_EVSYSChannelInitialized[evsysChannel] && !force) + { + debugPrintf("initEVSYS() channel %d is already initialized", evsysChannel); + return -1; + } + + static bool PM_APBCMASK_EVSYS_initialized = false; + if(!PM_APBCMASK_EVSYS_initialized) + { + PM->APBCMASK.bit.EVSYS_ = 0b1; + PM_APBCMASK_EVSYS_initialized = true; + } + + if(evsysChannel > EVSYS_GCLK_ID_SIZE - 1) + { + debugPrintf("initEVSYS() channel %d out of bounds (max %d)", evsysChannel, EVSYS_GCLK_ID_SIZE); + return -1; + } + if(EVSYS_ID_USER > EVSYS_ID_USER_PTC_STCONV) + { + debugPrintf("initEVSYS() EVSYS_ID_USER %d out of bounds (max %d)", EVSYS_ID_USER, EVSYS_ID_USER_PTC_STCONV); + return -1; + } + if(EVSYS_ID_GEN > EVSYS_ID_GEN_PTC_WCOMP) + { + debugPrintf("initEVSYS() EVSYS_ID_GEN %d out of bounds (max %d)", EVSYS_ID_GEN, EVSYS_ID_GEN_PTC_WCOMP); + return -1; + } + + if(EVSYS_CHANNEL_PATH == EVSYS_CHANNEL_PATH_ASYNCHRONOUS_Val && EVSYS_CHANNEL_EDGSEL != EVSYS_CHANNEL_EDGSEL_NO_EVT_OUTPUT_Val) + { + EVSYS_CHANNEL_EDGSEL = EVSYS_CHANNEL_EDGSEL_NO_EVT_OUTPUT_Val; + debugPrintf("initEVSYS() warning, channel %d is configured with asynchronous path, forcing EDGSEL to 'NO_EVT_OUTPUT', note that interrupts aren't supported with that path", evsysChannel); + } + + g_EVSYSChannelInitialized[evsysChannel] = true; + + /* Turn on the peripheral interface clock and select GCLK */ + GCLK_CLKCTRL_Type clkctrl; + clkctrl.bit.WRTLOCK = 0; + clkctrl.bit.CLKEN = 1; + clkctrl.bit.ID = EVSYS_GCLK_ID_LSB + evsysChannel; //enable clock for channel 0 + clkctrl.bit.GEN = GCLK_CLKCTRL_GEN_GCLK0_Val; /* GCLK_GENERATOR_0 */ + GCLK->CLKCTRL.reg = clkctrl.reg; + + // event user (destination) + EVSYS_USER_Type user; + user.bit.CHANNEL = evsysChannel + 1; /* use channel 0 p421: "Note that to select channel n, the value (n+1) must be written to the USER.CHANNEL bit group." */ + user.bit.USER = EVSYS_ID_USER; + EVSYS->USER.reg = user.reg; + + + // event generator (source) + EVSYS_CHANNEL_Type channel; + channel.bit.EDGSEL = EVSYS_CHANNEL_EDGSEL; + channel.bit.PATH = EVSYS_CHANNEL_PATH; //--> EVSYS_ID_USER_ADC_SYNC: Asynchronous path only + channel.bit.EVGEN = EVSYS_ID_GEN; + channel.bit.SWEVT = 0b0; /* no software trigger */ + channel.bit.CHANNEL = evsysChannel; + EVSYS->CHANNEL.reg = channel.reg; + + + return 0; + + +} + +static DmacDescriptor descriptor_section[12] __attribute__ ((aligned (16))); +static volatile DmacDescriptor write_back[12] __attribute__ ((aligned (16))); +void initDMAC() +{ + // probably on by default + PM->AHBMASK.reg |= PM_AHBMASK_DMAC ; + PM->APBBMASK.reg |= PM_APBBMASK_DMAC ; + + DMAC->BASEADDR.reg = (uint32_t)descriptor_section; // Descriptor Base Memory Address + DMAC->WRBADDR.reg = (uint32_t)write_back; //Write-Back Memory Base Address + + + DMAC_PRICTRL0_Type prictrl0{.reg = 0}; + + prictrl0.bit.RRLVLEN0 = 0b1; //enable round-robin + prictrl0.bit.RRLVLEN1 = 0b1; //enable round-robin + prictrl0.bit.RRLVLEN2 = 0b1; //enable round-robin + prictrl0.bit.RRLVLEN3 = 0b1; //enable round-robin + + DMAC->PRICTRL0.reg = prictrl0.reg; + + DMAC_CTRL_Type ctrl{.reg = 0}; + ctrl.bit.DMAENABLE = 0b1; + ctrl.bit.LVLEN0 = 0b1; + ctrl.bit.LVLEN1 = 0b1; + ctrl.bit.LVLEN2 = 0b1; + ctrl.bit.LVLEN3 = 0b1; + ctrl.bit.CRCENABLE = 0b0; + + DMAC->CTRL.reg = ctrl.reg; + + NVIC_EnableIRQ( DMAC_IRQn ) ; +} + +bool g_DMACChannelInitialized[] = {false, false, false, false, false, false, false, false, false, false, false, false, false, false, false, false}; + +static void (*DMAC_Handlers[12])(volatile DMAC_CHINTFLAG_Type & flags, volatile DMAC_CHCTRLA_Type & chctrla); + +int initDMAChannel(uint8_t channel, DMAC_CHINTENSET_Type chintset, DMAC_CHCTRLB_Type chctrlb, const DmacDescriptor & descriptor, void (*interrupt_handler)(volatile DMAC_CHINTFLAG_Type &, volatile DMAC_CHCTRLA_Type &), bool force) +{ + + if(channel > 11) + { + debugPrintf("initDMAChannel() channel %d out of bounds (max %d)", channel, 12); + return -1; + } + + if(g_DMACChannelInitialized[channel] && !force) + { + debugPrintf("initDMAChannel() channel %d is already initialized", channel); + return -1; + } + + g_DMACChannelInitialized[channel] = true; + //select the channel + DMAC->CHID.bit.ID = channel; + + // disable and reset the channel + DMAC->CHCTRLA.bit.ENABLE = 0b0; //must be done **before** SWRST + DMAC->CHCTRLA.bit.SWRST = 0b1; + + DMAC->CHINTENSET.reg = chintset.reg ; // enable all 3 interrupts + DMAC->CHCTRLB.reg = chctrlb.reg; + + memcpy(&descriptor_section[channel], &descriptor, sizeof(DmacDescriptor)); + + DMAC_Handlers[channel] = interrupt_handler; + + // start channel + DMAC->CHCTRLA.bit.ENABLE = 0b1; + + + return 0; +} + + +void trigDMACChannel(uint8_t channel) +{ + + switch (channel) + { + case 0: DMAC->SWTRIGCTRL.bit.SWTRIG0 = 0b1; break; + case 1: DMAC->SWTRIGCTRL.bit.SWTRIG1 = 0b1; break; + case 2: DMAC->SWTRIGCTRL.bit.SWTRIG2 = 0b1; break; + case 3: DMAC->SWTRIGCTRL.bit.SWTRIG3 = 0b1; break; + case 4: DMAC->SWTRIGCTRL.bit.SWTRIG4 = 0b1; break; + case 5: DMAC->SWTRIGCTRL.bit.SWTRIG5 = 0b1; break; + case 6: DMAC->SWTRIGCTRL.bit.SWTRIG6 = 0b1; break; + case 7: DMAC->SWTRIGCTRL.bit.SWTRIG7 = 0b1; break; + case 8: DMAC->SWTRIGCTRL.bit.SWTRIG8 = 0b1; break; + case 9: DMAC->SWTRIGCTRL.bit.SWTRIG9 = 0b1; break; + case 10: DMAC->SWTRIGCTRL.bit.SWTRIG10 = 0b1; break; + case 11: DMAC->SWTRIGCTRL.bit.SWTRIG11 = 0b1; break; + + default: + debugPrintf("Error in adcToDMATransfer() [DMAC] Bad channel number %d"); + } +} + + +void DMAC_Handler() __attribute__((weak)); +void DMAC_Handler() { + + DMAC->CHID.bit.ID = DMAC->INTPEND.bit.ID; + + void (*interrupt_handler)(volatile DMAC_CHINTFLAG_Type &, volatile DMAC_CHCTRLA_Type &) = DMAC_Handlers[DMAC->CHID.bit.ID]; + + if(interrupt_handler != nullptr) + interrupt_handler(DMAC->CHINTFLAG, DMAC->CHCTRLA); + + +} #endif //_SAMD21_ diff --git a/src/common/hardware_specific/samd_mcu.cpp b/src/common/hardware_specific/samd_mcu.cpp index 35c378e3..736dc8a7 100644 --- a/src/common/hardware_specific/samd_mcu.cpp +++ b/src/common/hardware_specific/samd_mcu.cpp @@ -19,6 +19,25 @@ SercomSpiClockMode from_SPI_MODE(int spi_mode) } } +uint32_t computeDSTADDR(uint8_t * startAddress, uint32_t STEPSEL, uint32_t STEPSIZE, uint32_t BEATSIZE, uint32_t BTCNT) +{ + /* + p.283 When destination address incrementation is configured (BTCTRL.DSTINC is one), SRCADDR must be set to the + destination address of the last beat transfer in the block transfer. The destination address should be calculated as + follows: + DSTADDR = DSTADDRSTART + BTCNT ⋅ ( BEATSIZE + 1 ) ⋅ 2 STEPSIZE , where BTCTRL.STEPSEL is zero + DSTADDR = DSTADDRSTART + BTCNT ⋅ ( BEATSIZE + 1 ) , where BTCTRL.STEPSEL is one + - DSTADDRSTART is the destination address of the first beat transfer in the block transfer + - BTCNT is the initial number of beats remaining in the block transfer + - BEATSIZE is the configured number of bytes in a beat + - STEPSIZE is the configured number of beats for each incrementation + */ + uint32_t factor = STEPSEL == 0 ? (1 << STEPSIZE) /*2^STEPSIZE*/: 1; + + return (uint32_t)(startAddress + BTCNT * (BEATSIZE + 1) * factor); // end address +} + + const SamdPinDefinition * getSamdPinDefinition(int arduinoPin) { if(arduinoPin < 0) @@ -26,7 +45,7 @@ const SamdPinDefinition * getSamdPinDefinition(int arduinoPin) debugPrint(F("getSamdPinDefinition() : pin < 0")); return nullptr; } - if(arduinoPin > PINS_COUNT-1) + if((uint32_t)arduinoPin > (PINS_COUNT-1)) { debugPrintf("getSamdPinDefinition() : arduino pin %d above %d", arduinoPin, PINS_COUNT-1); return nullptr; @@ -60,4 +79,5 @@ const SamdPinDefinition * getSamdPinDefinition(int arduinoPin) debugPrintf("getSamdPinDefinition(): Arduino pin: %d, PORT%c%d, SERCOM%d:PAD[%d], SERCOM_ALT%d:PAD[%d] \n\r", arduinoPin, 'A' + port, port_pin, rv->sercom, rv->sercom_pad, rv->sercom_alt, rv->sercom_pad_alt); return rv; -} \ No newline at end of file +} + diff --git a/src/common/hardware_specific/samd_mcu.h b/src/common/hardware_specific/samd_mcu.h index 104ad6ee..303e85a0 100644 --- a/src/common/hardware_specific/samd_mcu.h +++ b/src/common/hardware_specific/samd_mcu.h @@ -159,7 +159,9 @@ typedef struct _SamdPinDefinition extern const int8_t g_SamdMapPortPin[2][32]; -extern const SamdPinDefinition g_SamdPinDefinitions[]; +extern const SamdPinDefinition g_SamdPinDefinitions[]; // *Warning! these pins indexed by the order of "Table 6-1. PORT Function Multiplexing" p21 of samd21 spec sheet + +const SamdPinDefinition * getSamdPinDefinition(int arduinoPin); #ifdef SIMPLEFOC_SAMD_DEBUG @@ -177,4 +179,13 @@ static char buffer[1000]; SercomSpiClockMode from_SPI_MODE(int spi_mode); -const SamdPinDefinition * getSamdPinDefinition(int arduinoPin); + +extern bool g_EVSYSChannelInitialized[]; +int initEVSYS(uint8_t evsysChannel, uint16_t EVSYS_ID_USER, uint16_t EVSYS_ID_GEN, uint16_t EVSYS_CHANNEL_PATH, uint16_t EVSYS_CHANNEL_EDGSEL, bool force = false); + + +extern bool g_DMACChannelInitialized[]; +void initDMAC(); +int initDMAChannel(uint8_t channel, DMAC_CHINTENSET_Type chintset, DMAC_CHCTRLB_Type chctrlb, const DmacDescriptor & descriptor, void (*interrupt_handler)(volatile DMAC_CHINTFLAG_Type &, volatile DMAC_CHCTRLA_Type &), bool force = false); +void trigDMACChannel(uint8_t channel); +uint32_t computeDSTADDR(uint8_t * startAddress, uint32_t STEPSEL, uint32_t STEPSIZE, uint32_t BEATSIZE, uint32_t BTCNT); \ No newline at end of file diff --git a/src/current_sense/hardware_specific/samd21_mcu.cpp b/src/current_sense/hardware_specific/samd21_mcu.cpp index 6ad63a0d..82d31355 100644 --- a/src/current_sense/hardware_specific/samd21_mcu.cpp +++ b/src/current_sense/hardware_specific/samd21_mcu.cpp @@ -1,8 +1,8 @@ #include "samd21_mcu.h" #include "../hardware_api.h" - - +#include "../../common/hardware_specific/samd_mcu.h" +#include static int _pinA, _pinB, _pinC; static uint16_t a_raw = 0xFFFF, b_raw = 0xFFFF, c_raw = 0xFFFF; // updated by adcStopWithDMA when configured in freerunning mode static SAMDCurrentSenseADCDMA instance; @@ -104,12 +104,12 @@ int EVSYS_ID_GEN_TCC_OVF, int pinAREF, float voltageAREF initPins(); if(this->EVSYS_ID_GEN_TCC_OVF != -1) { - #ifdef SIMPLEFOC_SAMD_DEBUG - SIMPLEFOC_SAMD_DEBUG_SERIAL.print("Configuring EVSYS for ADC with EVSYS_ID_GEN_TCC_OVF"); - SIMPLEFOC_SAMD_DEBUG_SERIAL.println(this->EVSYS_ID_GEN_TCC_OVF); - #endif + debugPrintf("Configuring EVSYS for ADC with EVSYS_ID_GEN #%d", this->EVSYS_ID_GEN_TCC_OVF); + + if(initEVSYS() != 0) + return; - initEVSYS(); + debugPrintln("a"); initDMA(); initDMAChannel(); } @@ -118,19 +118,13 @@ int EVSYS_ID_GEN_TCC_OVF, int pinAREF, float voltageAREF } -bool SAMDCurrentSenseADCDMA::readResults(uint16_t & a, uint16_t & b, uint16_t & c){ - // if(ADC->CTRLA.bit.ENABLE) - // return false; - uint32_t ainA = g_APinDescription[pinA].ulADCChannelNumber; - uint32_t ainB = g_APinDescription[pinB].ulADCChannelNumber; +uint32_t SAMDCurrentSenseADCDMA::readResults(uint16_t & a, uint16_t & b, uint16_t & c){ + a = adcBuffer[ainA]; b = adcBuffer[ainB]; if(_isset(pinC)) - { - uint32_t ainC = g_APinDescription[pinC].ulADCChannelNumber; c = adcBuffer[ainC]; - } - return true; + return 0; // TODO: return timestamp } @@ -140,23 +134,33 @@ float SAMDCurrentSenseADCDMA::toVolts(uint16_t counts) { void SAMDCurrentSenseADCDMA::initPins(){ + refsel = ADC_REFCTRL_REFSEL_INTVCC1_Val; if(pinAREF != -1) { - SIMPLEFOC_SAMD_DEBUG_SERIAL.print("wtf"); - if(g_APinDescription[pinAREF].ulPort == EPortType::PORTA && g_APinDescription[pinAREF].ulPin == 3) - pinMode(pinAREF, INPUT); + debugPrintf("Using AREF pin %d", pinAREF); + switch(getSamdPinDefinition(pinAREF)->vref) + { + case VRef::VREFA: + refsel = ADC_REFCTRL_REFSEL_AREFA_Val; + break; + case VRef::VREFB: + refsel = ADC_REFCTRL_REFSEL_AREFB_Val; + break; + default: + debugPrintf("Error: pin %d is not a valid AREF pin, falling back to 'INTVCC1'", pinAREF); + } } pinMode(pinA, INPUT); pinMode(pinB, INPUT); - uint32_t ainA = g_APinDescription[pinA].ulADCChannelNumber; - uint32_t ainB = g_APinDescription[pinB].ulADCChannelNumber; + ainA = getSamdPinDefinition(pinA)->adc_channel; + ainB = getSamdPinDefinition(pinB)->adc_channel; firstAIN = min(ainA, ainB); lastAIN = max(ainA, ainB); if( _isset(pinC) ) { - uint32_t ainC = g_APinDescription[pinC].ulADCChannelNumber; + ainC = getSamdPinDefinition(pinC)->adc_channel; pinMode(pinC, INPUT); firstAIN = min(firstAIN, ainC); lastAIN = max(lastAIN, ainC); @@ -169,9 +173,9 @@ void SAMDCurrentSenseADCDMA::initPins(){ void SAMDCurrentSenseADCDMA::initADC(){ - analogRead(pinA); // do some pin init pinPeripheral() - analogRead(pinB); // do some pin init pinPeripheral() - analogRead(pinC); // do some pin init pinPeripheral() + pinPeripheral(pinA, PIO_ANALOG); + pinPeripheral(pinB, PIO_ANALOG); + pinPeripheral(pinC, PIO_ANALOG); ADC->CTRLA.bit.ENABLE = 0x00; // Disable ADC @@ -202,7 +206,7 @@ void SAMDCurrentSenseADCDMA::initADC(){ /* Configure reference */ ADC_REFCTRL_Type refctrl{.reg = 0}; // refctrl.bit.REFCOMP = 1; /* enable reference compensation */ - refctrl.bit.REFSEL = this->pinAREF != -1 ? ADC_REFCTRL_REFSEL_AREFA_Val : ADC_REFCTRL_REFSEL_INTVCC1_Val; + refctrl.bit.REFSEL = refsel; ADC->REFCTRL.reg = refctrl.reg; /* @@ -229,7 +233,7 @@ void SAMDCurrentSenseADCDMA::initADC(){ 0x1D-0x1F Reserved */ ADC_INPUTCTRL_Type inputctrl{.reg = 0}; - inputctrl.bit.GAIN = refctrl.bit.REFSEL == ADC_REFCTRL_REFSEL_AREFA_Val ? ADC_INPUTCTRL_GAIN_1X_Val : ADC_INPUTCTRL_GAIN_DIV2_Val; + inputctrl.bit.GAIN = refctrl.bit.REFSEL == ADC_REFCTRL_REFSEL_INTVCC1_Val ? ADC_INPUTCTRL_GAIN_DIV2_Val : ADC_INPUTCTRL_GAIN_1X_Val; inputctrl.bit.MUXPOS = firstAIN; inputctrl.bit.MUXNEG = ADC_INPUTCTRL_MUXNEG_GND_Val; inputctrl.bit.INPUTSCAN = lastAIN - inputctrl.bit.MUXPOS + 1; // so the adc will scan from oneBeforeFirstAIN to lastAIN (inclusive) @@ -243,13 +247,13 @@ void SAMDCurrentSenseADCDMA::initADC(){ avgctrl.bit.SAMPLENUM = ADC_AVGCTRL_SAMPLENUM_1_Val, ADC->AVGCTRL.reg = avgctrl.reg; /* Configure sample length */ - ADC->SAMPCTRL.reg = ADC_SAMPCTRL_SAMPLEN(5); //sample length in 1/2 CLK_ADC cycles, see GCLK_ADC and ADC_CTRLB_PRESCALER_DIV16 + ADC->SAMPCTRL.reg = ADC_SAMPCTRL_SAMPLEN(5); //sample length in 1/2 CLK_ADC cycles, see GCLK_ADC and ADC_CTRLB_PRESCALER_DIV4_Val // according to the specsheet: fGCLK_ADC ADC input clock frequency 48 MHz, so same as fCPU ADCsync(); ADC_CTRLB_Type ctrlb{.reg = 0}; /* ADC clock is 8MHz / 4 = 2MHz */ - ctrlb.bit.PRESCALER = ADC_CTRLB_PRESCALER_DIV16_Val; + ctrlb.bit.PRESCALER = ADC_CTRLB_PRESCALER_DIV4_Val; ctrlb.bit.RESSEL = ADC_CTRLB_RESSEL_12BIT_Val; ctrlb.bit.CORREN = 0; ctrlb.bit.FREERUN = 0; @@ -271,55 +275,31 @@ void SAMDCurrentSenseADCDMA::initADC(){ // NVIC_EnableIRQ( ADC_IRQn ); ADC->CTRLA.bit.ENABLE = 0x01; - + ADCsync(); } -DmacDescriptor descriptor_section[12] __attribute__ ((aligned (16))); -volatile DmacDescriptor write_back[12] __attribute__ ((aligned (16))); +// DmacDescriptor descriptor_section[12] __attribute__ ((aligned (16))); +// volatile DmacDescriptor write_back[12] __attribute__ ((aligned (16))); void SAMDCurrentSenseADCDMA::initDMA() { - // probably on by default - PM->AHBMASK.reg |= PM_AHBMASK_DMAC ; - PM->APBBMASK.reg |= PM_APBBMASK_DMAC ; - - DMAC->BASEADDR.reg = (uint32_t)descriptor_section; // Descriptor Base Memory Address - DMAC->WRBADDR.reg = (uint32_t)write_back; //Write-Back Memory Base Address - - - DMAC_PRICTRL0_Type prictrl0{.reg = 0}; - - prictrl0.bit.RRLVLEN0 = 0b1; //enable round-robin - prictrl0.bit.RRLVLEN1 = 0b1; //enable round-robin - prictrl0.bit.RRLVLEN2 = 0b1; //enable round-robin - prictrl0.bit.RRLVLEN3 = 0b1; //enable round-robin - - DMAC->PRICTRL0.reg = prictrl0.reg; - - DMAC_CTRL_Type ctrl{.reg = 0}; - ctrl.bit.DMAENABLE = 0b1; - ctrl.bit.LVLEN0 = 0b1; - ctrl.bit.LVLEN1 = 0b1; - ctrl.bit.LVLEN2 = 0b1; - ctrl.bit.LVLEN3 = 0b1; - ctrl.bit.CRCENABLE = 0b0; - - DMAC->CTRL.reg = ctrl.reg; - - NVIC_EnableIRQ( DMAC_IRQn ) ; + ::initDMAC(); } DmacDescriptor descriptors[20] __attribute__ ((aligned (16))); -void SAMDCurrentSenseADCDMA::initDMAChannel() { +void dmac_adc_Handler(volatile DMAC_CHINTFLAG_Type & flags, volatile DMAC_CHCTRLA_Type & chctrla) +{ + chctrla.bit.ENABLE = 0b1; + flags.bit.TCMPL = 0b1; + ADC->INPUTCTRL.bit.INPUTOFFSET = 0; + instance.dma_i++; +} - //select the channel - DMAC->CHID.bit.ID = channelDMA; +void SAMDCurrentSenseADCDMA::initDMAChannel() { - // disable and reset the channel - DMAC->CHCTRLA.bit.ENABLE = 0b0; //must be done **before** SWRST - DMAC->CHCTRLA.bit.SWRST = 0b1; + DMAC_CHINTENSET_Type chinset{.reg = 0}; - DMAC->CHINTENSET.bit.TCMPL = DMAC_CHINTENSET_MASK ; // enable all 3 interrupts + chinset.bit.TCMPL = 0b1; // configure the channel DMAC_CHCTRLB_Type chctrlb{.reg = 0}; @@ -351,97 +331,36 @@ void SAMDCurrentSenseADCDMA::initDMAChannel() { btctrl.bit.DSTINC = 0b1; /*!< bit: 11 Destination Address Increment Enable */ btctrl.bit.STEPSEL = DMAC_BTCTRL_STEPSEL_DST_Val; /*!< bit: 12 Step Selection */ btctrl.bit.STEPSIZE = DMAC_BTCTRL_STEPSIZE_X1_Val; /*!< bit: 13..15 Address Increment Step Size */ - /* - p.283 When destination address incrementation is configured (BTCTRL.DSTINC is one), SRCADDR must be set to the - destination address of the last beat transfer in the block transfer. The destination address should be calculated as - follows: - DSTADDR = DSTADDRSTART + BTCNT ⋅ ( BEATSIZE + 1 ) ⋅ 2 STEPSIZE , where BTCTRL.STEPSEL is zero - DSTADDR = DSTADDRSTART + BTCNT ⋅ ( BEATSIZE + 1 ) , where BTCTRL.STEPSEL is one - - DSTADDRSTART is the destination address of the first beat transfer in the block transfer - - BTCNT is the initial number of beats remaining in the block transfer - - BEATSIZE is the configured number of bytes in a beat - - STEPSIZE is the configured number of beats for each incrementation - */ - uint32_t factor = btctrl.bit.STEPSEL == 0 ? (1 << btctrl.bit.STEPSIZE) /*2^STEPSIZE*/: 1; - - descriptors[i].DSTADDR.reg = (uint32_t)(((uint8_t*)&adcBuffer[i]) + descriptors[i].BTCNT.reg * (btctrl.bit.BEATSIZE + 1) * factor); // end address - + descriptors[i].DSTADDR.reg = computeDSTADDR((uint8_t*)&adcBuffer[i], btctrl.bit.STEPSEL, btctrl.bit.STEPSIZE, btctrl.bit.BEATSIZE, descriptors[i].BTCNT.reg); } + + + - memcpy(&descriptor_section[channelDMA], &descriptors[firstAIN], sizeof(DmacDescriptor)); + ::initDMAChannel(channelDMA, chinset, chctrlb, descriptors[firstAIN], dmac_adc_Handler); - // start channel - DMAC->CHCTRLA.bit.ENABLE = 0b1; } -void SAMDCurrentSenseADCDMA::initEVSYS() +int SAMDCurrentSenseADCDMA::initEVSYS() { - /* Turn on the digital interface clock */ - PM->APBCMASK.reg |= PM_APBCMASK_EVSYS; - - /* Turn on the peripheral interface clock and select GCLK */ - GCLK_CLKCTRL_Type clkctrl0; - clkctrl0.bit.WRTLOCK = 0; - clkctrl0.bit.CLKEN = 1; - clkctrl0.bit.ID = EVSYS_GCLK_ID_0; //enable clock for channel 0 - clkctrl0.bit.GEN = GCLK_CLKCTRL_GEN_GCLK0_Val; /* GCLK_GENERATOR_0 */ - GCLK->CLKCTRL.reg = clkctrl0.reg; - - GCLK_CLKCTRL_Type clkctrl1; - clkctrl1.bit.WRTLOCK = 0; - clkctrl1.bit.CLKEN = 1; - clkctrl1.bit.ID = EVSYS_GCLK_ID_1; //enable clock for channel 1 - clkctrl1.bit.GEN = GCLK_CLKCTRL_GEN_GCLK0_Val; /* GCLK_GENERATOR_0 */ - GCLK->CLKCTRL.reg = clkctrl1.reg; - - // event user (destination) - EVSYS_USER_Type user_0; - user_0.bit.CHANNEL = 0 + 1; /* use channel 0 p421: "Note that to select channel n, the value (n+1) must be written to the USER.CHANNEL bit group." */ - user_0.bit.USER = EVSYS_ID_USER_ADC_SYNC; /* ADC Sync*/ - EVSYS->USER.reg = user_0.reg; - - // event user (destination) - EVSYS_USER_Type user_1; - user_1.bit.CHANNEL = 1 + 1; /* p421: "Note that to select channel n, the value (n+1) must be written to the USER.CHANNEL bit group." */ - // user_1.bit.USER = EVSYS_ID_USER_DMAC_CH_0; /* ADC Start*/ - user_1.bit.USER = EVSYS_ID_USER_ADC_START; /* ADC Start*/ - EVSYS->USER.reg = user_1.reg; - - // event generator (source) - EVSYS_CHANNEL_Type channel_0; - channel_0.bit.EDGSEL = EVSYS_CHANNEL_EDGSEL_NO_EVT_OUTPUT_Val; - channel_0.bit.PATH = EVSYS_CHANNEL_PATH_ASYNCHRONOUS_Val; //--> EVSYS_ID_USER_ADC_SYNC: Asynchronous path only - channel_0.bit.EVGEN = this->EVSYS_ID_GEN_TCC_OVF; /* TCCO Timer OVF */ - channel_0.bit.SWEVT = 0b0; /* no software trigger */ - channel_0.bit.CHANNEL = user_0.bit.CHANNEL - 1; /* use channel 0 */ - EVSYS->CHANNEL.reg = channel_0.reg; - - // event generator (source) - EVSYS_CHANNEL_Type channel_1{.reg = 0}; - channel_1.bit.EDGSEL = EVSYS_CHANNEL_EDGSEL_NO_EVT_OUTPUT_Val; - channel_1.bit.PATH = EVSYS_CHANNEL_PATH_ASYNCHRONOUS_Val; //--> EVSYS_ID_USER_ADC_START: Asynchronous path only + if(::initEVSYS(0, EVSYS_ID_USER_ADC_SYNC, this->EVSYS_ID_GEN_TCC_OVF, EVSYS_CHANNEL_PATH_ASYNCHRONOUS_Val, EVSYS_CHANNEL_EDGSEL_NO_EVT_OUTPUT_Val) != 0) + return -1; + + uint16_t EVGEN_DMAC = 0; switch(channelDMA) { - case 0: channel_1.bit.EVGEN = EVSYS_ID_GEN_DMAC_CH_0; break; - case 1: channel_1.bit.EVGEN = EVSYS_ID_GEN_DMAC_CH_1; break; - case 2: channel_1.bit.EVGEN = EVSYS_ID_GEN_DMAC_CH_2; break; - case 3: channel_1.bit.EVGEN = EVSYS_ID_GEN_DMAC_CH_3; break; + case 0: EVGEN_DMAC = EVSYS_ID_GEN_DMAC_CH_0; break; + case 1: EVGEN_DMAC = EVSYS_ID_GEN_DMAC_CH_1; break; + case 2: EVGEN_DMAC = EVSYS_ID_GEN_DMAC_CH_2; break; + case 3: EVGEN_DMAC = EVSYS_ID_GEN_DMAC_CH_3; break; default: - #ifdef SIMPLEFOC_SAMD_DEBUG - SIMPLEFOC_SAMD_DEBUG_SERIAL.print("Bad dma channel, only 0,1,2 or 3 are supported : "); - SIMPLEFOC_SAMD_DEBUG_SERIAL.println(channelDMA); - #endif + debugPrintf("initEVSYS(): Bad dma channel %d. Only 0,1,2 or 3 are supported.", channelDMA); break; } - - channel_1.bit.SWEVT = 0b0; - channel_1.bit.CHANNEL = user_1.bit.CHANNEL - 1; - EVSYS->CHANNEL.reg = channel_1.reg; - + if(::initEVSYS(1, EVSYS_ID_USER_ADC_START, EVGEN_DMAC, EVSYS_CHANNEL_PATH_ASYNCHRONOUS_Val, EVSYS_CHANNEL_EDGSEL_NO_EVT_OUTPUT_Val) != 0) + return -1; - // EVSYS does not emit interrups on asynchronous path - // EVSYS->INTENSET.reg = EVSYS_INTENSET_MASK; - // NVIC_EnableIRQ( EVSYS_IRQn ) ; + return 0; } void ADC_Handler() __attribute__((weak)); @@ -458,13 +377,3 @@ void ADC_Handler() ADC->INTFLAG.bit.SYNCRDY = 0b1; } -void DMAC_Handler() __attribute__((weak)); -void DMAC_Handler() { - - instance.dma_i++; - DMAC->CHID.bit.ID = DMAC->INTPEND.bit.ID; - DMAC->CHINTFLAG.bit.TCMPL = 0b1; // clear - DMAC->CHCTRLA.bit.ENABLE = 0b1; - ADC->INPUTCTRL.bit.INPUTOFFSET = 0; - -} \ No newline at end of file diff --git a/src/current_sense/hardware_specific/samd21_mcu.h b/src/current_sense/hardware_specific/samd21_mcu.h index a124aefc..09f23845 100644 --- a/src/current_sense/hardware_specific/samd21_mcu.h +++ b/src/current_sense/hardware_specific/samd21_mcu.h @@ -17,7 +17,7 @@ class SAMDCurrentSenseADCDMA static SAMDCurrentSenseADCDMA * getHardwareAPIInstance(); SAMDCurrentSenseADCDMA(); void init(int pinA, int pinB, int pinC, int EVSYS_ID_GEN_TCC_OVF = -1, int pinAREF = -1, float voltageAREF = 3.3, uint32_t adcBits = 12, uint32_t channelDMA = 3); - bool readResults(uint16_t & a, uint16_t & b, uint16_t & c); + uint32_t readResults(uint16_t & a, uint16_t & b, uint16_t & c); float toVolts(uint16_t counts); uint16_t adcBuffer[20]; int adc_i = 0; @@ -30,19 +30,23 @@ class SAMDCurrentSenseADCDMA void initADC(); void initDMA(); void initDMAChannel(); - void initEVSYS(); + int initEVSYS(); uint32_t oneBeforeFirstAIN; // hack to discard first noisy readout uint32_t firstAIN; uint32_t lastAIN; + uint32_t ainA; + uint32_t ainB; + uint32_t ainC; + uint32_t refsel; uint32_t bufferSize = 0; - uint32_t pinA; - uint32_t pinB; - uint32_t pinC; - uint32_t pinAREF; + int pinA; + int pinB; + int pinC; + int pinAREF; uint32_t channelDMA; // DMA channel bool freeRunning; diff --git a/src/drivers/hardware_specific/samd21_mcu.cpp b/src/drivers/hardware_specific/samd21_mcu.cpp index 0c28a7d4..9bbb7a25 100644 --- a/src/drivers/hardware_specific/samd21_mcu.cpp +++ b/src/drivers/hardware_specific/samd21_mcu.cpp @@ -195,60 +195,6 @@ void configureTCC(tccConfiguration& tccConfig, long pwm_frequency, bool negate, GCLK_CLKCTRL_ID_ofthistcc; // Feed GCLK4 to tcc while (GCLK->STATUS.bit.SYNCBUSY); // Wait for synchronization - - static bool isOVFEOConfigured = false; - if(enableOVFEO) - { - if(isOVFEOConfigured) - { - debugPrintln("Fatal error! OVFEO already configured, you can only configure it once."); - } - else - { - - debugPrintf("Configuring EVSYS for TCC channel %d\n\r", tccConfig.tcc.tccn); - - uint8_t evgen_tcc = 0; - switch (tccConfig.tcc.tccn){ - case 0: evgen_tcc = EVSYS_ID_GEN_TCC0_OVF; break; - case 1: evgen_tcc = EVSYS_ID_GEN_TCC1_OVF; break; - case 2: evgen_tcc = EVSYS_ID_GEN_TCC2_OVF; break; - // case 3: channel.bit.EVGEN = EVSYS_ID_GEN_TC3_OVF; break; // TODO: we need the TC equivalent of TCC_WAVEB_WAVEGENB_DSBOTTOM - // case 4: channel.bit.EVGEN = EVSYS_ID_GEN_TC4_OVF; break; // TODO: we need the TC equivalent of TCC_WAVEB_WAVEGENB_DSBOTTOM - // case 5: channel.bit.EVGEN = EVSYS_ID_GEN_TC5_OVF; break; // TODO: we need the TC equivalent of TCC_WAVEB_WAVEGENB_DSBOTTOM - default: - debugPrintf("Error in configureTCC() [EVSYS] Bad tccn number: %d\n\r", tccConfig.tcc.tccn); - return; - } - - EVSYS_USER_Type userADC; - userADC.bit.CHANNEL = tccConfig.tcc.tccn + 1; /* use channel tccn p421: "Note that to select channel n, the value (n+1) must be written to the USER.CHANNEL bit group." */ - userADC.bit.USER = EVSYS_ID_USER_ADC_SYNC; /* ADC Start*/ - EVSYS->USER.reg = userADC.reg; - // configuration of the event system so that it triggers an "EVSYS_ID_USER_ADC_SYNC" at each timer overflow (underflow) - EVSYS_CHANNEL_Type channelTCC_ADC; - channelTCC_ADC.bit.EVGEN = evgen_tcc; - channelTCC_ADC.bit.EDGSEL = EVSYS_CHANNEL_EDGSEL_NO_EVT_OUTPUT_Val; - channelTCC_ADC.bit.PATH = EVSYS_CHANNEL_PATH_ASYNCHRONOUS_Val; //EVSYS_ID_USER_ADC_SYNC is asynchronous only - channelTCC_ADC.bit.SWEVT = 0; /* no software trigger */ - channelTCC_ADC.bit.CHANNEL = userADC.bit.CHANNEL - 1; - EVSYS->CHANNEL.reg = channelTCC_ADC.reg; - - EVSYS_USER_Type userDMA3; - userDMA3.bit.CHANNEL = tccConfig.tcc.tccn + 6 + 1; /* use channel tccn p421: "Note that to select channel n, the value (n+1) must be written to the USER.CHANNEL bit group." */ - userDMA3.bit.USER = EVSYS_ID_USER_DMAC_CH_3; /* DMA_Start*/ - EVSYS->USER.reg = userDMA3.reg; - - EVSYS_CHANNEL_Type channelTCC_DMA3; - channelTCC_DMA3.bit.EVGEN = evgen_tcc; - channelTCC_DMA3.bit.EDGSEL = EVSYS_CHANNEL_EDGSEL_NO_EVT_OUTPUT_Val; - channelTCC_DMA3.bit.PATH = EVSYS_CHANNEL_PATH_RESYNCHRONIZED_Val; //DMA and TCC use different clocks (I think?) - channelTCC_DMA3.bit.SWEVT = 0; /* no software trigger */ - channelTCC_DMA3.bit.CHANNEL = userDMA3.bit.CHANNEL - 1; - EVSYS->CHANNEL.reg = channelTCC_DMA3.reg; - } - } - tccConfigured[tccConfig.tcc.tccn] = true; @@ -372,12 +318,6 @@ void configureTCC(tccConfiguration& tccConfig, long pwm_frequency, bool negate, } -void EVSYS_Init(int chaninfo) -{ - - - -} From 115f0bae69d1926caeea94a737b674a7f54696ce Mon Sep 17 00:00:00 2001 From: maxime Date: Sat, 5 Jun 2021 20:51:20 -0600 Subject: [PATCH 10/15] Seems that the loop is complete, we have async readings all around --- src/common/hardware_specific/samd21.cpp | 64 +++++++- .../hardware_specific/samd21_AdvancedSPI.cpp | 20 ++- .../hardware_specific/samd21_AdvancedSPI.h | 2 - src/common/hardware_specific/samd_mcu.h | 25 ++- .../hardware_specific/samd21_mcu.cpp | 15 +- .../hardware_specific/samd21_mcu.h | 11 +- src/drivers/hardware_specific/samd21_mcu.cpp | 2 + src/sensors/MagneticSensor.cpp | 58 +++++++ src/sensors/MagneticSensor.h | 37 +++++ .../SAMDMagneticSensorSPI.cpp | 150 ++++++++++++++++++ .../hardware_specific/SAMDMagneticSensorSPI.h | 39 +++++ 11 files changed, 399 insertions(+), 24 deletions(-) create mode 100644 src/sensors/MagneticSensor.cpp create mode 100644 src/sensors/MagneticSensor.h create mode 100644 src/sensors/hardware_specific/SAMDMagneticSensorSPI.cpp create mode 100644 src/sensors/hardware_specific/SAMDMagneticSensorSPI.h diff --git a/src/common/hardware_specific/samd21.cpp b/src/common/hardware_specific/samd21.cpp index 5ad408bd..25ee8c30 100644 --- a/src/common/hardware_specific/samd21.cpp +++ b/src/common/hardware_specific/samd21.cpp @@ -231,11 +231,11 @@ void initDMAC() NVIC_EnableIRQ( DMAC_IRQn ) ; } -bool g_DMACChannelInitialized[] = {false, false, false, false, false, false, false, false, false, false, false, false, false, false, false, false}; +bool g_DMACChannelInitialized[] = {false, false, false, false, false, false, false, false, false, false, false, false}; -static void (*DMAC_Handlers[12])(volatile DMAC_CHINTFLAG_Type & flags, volatile DMAC_CHCTRLA_Type & chctrla); +static DMACInterruptCallback * DMAC_Handlers[12] = {nullptr, nullptr, nullptr, nullptr, nullptr, nullptr, nullptr, nullptr, nullptr, nullptr, nullptr}; -int initDMAChannel(uint8_t channel, DMAC_CHINTENSET_Type chintset, DMAC_CHCTRLB_Type chctrlb, const DmacDescriptor & descriptor, void (*interrupt_handler)(volatile DMAC_CHINTFLAG_Type &, volatile DMAC_CHCTRLA_Type &), bool force) +int initDMAChannel(uint8_t channel, DMAC_CHINTENSET_Type chintset, DMAC_CHCTRLB_Type chctrlb, const DmacDescriptor & descriptor, DMACInterruptCallback * interrupt_handler, bool force) { if(channel > 11) @@ -297,16 +297,66 @@ void trigDMACChannel(uint8_t channel) } -void DMAC_Handler() __attribute__((weak)); void DMAC_Handler() { DMAC->CHID.bit.ID = DMAC->INTPEND.bit.ID; - void (*interrupt_handler)(volatile DMAC_CHINTFLAG_Type &, volatile DMAC_CHCTRLA_Type &) = DMAC_Handlers[DMAC->CHID.bit.ID]; + DMACInterruptCallback *interrupt_handler = DMAC_Handlers[DMAC->CHID.bit.ID]; if(interrupt_handler != nullptr) - interrupt_handler(DMAC->CHINTFLAG, DMAC->CHCTRLA); + (*interrupt_handler)(DMAC->CHINTFLAG, DMAC->CHCTRLA); +} + +typedef struct _InerruptHandlers +{ + uint8_t count = 0; + TccInterruptCallback* handlers[MAX_HANDLERS]; +} InerruptHandlers; + +static InerruptHandlers TCC_Handlers[TCC_INST_NUM]; + + +Tcc * addTCCHandler(uint8_t tccn, TccInterruptCallback * interrupt_handler) +{ + + if(interrupt_handler == nullptr) + { + debugPrintln("initTCCHandler() Error: illegal nullptr handler"); + return nullptr; + } - + if(tccn >= TCC_INST_NUM) + { + debugPrintf("initTCCHandler() Error: tccn %d is out of bounds (only &d TCC units)", tccn, TCC_INST_NUM); + return nullptr; + } + + if(TCC_Handlers[tccn].count < MAX_HANDLERS) + { + + TCC_Handlers[tccn].handlers[TCC_Handlers[0].count] = interrupt_handler; + TCC_Handlers[tccn].count++; + switch(tccn) + { + case 0: if(TCC_Handlers[tccn].count == 0) NVIC_EnableIRQ( TCC0_IRQn ) ; return TCC0; + case 1: if(TCC_Handlers[tccn].count == 0) NVIC_EnableIRQ( TCC1_IRQn ) ; return TCC1; + case 2: if(TCC_Handlers[tccn].count == 0) NVIC_EnableIRQ( TCC2_IRQn ) ; return TCC2; + default: return nullptr; + } + return 0; + } + // else + debugPrintf("initTCCHandler() Error: maximum number of handlers (%d) reached for TCC (%d)", MAX_HANDLERS, tccn); + return nullptr; } + +#define TCCN_Handler(tccn, tcc)\ + for(int i = 0; i < TCC_Handlers[tccn].count; i++)\ + (*TCC_Handlers[tccn].handlers[i])(tcc); + + +void TCC0_Handler() { TCCN_Handler(0, TCC0); } +void TCC1_Handler() { TCCN_Handler(1, TCC1); } +void TCC2_Handler() { TCCN_Handler(2, TCC2); } + #endif //_SAMD21_ diff --git a/src/common/hardware_specific/samd21_AdvancedSPI.cpp b/src/common/hardware_specific/samd21_AdvancedSPI.cpp index bc0cd111..9d7d06f5 100644 --- a/src/common/hardware_specific/samd21_AdvancedSPI.cpp +++ b/src/common/hardware_specific/samd21_AdvancedSPI.cpp @@ -1,7 +1,4 @@ #include "samd21_AdvancedSPI.h" - -#ifdef _SAMD21_ - #include @@ -198,6 +195,19 @@ int SAMDAdvancedSPI::init(SercomSpiClockMode spi_mode, uint32_t clock_speed) } +byte SAMDAdvancedSPI::spiCalcEvenParity(word value) +{ + byte cnt = 0; + byte i; + + for (i = 0; i < 16; i++) + { + if (value & 0x1) cnt++; + value >>= 1; + } + return cnt & 0x1; +} + uint8_t SAMDAdvancedSPI::transfer(uint8_t data) { @@ -240,6 +250,4 @@ void SAMDAdvancedSPI::close(){ sercomCfg.sercom->SPI.CTRLA.bit.SWRST = 1; //Wait both bits Software Reset from CTRLA and SYNCBUSY are equal to 0 while(sercomCfg.sercom->SPI.CTRLA.bit.SWRST || sercomCfg.sercom->SPI.SYNCBUSY.bit.SWRST); -} - -#endif \ No newline at end of file +} \ No newline at end of file diff --git a/src/common/hardware_specific/samd21_AdvancedSPI.h b/src/common/hardware_specific/samd21_AdvancedSPI.h index 8acb7f96..0b4fff70 100644 --- a/src/common/hardware_specific/samd21_AdvancedSPI.h +++ b/src/common/hardware_specific/samd21_AdvancedSPI.h @@ -1,9 +1,7 @@ #pragma once - #include #ifdef _SAMD21_ - #include "samd_mcu.h" class SAMDAdvancedSPI diff --git a/src/common/hardware_specific/samd_mcu.h b/src/common/hardware_specific/samd_mcu.h index 303e85a0..ea89873b 100644 --- a/src/common/hardware_specific/samd_mcu.h +++ b/src/common/hardware_specific/samd_mcu.h @@ -1,4 +1,5 @@ #pragma once + #include @@ -177,15 +178,35 @@ static char buffer[1000]; #define debugPrintln(arg) ; #endif +//SPI SercomSpiClockMode from_SPI_MODE(int spi_mode); +//EVSYS extern bool g_EVSYSChannelInitialized[]; int initEVSYS(uint8_t evsysChannel, uint16_t EVSYS_ID_USER, uint16_t EVSYS_ID_GEN, uint16_t EVSYS_CHANNEL_PATH, uint16_t EVSYS_CHANNEL_EDGSEL, bool force = false); +//DMA extern bool g_DMACChannelInitialized[]; void initDMAC(); -int initDMAChannel(uint8_t channel, DMAC_CHINTENSET_Type chintset, DMAC_CHCTRLB_Type chctrlb, const DmacDescriptor & descriptor, void (*interrupt_handler)(volatile DMAC_CHINTFLAG_Type &, volatile DMAC_CHCTRLA_Type &), bool force = false); +class DMACInterruptCallback +{ + public: + virtual void operator()(volatile DMAC_CHINTFLAG_Type &, volatile DMAC_CHCTRLA_Type &) = 0; +}; + +int initDMAChannel(uint8_t channel, DMAC_CHINTENSET_Type chintset, DMAC_CHCTRLB_Type chctrlb, const DmacDescriptor & descriptor, DMACInterruptCallback * interrupt_handler, bool force = false); void trigDMACChannel(uint8_t channel); -uint32_t computeDSTADDR(uint8_t * startAddress, uint32_t STEPSEL, uint32_t STEPSIZE, uint32_t BEATSIZE, uint32_t BTCNT); \ No newline at end of file +uint32_t computeDSTADDR(uint8_t * startAddress, uint32_t STEPSEL, uint32_t STEPSIZE, uint32_t BEATSIZE, uint32_t BTCNT); + + +//TC/TCC +#define MAX_HANDLERS 12 +class TccInterruptCallback +{ + public: + virtual void operator()(Tcc * tcc) = 0; +}; + +Tcc * addTCCHandler(uint8_t tccn, TccInterruptCallback * interrupt_handler); \ No newline at end of file diff --git a/src/current_sense/hardware_specific/samd21_mcu.cpp b/src/current_sense/hardware_specific/samd21_mcu.cpp index 82d31355..f21c7bb1 100644 --- a/src/current_sense/hardware_specific/samd21_mcu.cpp +++ b/src/current_sense/hardware_specific/samd21_mcu.cpp @@ -289,10 +289,7 @@ DmacDescriptor descriptors[20] __attribute__ ((aligned (16))); void dmac_adc_Handler(volatile DMAC_CHINTFLAG_Type & flags, volatile DMAC_CHCTRLA_Type & chctrla) { - chctrla.bit.ENABLE = 0b1; - flags.bit.TCMPL = 0b1; - ADC->INPUTCTRL.bit.INPUTOFFSET = 0; - instance.dma_i++; + } void SAMDCurrentSenseADCDMA::initDMAChannel() { @@ -337,10 +334,18 @@ void SAMDCurrentSenseADCDMA::initDMAChannel() { - ::initDMAChannel(channelDMA, chinset, chctrlb, descriptors[firstAIN], dmac_adc_Handler); + ::initDMAChannel(channelDMA, chinset, chctrlb, descriptors[firstAIN], this); } +void SAMDCurrentSenseADCDMA::operator()(volatile DMAC_CHINTFLAG_Type & flags, volatile DMAC_CHCTRLA_Type & chctrla) +{ + chctrla.bit.ENABLE = 0b1; + flags.bit.TCMPL = 0b1; + ADC->INPUTCTRL.bit.INPUTOFFSET = 0; + dma_i++; +} + int SAMDCurrentSenseADCDMA::initEVSYS() { if(::initEVSYS(0, EVSYS_ID_USER_ADC_SYNC, this->EVSYS_ID_GEN_TCC_OVF, EVSYS_CHANNEL_PATH_ASYNCHRONOUS_Val, EVSYS_CHANNEL_EDGSEL_NO_EVT_OUTPUT_Val) != 0) diff --git a/src/current_sense/hardware_specific/samd21_mcu.h b/src/current_sense/hardware_specific/samd21_mcu.h index 09f23845..e9663c54 100644 --- a/src/current_sense/hardware_specific/samd21_mcu.h +++ b/src/current_sense/hardware_specific/samd21_mcu.h @@ -3,22 +3,29 @@ #include - +#include +#include "../../common/hardware_specific/samd_mcu.h" // #define SIMPLEFOC_SAMD_DEBUG #if !defined(SIMPLEFOC_SAMD_DEBUG_SERIAL) #define SIMPLEFOC_SAMD_DEBUG_SERIAL Serial #endif -class SAMDCurrentSenseADCDMA +class SAMDCurrentSenseADCDMA : public DMACInterruptCallback { public: static SAMDCurrentSenseADCDMA * getHardwareAPIInstance(); SAMDCurrentSenseADCDMA(); + void init(int pinA, int pinB, int pinC, int EVSYS_ID_GEN_TCC_OVF = -1, int pinAREF = -1, float voltageAREF = 3.3, uint32_t adcBits = 12, uint32_t channelDMA = 3); + uint32_t readResults(uint16_t & a, uint16_t & b, uint16_t & c); + + void operator()(volatile DMAC_CHINTFLAG_Type &, volatile DMAC_CHCTRLA_Type &) override; + float toVolts(uint16_t counts); + uint16_t adcBuffer[20]; int adc_i = 0; int dma_i = 0; diff --git a/src/drivers/hardware_specific/samd21_mcu.cpp b/src/drivers/hardware_specific/samd21_mcu.cpp index 9bbb7a25..d987f838 100644 --- a/src/drivers/hardware_specific/samd21_mcu.cpp +++ b/src/drivers/hardware_specific/samd21_mcu.cpp @@ -349,4 +349,6 @@ void writeSAMDDutyCycle(int chaninfo, float dc) { + + #endif diff --git a/src/sensors/MagneticSensor.cpp b/src/sensors/MagneticSensor.cpp new file mode 100644 index 00000000..510b9518 --- /dev/null +++ b/src/sensors/MagneticSensor.cpp @@ -0,0 +1,58 @@ +#include "MagneticSensor.h" +#include +#include "../common/hardware_specific/samd_mcu.h" +MagneticSensor::MagneticSensor(uint32_t cpr) +: cpr(cpr) +{ + +} + + +void MagneticSensor::init(){ + + // velocity calculation init + angle_prev = 0; + // full rotations tracking number + full_rotation_count = 0; +} + +void MagneticSensor::updateFullRotationCount() +{ + int delta = raw_count - raw_count_prev; + // if overflow happened track it as full rotation + if( abs(delta) > (cpr*4/5) ) + full_rotation_count += delta > 0 ? -cpr : cpr; + +} + +// get current angle (rad) +float MagneticSensor::getAngle(){ + + // raw data from sensor + raw_count_prev = raw_count; + uint64_t timestamp_tmp; + raw_count = getRawCount(timestamp_tmp); + + if(timestamp_tmp == timestamp_us) //no new measurement + return angle_prev; + + timestamp_us_prev = timestamp_us; + timestamp_us = timestamp_tmp; + + updateFullRotationCount(); + + angle_prev = angle; + angle = ( (float) (full_rotation_count + (int)raw_count) / (float)cpr) * _2PI; + + return angle; +} + +// get velocity (rad/s) +float MagneticSensor::getVelocity() +{ + if(timestamp_us == timestamp_us_prev) + return 0.0f; + + unsigned long delta = (timestamp_us < timestamp_us_prev) ? ULONG_MAX - timestamp_us_prev + timestamp_us : timestamp_us - timestamp_us_prev; + return (angle - angle_prev)/(delta * 1e-6f); +} diff --git a/src/sensors/MagneticSensor.h b/src/sensors/MagneticSensor.h new file mode 100644 index 00000000..645ef48c --- /dev/null +++ b/src/sensors/MagneticSensor.h @@ -0,0 +1,37 @@ +#pragma once +#include "Arduino.h" +#include "../common/base_classes/Sensor.h" +#include "../common/foc_utils.h" +#include "../common/time_utils.h" + + +class MagneticSensor: public Sensor +{ + public: + MagneticSensor(uint32_t cpr); + + // implementation of abstract functions of the Sensor class + /** get current angle (rad) */ + float getAngle() override; + /** get current angular velocity (rad/s) **/ + float getVelocity() override; + + virtual void init(); + + virtual uint32_t getRawCount(uint64_t & timestamp_us) = 0; + + private: + + virtual void updateFullRotationCount(); + + uint32_t cpr; //!< Maximum range of the magnetic sensor + + // total angle tracking variables + int32_t full_rotation_count; //! 0) + { + Tcc * tcc = addTCCHandler(tccN, this); + tcc->INTENSET.bit.OVF = 0b1; + } +} + +void SAMDMagneticSensorSPI::tccHandler(Tcc * tcc) +{ + +} + +void SAMDMagneticSensorSPI::operator()(Tcc * tcc) +{ + digitalWrite(this->spi->pinSS, LOW); + + trigDMACChannel(dmaTX); + DMAC->CHID.reg = DMAC_CHID_ID(dmaTX); + DMAC->CHCTRLA.bit.ENABLE = 0b1; + DMAC->CHID.reg = DMAC_CHID_ID(dmaRX); + DMAC->CHCTRLA.bit.ENABLE = 0b1; + + tcc->INTFLAG.bit.OVF = 0b1; +} + +void SAMDMagneticSensorSPI::operator()(volatile DMAC_CHINTFLAG_Type &, volatile DMAC_CHCTRLA_Type &) +{ + if(DMAC->CHID.bit.ID == dmaTX) + DMAC->CHCTRLA.bit.ENABLE = 0b0; + + if(DMAC->CHID.bit.ID == dmaRX) + { + DMAC->CHCTRLA.bit.ENABLE = 0b0; + digitalWrite(this->spi->pinSS, HIGH); + } +} + +void SAMDMagneticSensorSPI::init(SAMDAdvancedSPI* _spi) +{ + + spi = _spi; + spi->init(from_SPI_MODE(config.spi_mode), config.clock_speed); + MagneticSensor::init(); + if(tccN > 0) + { + initDMA(); + } +} + +// function reading the raw counter of the magnetic sensor +uint32_t SAMDMagneticSensorSPI::getRawCount(uint64_t & timestamp_us){ + word command = config.angle_register; + + if (config.command_rw_bit > 0) + command |= (1 << config.command_rw_bit); + + if (config.command_parity_bit > 0) + command |= ((word)spi->spiCalcEvenParity(command) << config.command_parity_bit); //Add a parity bit on the the MSB + + spi->transfer16(command); + timestamp_us = _micros(); + delayMicroseconds(10); + + word register_value = spi->transfer16(command); + + register_value = register_value >> (1 + config.data_start_bit - config.bit_resolution); //this should shift data to the rightmost bits of the word + + const static word data_mask = 0xFFFF >> (16 - config.bit_resolution); + + return register_value & data_mask; // Return the data, stripping the non data (e.g parity) bits +} + + + +void SAMDMagneticSensorSPI::initDMA() { + + + DMAC_CHINTENSET_Type chinset{.reg = 0}; + chinset.bit.TCMPL = 0b1; + //SerialUSB.println("d"); + // configure the channel + DMAC_CHCTRLB_Type chctrlb{.reg = 0}; + chctrlb.bit.LVL = DMAC_CHCTRLB_LVL_LVL0_Val; + chctrlb.bit.EVIE = 0b0; //input (USER) event enabled? + chctrlb.bit.EVOE = 0b0; //output (GEN) event enabled? + chctrlb.bit.EVACT = DMAC_CHCTRLB_EVACT_NOACT_Val; //only used if EVIE is set + chctrlb.bit.TRIGSRC = SERCOM4_DMAC_ID_TX; + chctrlb.bit.TRIGACT = DMAC_CHCTRLB_TRIGACT_BEAT_Val; //block, beat or transaction + + + volatile DMAC_BTCTRL_Type & btctrl_write = SPIdescriptors[0].BTCTRL; + btctrl_write.bit.VALID = 0b1; /*!< bit: 0 Descriptor Valid */ + + //we want no events after the last adc read + btctrl_write.bit.EVOSEL = DMAC_BTCTRL_EVOSEL_DISABLE_Val; /*!< bit: 1.. 2 Event Output Selection */ + + btctrl_write.bit.BLOCKACT = DMAC_BTCTRL_BLOCKACT_NOACT_Val; /*!< bit: 3.. 4 Block Action */ + btctrl_write.bit.BEATSIZE = DMAC_BTCTRL_BEATSIZE_BYTE_Val; /*!< bit: 8.. 9 Beat Size (byte, half-words, words) */ + btctrl_write.bit.SRCINC = 0b1; /*!< bit: 10 Source Address Increment Enable */ + btctrl_write.bit.DSTINC = 0b0; /*!< bit: 11 Destination Address Increment Enable */ + btctrl_write.bit.STEPSEL = DMAC_BTCTRL_STEPSEL_SRC_Val; /*!< bit: 12 Step Selection */ + btctrl_write.bit.STEPSIZE = DMAC_BTCTRL_STEPSIZE_X1_Val; /*!< bit: 13..15 Address Increment Step Size */ + + SPIdescriptors[0].DESCADDR.reg = 0; //next descriptor + SPIdescriptors[0].BTCNT.reg = SPIbufferSize; + SPIdescriptors[0].DSTADDR.reg = (uint32_t)&spi->sercomCfg.sercom->SPI.DATA.reg; + SPIdescriptors[0].SRCADDR.reg = ((uint32_t)&SPItransmitBuffer[0]) + SPIbufferSize; + + + ::initDMAChannel(dmaTX, chinset, chctrlb, SPIdescriptors[0], this); + + // configure the channel + chctrlb.reg = 0; + chctrlb.bit.LVL = DMAC_CHCTRLB_LVL_LVL0_Val; + chctrlb.bit.EVIE = 0b0; //input (USER) event enabled? + chctrlb.bit.EVOE = 0b0; //output (GEN) event enabled? + chctrlb.bit.EVACT = DMAC_CHCTRLB_EVACT_NOACT_Val; //only used if EVIE is set + chctrlb.bit.TRIGSRC = SERCOM4_DMAC_ID_RX; + chctrlb.bit.TRIGACT = DMAC_CHCTRLB_TRIGACT_BEAT_Val; //block, beat or transaction + + // ///////////////////////////////////////////////// + // // read DMA + + volatile DMAC_BTCTRL_Type & btctrl_read = SPIdescriptors[1].BTCTRL; + btctrl_read.bit.VALID = 0b1; /*!< bit: 0 Descriptor Valid */ + + //we want no events after the last adc read + btctrl_read.bit.EVOSEL = DMAC_BTCTRL_EVOSEL_DISABLE_Val; /*!< bit: 1.. 2 Event Output Selection */ + + btctrl_read.bit.BLOCKACT = DMAC_BTCTRL_BLOCKACT_NOACT_Val; //isLast ? DMAC_BTCTRL_BLOCKACT_NOACT_Val : DMAC_BTCTRL_BLOCKACT_INT_Val; /*!< bit: 3.. 4 Block Action */ + btctrl_read.bit.BEATSIZE = DMAC_BTCTRL_BEATSIZE_BYTE_Val; /*!< bit: 8.. 9 Beat Size (byte, half-words, words) */ + btctrl_read.bit.SRCINC = 0b0; /*!< bit: 10 Source Address Increment Enable */ + btctrl_read.bit.DSTINC = 0b1; /*!< bit: 11 Destination Address Increment Enable */ + btctrl_read.bit.STEPSEL = DMAC_BTCTRL_STEPSEL_DST_Val; /*!< bit: 12 Step Selection */ + btctrl_read.bit.STEPSIZE = DMAC_BTCTRL_STEPSIZE_X1_Val; /*!< bit: 13..15 Address Increment Step Size */ + + SPIdescriptors[1].DESCADDR.reg = 0; + SPIdescriptors[1].BTCNT.reg = SPIbufferSize; + SPIdescriptors[1].DSTADDR.reg = ((uint32_t)&SPIreceiveBuffer[0]) + SPIbufferSize; + SPIdescriptors[1].SRCADDR.reg = (uint32_t) &spi->sercomCfg.sercom->SPI.DATA.reg; + + ::initDMAChannel(dmaTX, chinset, chctrlb, SPIdescriptors[1], this); + +} \ No newline at end of file diff --git a/src/sensors/hardware_specific/SAMDMagneticSensorSPI.h b/src/sensors/hardware_specific/SAMDMagneticSensorSPI.h new file mode 100644 index 00000000..e6c66abd --- /dev/null +++ b/src/sensors/hardware_specific/SAMDMagneticSensorSPI.h @@ -0,0 +1,39 @@ +#pragma once +#include +#include +#include "../MagneticSensor.h" +#include "../MagneticSensorSPI.h" +#include "../../common/hardware_specific/samd21_AdvancedSPI.h" + +#define SPIbufferSize 2 +class SAMDMagneticSensorSPI: public MagneticSensor, public TccInterruptCallback, public DMACInterruptCallback{ + public: + /** + * class constructor + * @param config SPI config + */ + SAMDMagneticSensorSPI(MagneticSensorSPIConfig_s config, int8_t tccN = -1, int8_t dmaTX = -1, int8_t dmaRX = -1); + + /** sensor initialise pins */ + void init(SAMDAdvancedSPI* _spi); + + uint32_t getRawCount(uint64_t & timestamp_us) override; + + void operator()(Tcc * tcc) override; + + void operator()(volatile DMAC_CHINTFLAG_Type &, volatile DMAC_CHCTRLA_Type &) override; + + private: + void tccHandler(Tcc * tcc); + void initDMA(); + MagneticSensorSPIConfig_s config; + + SAMDAdvancedSPI* spi; + int8_t tccN, dmaTX, dmaRX; + + + uint8_t SPIreceiveBuffer[SPIbufferSize]; + uint8_t SPItransmitBuffer[SPIbufferSize]; + + DmacDescriptor SPIdescriptors[2] __attribute__ ((aligned (16))); +}; \ No newline at end of file From e881c82af66e9f8b59971d1b7334971be8c5dc6f Mon Sep 17 00:00:00 2001 From: maxime Date: Mon, 7 Jun 2021 12:02:17 -0600 Subject: [PATCH 11/15] SPI was still poll-based, now everythings works togheter. replaced a few const char* with PSTR() --- src/common/hardware_specific/samd21.cpp | 57 +++++--- .../hardware_specific/samd21_AdvancedSPI.cpp | 22 ++-- src/common/hardware_specific/samd_mcu.cpp | 4 +- src/common/hardware_specific/samd_mcu.h | 3 +- .../hardware_specific/samd21_mcu.cpp | 59 +++------ .../hardware_specific/samd21_mcu.h | 9 +- .../SAMDMagneticSensorSPI.cpp | 124 +++++++++++------- .../hardware_specific/SAMDMagneticSensorSPI.h | 16 ++- 8 files changed, 159 insertions(+), 135 deletions(-) diff --git a/src/common/hardware_specific/samd21.cpp b/src/common/hardware_specific/samd21.cpp index 25ee8c30..0b6e6d8e 100644 --- a/src/common/hardware_specific/samd21.cpp +++ b/src/common/hardware_specific/samd21.cpp @@ -132,7 +132,7 @@ int initEVSYS(uint8_t evsysChannel, uint16_t EVSYS_ID_USER, uint16_t EVSYS_ID_GE if(g_EVSYSChannelInitialized[evsysChannel] && !force) { - debugPrintf("initEVSYS() channel %d is already initialized", evsysChannel); + debugPrintf_P(PSTR("initEVSYS() channel %d is already initialized"), evsysChannel); return -1; } @@ -145,24 +145,24 @@ int initEVSYS(uint8_t evsysChannel, uint16_t EVSYS_ID_USER, uint16_t EVSYS_ID_GE if(evsysChannel > EVSYS_GCLK_ID_SIZE - 1) { - debugPrintf("initEVSYS() channel %d out of bounds (max %d)", evsysChannel, EVSYS_GCLK_ID_SIZE); + debugPrintf_P(PSTR("initEVSYS() channel %d out of bounds (max %d)"), evsysChannel, EVSYS_GCLK_ID_SIZE); return -1; } if(EVSYS_ID_USER > EVSYS_ID_USER_PTC_STCONV) { - debugPrintf("initEVSYS() EVSYS_ID_USER %d out of bounds (max %d)", EVSYS_ID_USER, EVSYS_ID_USER_PTC_STCONV); + debugPrintf_P(PSTR("initEVSYS() EVSYS_ID_USER %d out of bounds (max %d)"), EVSYS_ID_USER, EVSYS_ID_USER_PTC_STCONV); return -1; } if(EVSYS_ID_GEN > EVSYS_ID_GEN_PTC_WCOMP) { - debugPrintf("initEVSYS() EVSYS_ID_GEN %d out of bounds (max %d)", EVSYS_ID_GEN, EVSYS_ID_GEN_PTC_WCOMP); + debugPrintf_P(PSTR("initEVSYS() EVSYS_ID_GEN %d out of bounds (max %d)"), EVSYS_ID_GEN, EVSYS_ID_GEN_PTC_WCOMP); return -1; } if(EVSYS_CHANNEL_PATH == EVSYS_CHANNEL_PATH_ASYNCHRONOUS_Val && EVSYS_CHANNEL_EDGSEL != EVSYS_CHANNEL_EDGSEL_NO_EVT_OUTPUT_Val) { EVSYS_CHANNEL_EDGSEL = EVSYS_CHANNEL_EDGSEL_NO_EVT_OUTPUT_Val; - debugPrintf("initEVSYS() warning, channel %d is configured with asynchronous path, forcing EDGSEL to 'NO_EVT_OUTPUT', note that interrupts aren't supported with that path", evsysChannel); + debugPrintf_P(PSTR("initEVSYS() warning, channel %d is configured with asynchronous path, forcing EDGSEL to 'NO_EVT_OUTPUT', note that interrupts aren't supported with that path"), evsysChannel); } g_EVSYSChannelInitialized[evsysChannel] = true; @@ -201,6 +201,15 @@ static DmacDescriptor descriptor_section[12] __attribute__ ((aligned (16))); static volatile DmacDescriptor write_back[12] __attribute__ ((aligned (16))); void initDMAC() { + static bool initialized = false; + + if(initialized) + { + debugPrintln(F("initDMA() already initialized, ignoring...")); + return; + } + initialized = true; + // probably on by default PM->AHBMASK.reg |= PM_AHBMASK_DMAC ; PM->APBBMASK.reg |= PM_APBBMASK_DMAC ; @@ -240,16 +249,17 @@ int initDMAChannel(uint8_t channel, DMAC_CHINTENSET_Type chintset, DMAC_CHCTRLB_ if(channel > 11) { - debugPrintf("initDMAChannel() channel %d out of bounds (max %d)", channel, 12); + debugPrintf_P(PSTR("FATAL: initDMAChannel() channel %d out of bounds (max %d)\n\r"), channel, 12); return -1; } if(g_DMACChannelInitialized[channel] && !force) { - debugPrintf("initDMAChannel() channel %d is already initialized", channel); + debugPrintf_P(PSTR("FATAL: initDMAChannel() channel %d is already initialized\n\r"), channel); return -1; } + g_DMACChannelInitialized[channel] = true; //select the channel DMAC->CHID.bit.ID = channel; @@ -267,8 +277,9 @@ int initDMAChannel(uint8_t channel, DMAC_CHINTENSET_Type chintset, DMAC_CHCTRLB_ // start channel DMAC->CHCTRLA.bit.ENABLE = 0b1; + debugPrintf_P(PSTR("initDMAChannel() inittializing channel %d\n\r"), channel); - + return 0; } @@ -292,19 +303,26 @@ void trigDMACChannel(uint8_t channel) case 11: DMAC->SWTRIGCTRL.bit.SWTRIG11 = 0b1; break; default: - debugPrintf("Error in adcToDMATransfer() [DMAC] Bad channel number %d"); + debugPrintf_P(PSTR("Error in adcToDMATransfer() [DMAC] Bad channel number %d"), channel); } } void DMAC_Handler() { - - DMAC->CHID.bit.ID = DMAC->INTPEND.bit.ID; + uint8_t channel = DMAC->INTPEND.bit.ID; + DMAC->CHID.bit.ID = channel; + DMACInterruptCallback *interrupt_handler = DMAC_Handlers[DMAC->CHID.bit.ID]; if(interrupt_handler != nullptr) - (*interrupt_handler)(DMAC->CHINTFLAG, DMAC->CHCTRLA); + (*interrupt_handler)(channel, DMAC->CHINTFLAG, DMAC->CHCTRLA); + else + { + debugPrintf_P(PSTR("DMAC_Handler() %d null handler\n\r"), channel); + DMAC->CHINTFLAG.reg = DMAC_CHINTFLAG_MASK; //clear all flag so we don't get trapped here + + } } typedef struct _InerruptHandlers @@ -321,13 +339,13 @@ Tcc * addTCCHandler(uint8_t tccn, TccInterruptCallback * interrupt_handler) if(interrupt_handler == nullptr) { - debugPrintln("initTCCHandler() Error: illegal nullptr handler"); + debugPrintln(F("initTCCHandler() Error: illegal nullptr handler")); return nullptr; } if(tccn >= TCC_INST_NUM) { - debugPrintf("initTCCHandler() Error: tccn %d is out of bounds (only &d TCC units)", tccn, TCC_INST_NUM); + debugPrintf_P(PSTR("initTCCHandler() Error: tccn %d is out of bounds (only %d TCC units)"), tccn, TCC_INST_NUM); return nullptr; } @@ -335,22 +353,23 @@ Tcc * addTCCHandler(uint8_t tccn, TccInterruptCallback * interrupt_handler) { TCC_Handlers[tccn].handlers[TCC_Handlers[0].count] = interrupt_handler; + bool doNVIC = TCC_Handlers[tccn].count == 0; TCC_Handlers[tccn].count++; switch(tccn) { - case 0: if(TCC_Handlers[tccn].count == 0) NVIC_EnableIRQ( TCC0_IRQn ) ; return TCC0; - case 1: if(TCC_Handlers[tccn].count == 0) NVIC_EnableIRQ( TCC1_IRQn ) ; return TCC1; - case 2: if(TCC_Handlers[tccn].count == 0) NVIC_EnableIRQ( TCC2_IRQn ) ; return TCC2; + case 0: if(doNVIC) NVIC_EnableIRQ( TCC0_IRQn ) ; return TCC0; + case 1: if(doNVIC) NVIC_EnableIRQ( TCC1_IRQn ) ; return TCC1; + case 2: if(doNVIC) NVIC_EnableIRQ( TCC2_IRQn ) ; return TCC2; default: return nullptr; } return 0; } // else - debugPrintf("initTCCHandler() Error: maximum number of handlers (%d) reached for TCC (%d)", MAX_HANDLERS, tccn); + debugPrintf_P(PSTR("initTCCHandler() Error: maximum number of handlers (%d) reached for TCC (%d)"), MAX_HANDLERS, tccn); return nullptr; } -#define TCCN_Handler(tccn, tcc)\ +#define TCCN_Handler(tccn, tcc) \ for(int i = 0; i < TCC_Handlers[tccn].count; i++)\ (*TCC_Handlers[tccn].handlers[i])(tcc); diff --git a/src/common/hardware_specific/samd21_AdvancedSPI.cpp b/src/common/hardware_specific/samd21_AdvancedSPI.cpp index 9d7d06f5..add333be 100644 --- a/src/common/hardware_specific/samd21_AdvancedSPI.cpp +++ b/src/common/hardware_specific/samd21_AdvancedSPI.cpp @@ -37,13 +37,13 @@ SAMDAdvancedSPI::SAMDAdvancedSPI(SercomChannel sercomChannel, uint8_t pinMOSI, u int SAMDAdvancedSPI::init(SercomSpiClockMode spi_mode, uint32_t clock_speed) { - debugPrintf("MOSI: "); + debugPrint(F("MOSI: ")); defMOSI = getSamdPinDefinition(pinMOSI); - debugPrintf("MISO: "); + debugPrint(F("MISO: ")); defMISO = getSamdPinDefinition(pinMISO); - debugPrintf("SCK : "); + debugPrint(F("SCK : ")); defSCK = getSamdPinDefinition(pinSCK); - debugPrintf("SS : "); + debugPrint(F("SS : ")); defSS = getSamdPinDefinition(pinSS); if(defMOSI == nullptr || defMISO == nullptr || defSCK == nullptr || (hardwareSS && defSS == nullptr)) @@ -109,7 +109,7 @@ int SAMDAdvancedSPI::init(SercomSpiClockMode spi_mode, uint32_t clock_speed) ctrla.bit.CPHA = 0b1; break; default: - debugPrint("SPI sercom CLOCK configuration error!"); + debugPrint(F("SPI sercom CLOCK configuration error!")); return -1; } @@ -124,7 +124,7 @@ int SAMDAdvancedSPI::init(SercomSpiClockMode spi_mode, uint32_t clock_speed) SS_pad = defSS->sercom_pad; peripheral = EPioType::PIO_SERCOM; - debugPrintf("Using sercom %d\n\r", defMOSI->sercom); + debugPrintf_P(PSTR("Using sercom %d\n\r"), defMOSI->sercom); } else if(defMOSI->sercom_alt == sercomChannel && defMISO->sercom_alt == sercomChannel && defSCK->sercom_alt == sercomChannel && (ctrlb.bit.MSSEN == 0 || defSS->sercom_alt == sercomChannel)) { @@ -134,11 +134,11 @@ int SAMDAdvancedSPI::init(SercomSpiClockMode spi_mode, uint32_t clock_speed) SS_pad = defSS->sercom_pad_alt; peripheral = EPioType::PIO_SERCOM_ALT; - debugPrintf("Using sercom alt %d\n\r", defMOSI->sercom_alt); + debugPrintf_P(PSTR("Using sercom alt %d\n\r"), defMOSI->sercom_alt); } else { - debugPrint("SPI sercom CHANNEL configuration error!"); + debugPrint(F("SPI sercom CHANNEL configuration error!")); return -1; } @@ -147,7 +147,7 @@ int SAMDAdvancedSPI::init(SercomSpiClockMode spi_mode, uint32_t clock_speed) pinPeripheral(pinSCK, peripheral); pinPeripheral(pinSS, ctrlb.bit.MSSEN ? peripheral : EPioType::PIO_OUTPUT); - debugPrintf("Using sercom pads MOSI[%d], MISO[%d], SCK[%d], SS[%d]\n\r", MOSI_pad, MISO_pad, SCK_pad, ctrlb.bit.MSSEN ? SS_pad : -1); + debugPrintf_P(PSTR("Using sercom pads MOSI[%d], MISO[%d], SCK[%d], SS[%d]\n\r"), MOSI_pad, MISO_pad, SCK_pad, ctrlb.bit.MSSEN ? SS_pad : -1); if(MOSI_pad == PAD_0 && SCK_pad == PAD_1 && (ctrlb.bit.MSSEN ? (SS_pad == PAD_2 && MISO_pad == PAD_3) : (MISO_pad == PAD_3 || MISO_pad == PAD_2))) @@ -175,11 +175,11 @@ int SAMDAdvancedSPI::init(SercomSpiClockMode spi_mode, uint32_t clock_speed) } else { - debugPrint("SPI sercom PADS configuration invalid!"); + debugPrint(F("SPI sercom PADS configuration invalid!")); return -1; } - debugPrintf("Using sercom DOPO %d and DIPO %d\n\r", ctrla.bit.DOPO, ctrla.bit.DIPO); + debugPrintf_P(PSTR("Using sercom DOPO %d and DIPO %d\n\r"), ctrla.bit.DOPO, ctrla.bit.DIPO); ctrla.bit.MODE = SERCOM_SPI_CTRLA_MODE_SPI_MASTER_Val; ctrla.bit.DORD = SercomDataOrder::MSB_FIRST; diff --git a/src/common/hardware_specific/samd_mcu.cpp b/src/common/hardware_specific/samd_mcu.cpp index 736dc8a7..6b0fa15a 100644 --- a/src/common/hardware_specific/samd_mcu.cpp +++ b/src/common/hardware_specific/samd_mcu.cpp @@ -47,7 +47,7 @@ const SamdPinDefinition * getSamdPinDefinition(int arduinoPin) } if((uint32_t)arduinoPin > (PINS_COUNT-1)) { - debugPrintf("getSamdPinDefinition() : arduino pin %d above %d", arduinoPin, PINS_COUNT-1); + debugPrintf_P(PSTR("getSamdPinDefinition() : arduino pin %d above %d"), arduinoPin, PINS_COUNT-1); return nullptr; } @@ -76,7 +76,7 @@ const SamdPinDefinition * getSamdPinDefinition(int arduinoPin) const SamdPinDefinition * rv = &g_SamdPinDefinitions[index]; - debugPrintf("getSamdPinDefinition(): Arduino pin: %d, PORT%c%d, SERCOM%d:PAD[%d], SERCOM_ALT%d:PAD[%d] \n\r", arduinoPin, 'A' + port, port_pin, rv->sercom, rv->sercom_pad, rv->sercom_alt, rv->sercom_pad_alt); + debugPrintf_P(PSTR("getSamdPinDefinition(): Arduino pin: %d, PORT%c%d, SERCOM%d:PAD[%d], SERCOM_ALT%d:PAD[%d] \n\r"), arduinoPin, 'A' + port, port_pin, rv->sercom, rv->sercom_pad, rv->sercom_alt, rv->sercom_pad_alt); return rv; } diff --git a/src/common/hardware_specific/samd_mcu.h b/src/common/hardware_specific/samd_mcu.h index ea89873b..e6e6eb13 100644 --- a/src/common/hardware_specific/samd_mcu.h +++ b/src/common/hardware_specific/samd_mcu.h @@ -172,6 +172,7 @@ template void debugPrintln(T message){ SIMPLEFOC_SAMD_DEBUG_SERIAL.println(message);} static char buffer[1000]; #define debugPrintf(args...) sprintf(buffer, args); debugPrint(buffer); +#define debugPrintf_P(args...) sprintf_P(buffer, args); debugPrint(buffer); #else #define debugPrintf(args...) ; #define debugPrint(arg) ; @@ -193,7 +194,7 @@ void initDMAC(); class DMACInterruptCallback { public: - virtual void operator()(volatile DMAC_CHINTFLAG_Type &, volatile DMAC_CHCTRLA_Type &) = 0; + virtual void operator()(uint8_t channel, volatile DMAC_CHINTFLAG_Type &, volatile DMAC_CHCTRLA_Type &) = 0; }; int initDMAChannel(uint8_t channel, DMAC_CHINTENSET_Type chintset, DMAC_CHCTRLB_Type chctrlb, const DmacDescriptor & descriptor, DMACInterruptCallback * interrupt_handler, bool force = false); diff --git a/src/current_sense/hardware_specific/samd21_mcu.cpp b/src/current_sense/hardware_specific/samd21_mcu.cpp index f21c7bb1..c5364c96 100644 --- a/src/current_sense/hardware_specific/samd21_mcu.cpp +++ b/src/current_sense/hardware_specific/samd21_mcu.cpp @@ -28,14 +28,13 @@ void _configureADCLowSide(const int pinA,const int pinB,const int pinC) */ void _readADCVoltagesLowSide(float & a, float & b, float & c) { - static int last_read = -1; + static uint64_t last_ts = _micros(); - while(last_read == instance.dma_i); - - last_read = instance.dma_i; - - instance.readResults(a_raw, b_raw, c_raw); - + if(last_ts != instance.timestamp_us) + { + last_ts = instance.timestamp_us; + instance.readResults(a_raw, b_raw, c_raw); + } a = instance.toVolts(a_raw); b = instance.toVolts(b_raw); if(_isset(_pinC)) @@ -87,7 +86,7 @@ SAMDCurrentSenseADCDMA::SAMDCurrentSenseADCDMA() void SAMDCurrentSenseADCDMA::init(int pinA, int pinB, int pinC, int EVSYS_ID_GEN_TCC_OVF, int pinAREF, float voltageAREF -, uint32_t adcBits, uint32_t channelDMA) +, uint8_t adcBits, uint8_t channelDMA) { this->pinA = pinA; this->pinB = pinB; @@ -104,13 +103,13 @@ int EVSYS_ID_GEN_TCC_OVF, int pinAREF, float voltageAREF initPins(); if(this->EVSYS_ID_GEN_TCC_OVF != -1) { - debugPrintf("Configuring EVSYS for ADC with EVSYS_ID_GEN #%d", this->EVSYS_ID_GEN_TCC_OVF); + debugPrintf(PSTR("Configuring EVSYS for ADC with EVSYS_ID_GEN #%d, DMA %d\n\r"), this->EVSYS_ID_GEN_TCC_OVF, channelDMA); + if(initEVSYS() != 0) return; - - debugPrintln("a"); initDMA(); + initDMAChannel(); } initADC(); @@ -137,7 +136,7 @@ void SAMDCurrentSenseADCDMA::initPins(){ refsel = ADC_REFCTRL_REFSEL_INTVCC1_Val; if(pinAREF != -1) { - debugPrintf("Using AREF pin %d", pinAREF); + debugPrintf(PSTR("Using AREF pin %d"), pinAREF); switch(getSamdPinDefinition(pinAREF)->vref) { case VRef::VREFA: @@ -147,7 +146,7 @@ void SAMDCurrentSenseADCDMA::initPins(){ refsel = ADC_REFCTRL_REFSEL_AREFB_Val; break; default: - debugPrintf("Error: pin %d is not a valid AREF pin, falling back to 'INTVCC1'", pinAREF); + debugPrintf(PSTR("Error: pin %d is not a valid AREF pin, falling back to 'INTVCC1'"), pinAREF); } } @@ -270,28 +269,16 @@ void SAMDCurrentSenseADCDMA::initADC(){ adc_evctrl.bit.STARTEI = 1; ADC->EVCTRL.reg = adc_evctrl.reg; - //not evsys + dma driven - // ADC->INTENSET.bit.RESRDY = 1; - // NVIC_EnableIRQ( ADC_IRQn ); - ADC->CTRLA.bit.ENABLE = 0x01; ADCsync(); } -// DmacDescriptor descriptor_section[12] __attribute__ ((aligned (16))); -// volatile DmacDescriptor write_back[12] __attribute__ ((aligned (16))); - void SAMDCurrentSenseADCDMA::initDMA() { ::initDMAC(); } DmacDescriptor descriptors[20] __attribute__ ((aligned (16))); -void dmac_adc_Handler(volatile DMAC_CHINTFLAG_Type & flags, volatile DMAC_CHCTRLA_Type & chctrla) -{ - -} - void SAMDCurrentSenseADCDMA::initDMAChannel() { DMAC_CHINTENSET_Type chinset{.reg = 0}; @@ -331,19 +318,16 @@ void SAMDCurrentSenseADCDMA::initDMAChannel() { descriptors[i].DSTADDR.reg = computeDSTADDR((uint8_t*)&adcBuffer[i], btctrl.bit.STEPSEL, btctrl.bit.STEPSIZE, btctrl.bit.BEATSIZE, descriptors[i].BTCNT.reg); } - - - ::initDMAChannel(channelDMA, chinset, chctrlb, descriptors[firstAIN], this); } -void SAMDCurrentSenseADCDMA::operator()(volatile DMAC_CHINTFLAG_Type & flags, volatile DMAC_CHCTRLA_Type & chctrla) +void SAMDCurrentSenseADCDMA::operator()(uint8_t channel, volatile DMAC_CHINTFLAG_Type & flags, volatile DMAC_CHCTRLA_Type & chctrla) { chctrla.bit.ENABLE = 0b1; flags.bit.TCMPL = 0b1; ADC->INPUTCTRL.bit.INPUTOFFSET = 0; - dma_i++; + timestamp_us = _micros(); } int SAMDCurrentSenseADCDMA::initEVSYS() @@ -359,7 +343,7 @@ int SAMDCurrentSenseADCDMA::initEVSYS() case 2: EVGEN_DMAC = EVSYS_ID_GEN_DMAC_CH_2; break; case 3: EVGEN_DMAC = EVSYS_ID_GEN_DMAC_CH_3; break; default: - debugPrintf("initEVSYS(): Bad dma channel %d. Only 0,1,2 or 3 are supported.", channelDMA); + debugPrintf(PSTR("initEVSYS(): Bad dma channel %u. Only 0,1,2 or 3 are supported."), channelDMA); break; } if(::initEVSYS(1, EVSYS_ID_USER_ADC_START, EVGEN_DMAC, EVSYS_CHANNEL_PATH_ASYNCHRONOUS_Val, EVSYS_CHANNEL_EDGSEL_NO_EVT_OUTPUT_Val) != 0) @@ -368,17 +352,4 @@ int SAMDCurrentSenseADCDMA::initEVSYS() return 0; } -void ADC_Handler() __attribute__((weak)); -void ADC_Handler() -{ - instance.adc_i++; - // only used if event system is not used - if(instance.EVSYS_ID_GEN_TCC_OVF == -1) - { - uint16_t offset = ADC->INPUTCTRL.bit.INPUTOFFSET; - instance.adcBuffer[ADC->INPUTCTRL.bit.MUXPOS + (offset > 0 ? offset - 1 : ADC->INPUTCTRL.bit.INPUTSCAN)] = ADC->RESULT.reg; - } - ADC->INTFLAG.bit.RESRDY = 0b1; - ADC->INTFLAG.bit.SYNCRDY = 0b1; -} diff --git a/src/current_sense/hardware_specific/samd21_mcu.h b/src/current_sense/hardware_specific/samd21_mcu.h index e9663c54..e32addfc 100644 --- a/src/current_sense/hardware_specific/samd21_mcu.h +++ b/src/current_sense/hardware_specific/samd21_mcu.h @@ -18,18 +18,17 @@ class SAMDCurrentSenseADCDMA : public DMACInterruptCallback static SAMDCurrentSenseADCDMA * getHardwareAPIInstance(); SAMDCurrentSenseADCDMA(); - void init(int pinA, int pinB, int pinC, int EVSYS_ID_GEN_TCC_OVF = -1, int pinAREF = -1, float voltageAREF = 3.3, uint32_t adcBits = 12, uint32_t channelDMA = 3); + void init(int pinA, int pinB, int pinC, int EVSYS_ID_GEN_TCC_OVF = -1, int pinAREF = -1, float voltageAREF = 3.3, uint8_t adcBits = 12, uint8_t channelDMA = 0); uint32_t readResults(uint16_t & a, uint16_t & b, uint16_t & c); - void operator()(volatile DMAC_CHINTFLAG_Type &, volatile DMAC_CHCTRLA_Type &) override; + void operator()(uint8_t channel, volatile DMAC_CHINTFLAG_Type &, volatile DMAC_CHCTRLA_Type &) override; float toVolts(uint16_t counts); uint16_t adcBuffer[20]; - int adc_i = 0; - int dma_i = 0; int EVSYS_ID_GEN_TCC_OVF; + uint64_t timestamp_us; private: @@ -54,7 +53,7 @@ class SAMDCurrentSenseADCDMA : public DMACInterruptCallback int pinB; int pinC; int pinAREF; - uint32_t channelDMA; // DMA channel + uint8_t channelDMA; // DMA channel bool freeRunning; float voltageAREF; diff --git a/src/sensors/hardware_specific/SAMDMagneticSensorSPI.cpp b/src/sensors/hardware_specific/SAMDMagneticSensorSPI.cpp index de7a5ab3..602eb647 100644 --- a/src/sensors/hardware_specific/SAMDMagneticSensorSPI.cpp +++ b/src/sensors/hardware_specific/SAMDMagneticSensorSPI.cpp @@ -2,66 +2,100 @@ #include "../../common/hardware_specific/samd_mcu.h" SAMDMagneticSensorSPI::SAMDMagneticSensorSPI(MagneticSensorSPIConfig_s config, int8_t tccN, int8_t dmaTX, int8_t dmaRX) -: MagneticSensor(1 << config.bit_resolution), config(config), tccN(tccN), dmaTX(dmaTX), dmaRX(dmaRX), SPItransmitBuffer{0xFF, 0xFF} +: MagneticSensor(1 << config.bit_resolution), config(config), tccN(tccN), dmaTX(dmaTX), dmaRX(dmaRX), pinLow(false) { - if(tccN > 0) + +} + +void SAMDMagneticSensorSPI::operator()(Tcc * tcc) +{ + tcc->INTFLAG.bit.OVF = 0b1; + // debugPrintln("\n\r*low*"); + if(!pinLow) { - Tcc * tcc = addTCCHandler(tccN, this); - tcc->INTENSET.bit.OVF = 0b1; + pinLow = true; + digitalWrite(this->spi->pinSS, LOW); + trigDMACChannel(dmaTX); + DMAC->CHID.bit.ID = dmaTX; + DMAC->CHCTRLA.bit.ENABLE = 0b1; + DMAC->CHID.bit.ID = dmaRX; + DMAC->CHCTRLA.bit.ENABLE = 0b1; + } + } -void SAMDMagneticSensorSPI::tccHandler(Tcc * tcc) +void SAMDMagneticSensorSPI::operator()(uint8_t channel, volatile DMAC_CHINTFLAG_Type & chintflag, volatile DMAC_CHCTRLA_Type & chctrla) { + chintflag.bit.TCMPL = 0b1; + + if(pinLow) + { + chctrla.bit.ENABLE = 0b0; + + if(channel == dmaRX) + { + digitalWrite(this->spi->pinSS, HIGH); + pinLow = false; + last_timestamp_us = _micros(); + } + } } -void SAMDMagneticSensorSPI::operator()(Tcc * tcc) +word SAMDMagneticSensorSPI::makeSPICommand() { - digitalWrite(this->spi->pinSS, LOW); - - trigDMACChannel(dmaTX); - DMAC->CHID.reg = DMAC_CHID_ID(dmaTX); - DMAC->CHCTRLA.bit.ENABLE = 0b1; - DMAC->CHID.reg = DMAC_CHID_ID(dmaRX); - DMAC->CHCTRLA.bit.ENABLE = 0b1; + word command = config.angle_register; - tcc->INTFLAG.bit.OVF = 0b1; + if (config.command_rw_bit > 0) + command |= (1 << config.command_rw_bit); + + if (config.command_parity_bit > 0) + command |= ((word)spi->spiCalcEvenParity(command) << config.command_parity_bit); //Add a parity bit on the the MSB + + return command; } -void SAMDMagneticSensorSPI::operator()(volatile DMAC_CHINTFLAG_Type &, volatile DMAC_CHCTRLA_Type &) +word SAMDMagneticSensorSPI::extractResult(word register_value) { - if(DMAC->CHID.bit.ID == dmaTX) - DMAC->CHCTRLA.bit.ENABLE = 0b0; + register_value = register_value >> (1 + config.data_start_bit - config.bit_resolution); //this should shift data to the rightmost bits of the word + + const static word data_mask = 0xFFFF >> (16 - config.bit_resolution); + + return register_value & data_mask; // Return the data, stripping the non data (e.g parity) bits - if(DMAC->CHID.bit.ID == dmaRX) - { - DMAC->CHCTRLA.bit.ENABLE = 0b0; - digitalWrite(this->spi->pinSS, HIGH); - } } void SAMDMagneticSensorSPI::init(SAMDAdvancedSPI* _spi) { - spi = _spi; - spi->init(from_SPI_MODE(config.spi_mode), config.clock_speed); - MagneticSensor::init(); - if(tccN > 0) - { - initDMA(); - } + spi = _spi; + command = makeSPICommand(); + spi->init(from_SPI_MODE(config.spi_mode), config.clock_speed); + MagneticSensor::init(); + if(tccN >= 0) + { + for(int i = 0; i < bufferSize; i++) + transmitBuffer[i] = command; + + ::initDMAC(); + initDMA(); + Tcc * tcc = addTCCHandler(tccN, this); + tcc->INTENSET.bit.OVF = 0b1; + } } // function reading the raw counter of the magnetic sensor -uint32_t SAMDMagneticSensorSPI::getRawCount(uint64_t & timestamp_us){ - word command = config.angle_register; - - if (config.command_rw_bit > 0) - command |= (1 << config.command_rw_bit); - - if (config.command_parity_bit > 0) - command |= ((word)spi->spiCalcEvenParity(command) << config.command_parity_bit); //Add a parity bit on the the MSB +uint32_t SAMDMagneticSensorSPI::getRawCount(uint64_t & timestamp_us) +{ + if(tccN >= 0) + { + union { uint16_t val; struct { uint8_t lsb; uint8_t msb; }; } t; + t.msb = receiveBuffer[0]; + t.lsb = receiveBuffer[1]; + timestamp_us = last_timestamp_us; + return (uint32_t)extractResult(t.val); + } spi->transfer16(command); timestamp_us = _micros(); @@ -69,11 +103,7 @@ uint32_t SAMDMagneticSensorSPI::getRawCount(uint64_t & timestamp_us){ word register_value = spi->transfer16(command); - register_value = register_value >> (1 + config.data_start_bit - config.bit_resolution); //this should shift data to the rightmost bits of the word - - const static word data_mask = 0xFFFF >> (16 - config.bit_resolution); - - return register_value & data_mask; // Return the data, stripping the non data (e.g parity) bits + return extractResult(register_value); } @@ -108,9 +138,9 @@ void SAMDMagneticSensorSPI::initDMA() { btctrl_write.bit.STEPSIZE = DMAC_BTCTRL_STEPSIZE_X1_Val; /*!< bit: 13..15 Address Increment Step Size */ SPIdescriptors[0].DESCADDR.reg = 0; //next descriptor - SPIdescriptors[0].BTCNT.reg = SPIbufferSize; + SPIdescriptors[0].BTCNT.reg = bufferSize; SPIdescriptors[0].DSTADDR.reg = (uint32_t)&spi->sercomCfg.sercom->SPI.DATA.reg; - SPIdescriptors[0].SRCADDR.reg = ((uint32_t)&SPItransmitBuffer[0]) + SPIbufferSize; + SPIdescriptors[0].SRCADDR.reg = ((uint32_t)&transmitBuffer[0]) + bufferSize; ::initDMAChannel(dmaTX, chinset, chctrlb, SPIdescriptors[0], this); @@ -141,10 +171,10 @@ void SAMDMagneticSensorSPI::initDMA() { btctrl_read.bit.STEPSIZE = DMAC_BTCTRL_STEPSIZE_X1_Val; /*!< bit: 13..15 Address Increment Step Size */ SPIdescriptors[1].DESCADDR.reg = 0; - SPIdescriptors[1].BTCNT.reg = SPIbufferSize; - SPIdescriptors[1].DSTADDR.reg = ((uint32_t)&SPIreceiveBuffer[0]) + SPIbufferSize; + SPIdescriptors[1].BTCNT.reg = bufferSize; + SPIdescriptors[1].DSTADDR.reg = ((uint32_t)&receiveBuffer[0]) + bufferSize; SPIdescriptors[1].SRCADDR.reg = (uint32_t) &spi->sercomCfg.sercom->SPI.DATA.reg; - ::initDMAChannel(dmaTX, chinset, chctrlb, SPIdescriptors[1], this); + ::initDMAChannel(dmaRX, chinset, chctrlb, SPIdescriptors[1], this); } \ No newline at end of file diff --git a/src/sensors/hardware_specific/SAMDMagneticSensorSPI.h b/src/sensors/hardware_specific/SAMDMagneticSensorSPI.h index e6c66abd..e3274258 100644 --- a/src/sensors/hardware_specific/SAMDMagneticSensorSPI.h +++ b/src/sensors/hardware_specific/SAMDMagneticSensorSPI.h @@ -5,7 +5,6 @@ #include "../MagneticSensorSPI.h" #include "../../common/hardware_specific/samd21_AdvancedSPI.h" -#define SPIbufferSize 2 class SAMDMagneticSensorSPI: public MagneticSensor, public TccInterruptCallback, public DMACInterruptCallback{ public: /** @@ -21,19 +20,24 @@ class SAMDMagneticSensorSPI: public MagneticSensor, public TccInterruptCallback, void operator()(Tcc * tcc) override; - void operator()(volatile DMAC_CHINTFLAG_Type &, volatile DMAC_CHCTRLA_Type &) override; + void operator()(uint8_t channel, volatile DMAC_CHINTFLAG_Type &, volatile DMAC_CHCTRLA_Type &) override; private: - void tccHandler(Tcc * tcc); void initDMA(); + word makeSPICommand(); + word extractResult(word register_value); MagneticSensorSPIConfig_s config; SAMDAdvancedSPI* spi; + word command; int8_t tccN, dmaTX, dmaRX; - + bool pinLow; - uint8_t SPIreceiveBuffer[SPIbufferSize]; - uint8_t SPItransmitBuffer[SPIbufferSize]; + static const uint8_t bufferSize = 2; + uint8_t receiveBuffer[bufferSize]; + uint8_t transmitBuffer[bufferSize]; + uint64_t last_timestamp_us; + DmacDescriptor SPIdescriptors[2] __attribute__ ((aligned (16))); }; \ No newline at end of file From 61a6c05dc695ed278795dd6ba8e20f7a854ba446 Mon Sep 17 00:00:00 2001 From: maxime Date: Tue, 8 Jun 2021 10:04:48 -0600 Subject: [PATCH 12/15] New _readADCVoltagesLowSide(a,b,c) API new _SAMD21_EVSYS_ define --- .github/workflows/ccpp.yml | 2 +- .../bluepill_position_control.ino | 26 +- .../bluepill_position_control.ino | 26 +- .../full_control_serial.ino | 20 +- .../full_control_serial.ino | 20 +- .../esp32_position_control.ino | 24 +- .../esp32_position_control.ino | 20 +- .../position_control/position_control.ino | 30 +-- .../nano33IoT_velocity_control.ino | 8 +- .../single_full_control_example.ino | 18 +- .../double_full_control_example.ino | 26 +- .../single_full_control_example.ino | 16 +- .../double_full_control_example.ino | 30 +-- .../single_full_control_example.ino | 18 +- .../smartstepper_control.ino | 61 +++++ .../encoder/angle_control/angle_control.ino | 40 ++-- .../angle_control/angle_control.ino | 36 +-- .../angle_control/angle_control.ino | 26 +- .../current_control/current_control.ino | 26 +- .../velocity_control/velocity_control.ino | 36 +-- .../hall_sensor/velocity_control.ino | 36 +-- .../velocity_control/velocity_control.ino | 26 +- .../full_control_serial.ino | 16 +- .../full_control_serial.ino | 18 +- .../full_control_serial.ino | 17 +- .../osc_esp32_3pwm/osc_esp32_3pwm.ino | 10 +- .../osc_esp32_fullcontrol.ino | 18 +- .../alignment_and_cogging_test.ino | 12 +- .../find_pole_pairs_number.ino | 46 ++-- .../find_pole_pairs_number.ino | 44 ++-- .../step_dir_listener_software_interrupt.ino | 16 +- .../step_dir_motor_example.ino | 12 +- .../bldc_driver_6pwm_standalone.ino | 6 +- .../stepper_driver_2pwm_standalone.ino | 6 +- keywords.txt | 4 + library.properties | 2 +- src/BLDCMotor.cpp | 22 +- src/StepperMotor.cpp | 66 ++--- src/common/base_classes/BLDCDriver.h | 22 +- src/common/base_classes/Sensor.cpp | 8 +- src/common/defaults.h | 43 ++-- src/common/foc_utils.cpp | 30 +-- src/common/foc_utils.h | 46 ++-- src/common/pid.cpp | 13 +- src/communication/Commander.h | 120 +++++----- src/current_sense/InlineCurrentSense.cpp | 45 ++-- src/current_sense/InlineCurrentSense.h | 9 +- src/current_sense/LowsideCurrentSense.cpp | 36 ++- src/current_sense/LowsideCurrentSense.h | 27 ++- src/current_sense/hardware_api.h | 19 +- .../hardware_specific/atmega_mcu.cpp | 63 +++++ .../hardware_specific/due_mcu.cpp | 46 ++++ .../hardware_specific/esp32_adc_driver.cpp | 225 ++++++++++++++++++ .../hardware_specific/esp32_adc_driver.h | 88 +++++++ .../hardware_specific/esp32_mcu.cpp | 17 +- .../hardware_specific/generic_mcu.cpp | 71 ++---- .../hardware_specific/samd21_mcu.cpp | 38 +-- .../hardware_specific/samd21_mcu.h | 15 +- .../hardware_specific/samd_mcu.cpp | 45 ++++ .../hardware_specific/stm32_mcu.cpp | 47 ++++ .../hardware_specific/teensy_mcu.cpp | 45 ++++ src/drivers/BLDCDriver3PWM.cpp | 14 +- src/drivers/BLDCDriver6PWM.cpp | 22 +- src/drivers/StepperDriver2PWM.cpp | 26 +- src/drivers/StepperDriver2PWM.h | 8 +- src/drivers/StepperDriver4PWM.cpp | 10 +- .../hardware_specific/atmega2560_mcu.cpp | 40 ++-- .../hardware_specific/atmega328_mcu.cpp | 42 ++-- src/drivers/hardware_specific/esp32_mcu.cpp | 88 +++---- src/drivers/hardware_specific/generic_mcu.cpp | 30 ++- src/drivers/hardware_specific/rp2040_mcu.cpp | 2 +- src/drivers/hardware_specific/samd_mcu.cpp | 8 +- src/drivers/hardware_specific/stm32_mcu.cpp | 30 +-- src/drivers/hardware_specific/teensy_mcu.cpp | 26 +- src/sensors/Encoder.cpp | 10 +- src/sensors/HallSensor.cpp | 19 +- src/sensors/MagneticSensorAnalog.cpp | 18 +- src/sensors/MagneticSensorI2C.cpp | 56 ++--- src/sensors/MagneticSensorPWM.cpp | 22 +- src/sensors/MagneticSensorSPI.cpp | 63 ++--- 80 files changed, 1578 insertions(+), 960 deletions(-) create mode 100644 examples/hardware_specific_examples/Smart_Stepper/smartstepper_control/smartstepper_control.ino create mode 100644 src/current_sense/hardware_specific/atmega_mcu.cpp create mode 100644 src/current_sense/hardware_specific/due_mcu.cpp create mode 100644 src/current_sense/hardware_specific/esp32_adc_driver.cpp create mode 100644 src/current_sense/hardware_specific/esp32_adc_driver.h create mode 100644 src/current_sense/hardware_specific/samd_mcu.cpp create mode 100644 src/current_sense/hardware_specific/stm32_mcu.cpp create mode 100644 src/current_sense/hardware_specific/teensy_mcu.cpp diff --git a/.github/workflows/ccpp.yml b/.github/workflows/ccpp.yml index 2377cd61..9eea4f03 100644 --- a/.github/workflows/ccpp.yml +++ b/.github/workflows/ccpp.yml @@ -11,4 +11,4 @@ jobs: uses: ArminJo/arduino-test-compile@v1.0.0 with: libraries: PciManager - examples-exclude: bluepill_position_control, esp32_position_control, esp32_i2c_dual_bus_example, stm32_i2c_dual_bus_example, magnetic_sensor_spi_alt_example, osc_esp32_3pwm, osc_esp32_fullcontrol, nano33IoT_velocity_control + examples-exclude: bluepill_position_control, esp32_position_control, esp32_i2c_dual_bus_example, stm32_i2c_dual_bus_example, magnetic_sensor_spi_alt_example, osc_esp32_3pwm, osc_esp32_fullcontrol, nano33IoT_velocity_control, smartstepper_control diff --git a/examples/hardware_specific_examples/Bluepill_examples/encoder/bluepill_position_control/bluepill_position_control.ino b/examples/hardware_specific_examples/Bluepill_examples/encoder/bluepill_position_control/bluepill_position_control.ino index 267c00fb..d0768fd2 100644 --- a/examples/hardware_specific_examples/Bluepill_examples/encoder/bluepill_position_control/bluepill_position_control.ino +++ b/examples/hardware_specific_examples/Bluepill_examples/encoder/bluepill_position_control/bluepill_position_control.ino @@ -1,9 +1,9 @@ /** - * + * * STM32 Bluepill position motion control example with encoder - * + * * The same example can be ran with any STM32 board - just make sure that put right pin numbers. - * + * */ #include @@ -32,10 +32,10 @@ void doTarget(char* cmd) { command.scalar(&target_angle, cmd); } void setup() { - + // initialize encoder sensor hardware encoder.init(); - encoder.enableInterrupts(doA, doB, doI); + encoder.enableInterrupts(doA, doB, doI); // link the motor to the sensor motor.linkSensor(&encoder); @@ -54,20 +54,20 @@ void setup() { // set motion control loop to be used motor.controller = MotionControlType::velocity; - // contoller configuration + // contoller configuration // default parameters in defaults.h // velocity PI controller parameters - motor.PID_velocity.P = 0.2; + motor.PID_velocity.P = 0.2f; motor.PID_velocity.I = 20; // default voltage_power_supply motor.voltage_limit = 6; // jerk control using voltage voltage ramp // default value is 300 volts per sec ~ 0.3V per millisecond motor.PID_velocity.output_ramp = 1000; - + // velocity low pass filtering time constant - motor.LPF_velocity.Tf = 0.01; + motor.LPF_velocity.Tf = 0.01f; // angle P controller motor.P_angle.P = 20; @@ -75,11 +75,11 @@ void setup() { motor.velocity_limit = 4; - // use monitoring with serial + // use monitoring with serial Serial.begin(115200); // comment out if not needed motor.useMonitoring(Serial); - + // initialize motor motor.init(); // align encoder and start FOC @@ -97,7 +97,7 @@ void loop() { // main FOC algorithm function // the faster you run this function the better // Arduino UNO loop ~1kHz - // Bluepill loop ~10kHz + // Bluepill loop ~10kHz motor.loopFOC(); // Motion control function @@ -109,7 +109,7 @@ void loop() { // function intended to be used with serial plotter to monitor motor variables // significantly slowing the execution down!!!! // motor.monitor(); - + // user communication command.run(); } \ No newline at end of file diff --git a/examples/hardware_specific_examples/Bluepill_examples/magnetic_sensor/bluepill_position_control/bluepill_position_control.ino b/examples/hardware_specific_examples/Bluepill_examples/magnetic_sensor/bluepill_position_control/bluepill_position_control.ino index 0adf4c17..1b47170d 100644 --- a/examples/hardware_specific_examples/Bluepill_examples/magnetic_sensor/bluepill_position_control/bluepill_position_control.ino +++ b/examples/hardware_specific_examples/Bluepill_examples/magnetic_sensor/bluepill_position_control/bluepill_position_control.ino @@ -1,9 +1,9 @@ /** - * + * * STM32 Bluepill position motion control example with magnetic sensor - * + * * The same example can be ran with any STM32 board - just make sure that put right pin numbers. - * + * */ #include @@ -47,37 +47,37 @@ void setup() { driver.init(); // link the motor and the driver motor.linkDriver(&driver); - + // choose FOC modulation (optional) motor.foc_modulation = FOCModulationType::SpaceVectorPWM; // set motion control loop to be used motor.controller = MotionControlType::angle; - // contoller configuration + // contoller configuration // default parameters in defaults.h // velocity PI controller parameters - motor.PID_velocity.P = 0.2; + motor.PID_velocity.P = 0.2f; motor.PID_velocity.I = 20; // maximal voltage to be set to the motor motor.voltage_limit = 6; - + // velocity low pass filtering time constant // the lower the less filtered - motor.LPF_velocity.Tf = 0.01; + motor.LPF_velocity.Tf = 0.01f; - // angle P controller + // angle P controller motor.P_angle.P = 20; // maximal velocity of the position control motor.velocity_limit = 40; - // use monitoring with serial + // use monitoring with serial Serial.begin(115200); // comment out if not needed motor.useMonitoring(Serial); - + // initialize motor motor.init(); // align sensor and start FOC @@ -99,7 +99,7 @@ void loop() { // main FOC algorithm function // the faster you run this function the better // Arduino UNO loop ~1kHz - // Bluepill loop ~10kHz + // Bluepill loop ~10kHz motor.loopFOC(); // Motion control function @@ -112,7 +112,7 @@ void loop() { // function intended to be used with serial plotter to monitor motor variables // significantly slowing the execution down!!!! // motor.monitor(); - + // user communication command.run(); } \ No newline at end of file diff --git a/examples/hardware_specific_examples/DRV8302_driver/3pwm_example/encoder/full_control_serial/full_control_serial.ino b/examples/hardware_specific_examples/DRV8302_driver/3pwm_example/encoder/full_control_serial/full_control_serial.ino index 746dde3d..842f383a 100644 --- a/examples/hardware_specific_examples/DRV8302_driver/3pwm_example/encoder/full_control_serial/full_control_serial.ino +++ b/examples/hardware_specific_examples/DRV8302_driver/3pwm_example/encoder/full_control_serial/full_control_serial.ino @@ -1,15 +1,15 @@ /** * Comprehensive BLDC motor control example using encoder and the DRV8302 board - * + * * Using serial terminal user can send motor commands and configure the motor and FOC in real-time: * - configure PID controller constants * - change motion control loops * - monitor motor variabels * - set target values - * - check all the configuration values - * + * - check all the configuration values + * * check the https://docs.simplefoc.com for full list of motor commands - * + * */ #include @@ -20,7 +20,7 @@ #define INH_C 11 #define EN_GATE 7 -#define M_PWM A1 +#define M_PWM A1 #define M_OC A2 #define OC_ADJ A3 @@ -45,7 +45,7 @@ void setup() { // initialize encoder sensor hardware encoder.init(); - encoder.enableInterrupts(doA, doB); + encoder.enableInterrupts(doA, doB); // link the motor to the sensor motor.linkSensor(&encoder); @@ -76,14 +76,14 @@ void setup() { // set control loop type to be used motor.controller = MotionControlType::torque; - // contoller configuration based on the controll type - motor.PID_velocity.P = 0.2; + // contoller configuration based on the controll type + motor.PID_velocity.P = 0.2f; motor.PID_velocity.I = 20; // default voltage_power_supply motor.voltage_limit = 12; // velocity low pass filtering time constant - motor.LPF_velocity.Tf = 0.01; + motor.LPF_velocity.Tf = 0.01f; // angle loop controller motor.P_angle.P = 20; @@ -111,7 +111,7 @@ void setup() { Serial.println(F("Run user commands to configure and the motor (find the full command list in docs.simplefoc.com) \n ")); Serial.println(F("Initial motion control loop is voltage loop.")); Serial.println(F("Initial target voltage 2V.")); - + _delay(1000); } diff --git a/examples/hardware_specific_examples/DRV8302_driver/6pwm_example/encoder/full_control_serial/full_control_serial.ino b/examples/hardware_specific_examples/DRV8302_driver/6pwm_example/encoder/full_control_serial/full_control_serial.ino index 3e936460..6c75855a 100644 --- a/examples/hardware_specific_examples/DRV8302_driver/6pwm_example/encoder/full_control_serial/full_control_serial.ino +++ b/examples/hardware_specific_examples/DRV8302_driver/6pwm_example/encoder/full_control_serial/full_control_serial.ino @@ -1,15 +1,15 @@ /** * Comprehensive BLDC motor control example using encoder and the DRV8302 board - * + * * Using serial terminal user can send motor commands and configure the motor and FOC in real-time: * - configure PID controller constants * - change motion control loops * - monitor motor variabels * - set target values - * - check all the configuration values - * + * - check all the configuration values + * * check the https://docs.simplefoc.com for full list of motor commands - * + * */ #include @@ -23,7 +23,7 @@ #define INL_C 10 #define EN_GATE 7 -#define M_PWM A1 +#define M_PWM A1 #define M_OC A2 #define OC_ADJ A3 @@ -48,7 +48,7 @@ void setup() { // initialize encoder sensor hardware encoder.init(); - encoder.enableInterrupts(doA, doB); + encoder.enableInterrupts(doA, doB); // link the motor to the sensor motor.linkSensor(&encoder); @@ -77,14 +77,14 @@ void setup() { // set control loop type to be used motor.controller = MotionControlType::torque; - // contoller configuration based on the controll type - motor.PID_velocity.P = 0.2; + // contoller configuration based on the controll type + motor.PID_velocity.P = 0.2f; motor.PID_velocity.I = 20; // default voltage_power_supply motor.voltage_limit = 12; // velocity low pass filtering time constant - motor.LPF_velocity.Tf = 0.01; + motor.LPF_velocity.Tf = 0.01f; // angle loop controller motor.P_angle.P = 20; @@ -112,7 +112,7 @@ void setup() { Serial.println(F("Run user commands to configure and the motor (find the full command list in docs.simplefoc.com) \n ")); Serial.println(F("Initial motion control loop is voltage loop.")); Serial.println(F("Initial target voltage 2V.")); - + _delay(1000); } diff --git a/examples/hardware_specific_examples/ESP32/encoder/esp32_position_control/esp32_position_control.ino b/examples/hardware_specific_examples/ESP32/encoder/esp32_position_control/esp32_position_control.ino index 9864ca0b..3943017b 100644 --- a/examples/hardware_specific_examples/ESP32/encoder/esp32_position_control/esp32_position_control.ino +++ b/examples/hardware_specific_examples/ESP32/encoder/esp32_position_control/esp32_position_control.ino @@ -1,4 +1,4 @@ -/** +/** * ESP32 position motion control example with encoder * */ @@ -23,14 +23,14 @@ Commander command = Commander(Serial); void doTarget(char* cmd) { command.scalar(&target_angle, cmd); } void setup() { - + // initialize encoder sensor hardware encoder.init(); - encoder.enableInterrupts(doA, doB); + encoder.enableInterrupts(doA, doB); // link the motor to the sensor motor.linkSensor(&encoder); - + // driver config // power supply voltage [V] driver.voltage_power_supply = 12; @@ -46,20 +46,20 @@ void setup() { // set motion control loop to be used motor.controller = MotionControlType::velocity; - // contoller configuration + // contoller configuration // default parameters in defaults.h // velocity PI controller parameters - motor.PID_velocity.P = 0.2; + motor.PID_velocity.P = 0.2f; motor.PID_velocity.I = 20; // default voltage_power_supply motor.voltage_limit = 6; // jerk control using voltage voltage ramp // default value is 300 volts per sec ~ 0.3V per millisecond motor.PID_velocity.output_ramp = 1000; - + // velocity low pass filtering time constant - motor.LPF_velocity.Tf = 0.01; + motor.LPF_velocity.Tf = 0.01f; // angle P controller motor.P_angle.P = 20; @@ -67,11 +67,11 @@ void setup() { motor.velocity_limit = 4; - // use monitoring with serial + // use monitoring with serial Serial.begin(115200); // comment out if not needed motor.useMonitoring(Serial); - + // initialize motor motor.init(); // align encoder and start FOC @@ -92,7 +92,7 @@ void loop() { // main FOC algorithm function // the faster you run this function the better // Arduino UNO loop ~1kHz - // Bluepill loop ~10kHz + // Bluepill loop ~10kHz motor.loopFOC(); // Motion control function @@ -104,7 +104,7 @@ void loop() { // function intended to be used with serial plotter to monitor motor variables // significantly slowing the execution down!!!! // motor.monitor(); - + // user communication command.run(); } \ No newline at end of file diff --git a/examples/hardware_specific_examples/ESP32/magnetic_sensor/esp32_position_control/esp32_position_control.ino b/examples/hardware_specific_examples/ESP32/magnetic_sensor/esp32_position_control/esp32_position_control.ino index 4bd53b55..77cfeeed 100644 --- a/examples/hardware_specific_examples/ESP32/magnetic_sensor/esp32_position_control/esp32_position_control.ino +++ b/examples/hardware_specific_examples/ESP32/magnetic_sensor/esp32_position_control/esp32_position_control.ino @@ -44,37 +44,37 @@ void setup() { driver.init(); // link the motor and the driver motor.linkDriver(&driver); - + // choose FOC modulation (optional) motor.foc_modulation = FOCModulationType::SpaceVectorPWM; // set motion control loop to be used motor.controller = MotionControlType::angle; - // contoller configuration + // contoller configuration // default parameters in defaults.h // velocity PI controller parameters - motor.PID_velocity.P = 0.2; + motor.PID_velocity.P = 0.2f; motor.PID_velocity.I = 20; // maximal voltage to be set to the motor motor.voltage_limit = 6; - + // velocity low pass filtering time constant // the lower the less filtered - motor.LPF_velocity.Tf = 0.01; + motor.LPF_velocity.Tf = 0.01f; - // angle P controller + // angle P controller motor.P_angle.P = 20; // maximal velocity of the position control motor.velocity_limit = 40; - // use monitoring with serial + // use monitoring with serial Serial.begin(115200); // comment out if not needed motor.useMonitoring(Serial); - + // initialize motor motor.init(); // align sensor and start FOC @@ -96,7 +96,7 @@ void loop() { // main FOC algorithm function // the faster you run this function the better // Arduino UNO loop ~1kHz - // Bluepill loop ~10kHz + // Bluepill loop ~10kHz motor.loopFOC(); // Motion control function @@ -109,7 +109,7 @@ void loop() { // function intended to be used with serial plotter to monitor motor variables // significantly slowing the execution down!!!! // motor.monitor(); - + // user communication command.run(); } \ No newline at end of file diff --git a/examples/hardware_specific_examples/HMBGC_example/position_control/position_control.ino b/examples/hardware_specific_examples/HMBGC_example/position_control/position_control.ino index e9355f65..5b5c8e40 100644 --- a/examples/hardware_specific_examples/HMBGC_example/position_control/position_control.ino +++ b/examples/hardware_specific_examples/HMBGC_example/position_control/position_control.ino @@ -1,15 +1,15 @@ /** - * + * * HMBGC position motion control example with encoder - * + * * - Motor is connected the MOT1 connector (MOT1 9,10,11; MOT2 3,5,6) * - Encoder is connected to A0 and A1 - * - * This board doesn't have any interrupt pins so we need to run all the encoder channels with the software interrupt library + * + * This board doesn't have any interrupt pins so we need to run all the encoder channels with the software interrupt library * - For this example we use: PciManager library : https://github.com/prampec/arduino-pcimanager - * + * * See docs.simplefoc.com for more info. - * + * */ #include // software interrupt library @@ -49,7 +49,7 @@ void setup() { PciManager.registerListener(&listenerB); // link the motor to the sensor motor.linkSensor(&encoder); - + // driver config // power supply voltage [V] driver.voltage_power_supply = 12; @@ -65,20 +65,20 @@ void setup() { // set motion control loop to be used motor.controller = MotionControlType::angle; - // contoller configuration + // contoller configuration // default parameters in defaults.h // velocity PI controller parameters - motor.PID_velocity.P = 0.2; + motor.PID_velocity.P = 0.2f; motor.PID_velocity.I = 20; // default voltage_power_supply motor.voltage_limit = 6; // jerk control using voltage voltage ramp // default value is 300 volts per sec ~ 0.3V per millisecond motor.PID_velocity.output_ramp = 1000; - + // velocity low pass filtering time constant - motor.LPF_velocity.Tf = 0.01; + motor.LPF_velocity.Tf = 0.01f; // angle P controller motor.P_angle.P = 20; @@ -86,11 +86,11 @@ void setup() { motor.velocity_limit = 4; - // use monitoring with serial + // use monitoring with serial Serial.begin(115200); // comment out if not needed motor.useMonitoring(Serial); - + // initialize motor motor.init(); // align encoder and start FOC @@ -109,7 +109,7 @@ void loop() { // main FOC algorithm function // the faster you run this function the better // Arduino UNO loop ~1kHz - // Bluepill loop ~10kHz + // Bluepill loop ~10kHz motor.loopFOC(); // Motion control function @@ -121,7 +121,7 @@ void loop() { // function intended to be used with serial plotter to monitor motor variables // significantly slowing the execution down!!!! // motor.monitor(); - + // user communication command.run(); } \ No newline at end of file diff --git a/examples/hardware_specific_examples/SAMD_examples/nano33IoT/nano33IoT_velocity_control/nano33IoT_velocity_control.ino b/examples/hardware_specific_examples/SAMD_examples/nano33IoT/nano33IoT_velocity_control/nano33IoT_velocity_control.ino index fc46a338..2c9baaef 100644 --- a/examples/hardware_specific_examples/SAMD_examples/nano33IoT/nano33IoT_velocity_control/nano33IoT_velocity_control.ino +++ b/examples/hardware_specific_examples/SAMD_examples/nano33IoT/nano33IoT_velocity_control/nano33IoT_velocity_control.ino @@ -17,7 +17,7 @@ BLDCMotor motor = BLDCMotor(7); BLDCDriver3PWM driver = BLDCDriver3PWM(6,5,8); // velocity set point variable -float target_velocity = 2.0; +float target_velocity = 2.0f; // instantiate the commander Commander command = Commander(SerialUSB); void doTarget(char* cmd) { command.scalar(&target_velocity, cmd); } @@ -35,11 +35,11 @@ void setup() { driver.init(); motor.linkDriver(&driver); motor.controller = MotionControlType::velocity; - motor.PID_velocity.P = 0.2; + motor.PID_velocity.P = 0.2f; motor.PID_velocity.I = 20; - motor.PID_velocity.D = 0.001; + motor.PID_velocity.D = 0.001f; motor.PID_velocity.output_ramp = 1000; - motor.LPF_velocity.Tf = 0.01; + motor.LPF_velocity.Tf = 0.01f; motor.voltage_limit = 9; //motor.P_angle.P = 20; motor.init(); diff --git a/examples/hardware_specific_examples/SimpleFOC-PowerShield/version_v02/single_full_control_example/single_full_control_example.ino b/examples/hardware_specific_examples/SimpleFOC-PowerShield/version_v02/single_full_control_example/single_full_control_example.ino index 16c7755b..0b3075d3 100644 --- a/examples/hardware_specific_examples/SimpleFOC-PowerShield/version_v02/single_full_control_example/single_full_control_example.ino +++ b/examples/hardware_specific_examples/SimpleFOC-PowerShield/version_v02/single_full_control_example/single_full_control_example.ino @@ -22,7 +22,7 @@ void setup() { // initialize encoder sensor hardware encoder.init(); - encoder.enableInterrupts(doA, doB); + encoder.enableInterrupts(doA, doB); // link the motor to the sensor motor.linkSensor(&encoder); @@ -38,15 +38,15 @@ void setup() { motor.torque_controller = TorqueControlType::foc_current; motor.controller = MotionControlType::torque; - // contoller configuration based on the controll type - motor.PID_velocity.P = 0.05; + // contoller configuration based on the controll type + motor.PID_velocity.P = 0.05f; motor.PID_velocity.I = 1; motor.PID_velocity.D = 0; // default voltage_power_supply motor.voltage_limit = 12; - + // velocity low pass filtering time constant - motor.LPF_velocity.Tf = 0.01; + motor.LPF_velocity.Tf = 0.01f; // angle loop controller motor.P_angle.P = 20; @@ -60,7 +60,7 @@ void setup() { motor.useMonitoring(Serial); motor.monitor_downsample = 0; // disable intially motor.monitor_variables = _MON_TARGET | _MON_VEL | _MON_ANGLE; // monitor target velocity and angle - + // current sense init and linking current_sense.init(); motor.linkCurrentSense(¤t_sense); @@ -68,7 +68,7 @@ void setup() { // initialise motor motor.init(); // align encoder and start FOC - motor.initFOC(); + motor.initFOC(); // set the inital target value motor.target = 0; @@ -76,9 +76,9 @@ void setup() { // subscribe motor to the commander command.add('M', doMotor, "motor"); - // Run user commands to configure and the motor (find the full command list in docs.simplefoc.com) + // Run user commands to configure and the motor (find the full command list in docs.simplefoc.com) Serial.println(F("Motor commands sketch | Initial motion control > torque/current : target 0Amps.")); - + _delay(1000); } diff --git a/examples/hardware_specific_examples/SimpleFOCShield/version_v1/double_full_control_example/double_full_control_example.ino b/examples/hardware_specific_examples/SimpleFOCShield/version_v1/double_full_control_example/double_full_control_example.ino index e9f4e840..c048956b 100644 --- a/examples/hardware_specific_examples/SimpleFOCShield/version_v1/double_full_control_example/double_full_control_example.ino +++ b/examples/hardware_specific_examples/SimpleFOCShield/version_v1/double_full_control_example/double_full_control_example.ino @@ -4,7 +4,7 @@ // BLDC motor & driver instance BLDCMotor motor1 = BLDCMotor(11); BLDCDriver3PWM driver1 = BLDCDriver3PWM(9, 5, 6, 8); - + // BLDC motor & driver instance BLDCMotor motor2 = BLDCMotor(11); BLDCDriver3PWM driver2 = BLDCDriver3PWM(3, 10, 11, 7); @@ -30,14 +30,14 @@ void setup() { // initialize encoder sensor hardware encoder1.init(); - encoder1.enableInterrupts(doA1, doB1); + encoder1.enableInterrupts(doA1, doB1); // initialize encoder sensor hardware encoder2.init(); - encoder2.enableInterrupts(doA2, doB2); + encoder2.enableInterrupts(doA2, doB2); // link the motor to the sensor motor1.linkSensor(&encoder1); motor2.linkSensor(&encoder2); - + // driver config // power supply voltage [V] @@ -55,14 +55,14 @@ void setup() { motor1.controller = MotionControlType::torque; motor2.controller = MotionControlType::torque; - // contoller configuration based on the controll type - motor1.PID_velocity.P = 0.05; + // contoller configuration based on the controll type + motor1.PID_velocity.P = 0.05f; motor1.PID_velocity.I = 1; motor1.PID_velocity.D = 0; // default voltage_power_supply motor1.voltage_limit = 12; - // contoller configuration based on the controll type - motor2.PID_velocity.P = 0.05; + // contoller configuration based on the controll type + motor2.PID_velocity.P = 0.05f; motor2.PID_velocity.I = 1; motor2.PID_velocity.D = 0; // default voltage_power_supply @@ -78,16 +78,16 @@ void setup() { // comment out if not needed motor1.useMonitoring(Serial); motor2.useMonitoring(Serial); - + // initialise motor motor1.init(); // align encoder and start FOC - motor1.initFOC(); - + motor1.initFOC(); + // initialise motor motor2.init(); // align encoder and start FOC - motor2.initFOC(); + motor2.initFOC(); // set the inital target value motor1.target = 2; @@ -99,7 +99,7 @@ void setup() { // Run user commands to configure and the motor (find the full command list in docs.simplefoc.com) Serial.println(F("Double motor sketch ready.")); - + _delay(1000); } diff --git a/examples/hardware_specific_examples/SimpleFOCShield/version_v1/single_full_control_example/single_full_control_example.ino b/examples/hardware_specific_examples/SimpleFOCShield/version_v1/single_full_control_example/single_full_control_example.ino index 5d02ecf2..10e26c9f 100644 --- a/examples/hardware_specific_examples/SimpleFOCShield/version_v1/single_full_control_example/single_full_control_example.ino +++ b/examples/hardware_specific_examples/SimpleFOCShield/version_v1/single_full_control_example/single_full_control_example.ino @@ -19,7 +19,7 @@ void setup() { // initialize encoder sensor hardware encoder.init(); - encoder.enableInterrupts(doA, doB); + encoder.enableInterrupts(doA, doB); // link the motor to the sensor motor.linkSensor(&encoder); @@ -33,15 +33,15 @@ void setup() { // set control loop type to be used motor.controller = MotionControlType::torque; - // contoller configuration based on the controll type - motor.PID_velocity.P = 0.05; + // contoller configuration based on the controll type + motor.PID_velocity.P = 0.05f; motor.PID_velocity.I = 1; motor.PID_velocity.D = 0; // default voltage_power_supply motor.voltage_limit = 12; - + // velocity low pass filtering time constant - motor.LPF_velocity.Tf = 0.01; + motor.LPF_velocity.Tf = 0.01f; // angle loop controller motor.P_angle.P = 20; @@ -55,11 +55,11 @@ void setup() { motor.useMonitoring(Serial); motor.monitor_downsample = 0; // disable intially motor.monitor_variables = _MON_TARGET | _MON_VEL | _MON_ANGLE; // monitor target velocity and angle - + // initialise motor motor.init(); // align encoder and start FOC - motor.initFOC(); + motor.initFOC(); // set the inital target value motor.target = 2; @@ -69,7 +69,7 @@ void setup() { // Run user commands to configure and the motor (find the full command list in docs.simplefoc.com) Serial.println(F("Motor commands sketch | Initial motion control > torque/voltage : target 2V.")); - + _delay(1000); } diff --git a/examples/hardware_specific_examples/SimpleFOCShield/version_v2/double_full_control_example/double_full_control_example.ino b/examples/hardware_specific_examples/SimpleFOCShield/version_v2/double_full_control_example/double_full_control_example.ino index 757ae0f1..3399d2fa 100644 --- a/examples/hardware_specific_examples/SimpleFOCShield/version_v2/double_full_control_example/double_full_control_example.ino +++ b/examples/hardware_specific_examples/SimpleFOCShield/version_v2/double_full_control_example/double_full_control_example.ino @@ -4,7 +4,7 @@ // BLDC motor & driver instance BLDCMotor motor1 = BLDCMotor(11); BLDCDriver3PWM driver1 = BLDCDriver3PWM(5, 10, 6, 8); - + // BLDC motor & driver instance BLDCMotor motor2 = BLDCMotor(11); BLDCDriver3PWM driver2 = BLDCDriver3PWM(3, 9, 11, 7); @@ -39,14 +39,14 @@ void setup() { // initialize encoder sensor hardware encoder1.init(); - encoder1.enableInterrupts(doA1, doB1); + encoder1.enableInterrupts(doA1, doB1); // initialize encoder sensor hardware encoder2.init(); - encoder2.enableInterrupts(doA2, doB2); + encoder2.enableInterrupts(doA2, doB2); // link the motor to the sensor motor1.linkSensor(&encoder1); motor2.linkSensor(&encoder2); - + // driver config // power supply voltage [V] @@ -64,14 +64,14 @@ void setup() { motor1.controller = MotionControlType::torque; motor2.controller = MotionControlType::torque; - // contoller configuration based on the controll type - motor1.PID_velocity.P = 0.05; + // contoller configuration based on the controll type + motor1.PID_velocity.P = 0.05f; motor1.PID_velocity.I = 1; motor1.PID_velocity.D = 0; // default voltage_power_supply motor1.voltage_limit = 12; - // contoller configuration based on the controll type - motor2.PID_velocity.P = 0.05; + // contoller configuration based on the controll type + motor2.PID_velocity.P = 0.05f; motor2.PID_velocity.I = 1; motor2.PID_velocity.D = 0; // default voltage_power_supply @@ -87,8 +87,8 @@ void setup() { // comment out if not needed motor1.useMonitoring(Serial); motor2.useMonitoring(Serial); - - + + // current sense init and linking current_sense1.init(); motor1.linkCurrentSense(¤t_sense1); @@ -99,12 +99,12 @@ void setup() { // initialise motor motor1.init(); // align encoder and start FOC - motor1.initFOC(); - + motor1.initFOC(); + // initialise motor motor2.init(); // align encoder and start FOC - motor2.initFOC(); + motor2.initFOC(); // set the inital target value motor1.target = 2; @@ -116,7 +116,7 @@ void setup() { // Run user commands to configure and the motor (find the full command list in docs.simplefoc.com) Serial.println(F("Double motor sketch ready.")); - + _delay(1000); } @@ -132,4 +132,4 @@ void loop() { // user communication command.run(); -} \ No newline at end of file +} diff --git a/examples/hardware_specific_examples/SimpleFOCShield/version_v2/single_full_control_example/single_full_control_example.ino b/examples/hardware_specific_examples/SimpleFOCShield/version_v2/single_full_control_example/single_full_control_example.ino index c0f6a10b..67d1d869 100644 --- a/examples/hardware_specific_examples/SimpleFOCShield/version_v2/single_full_control_example/single_full_control_example.ino +++ b/examples/hardware_specific_examples/SimpleFOCShield/version_v2/single_full_control_example/single_full_control_example.ino @@ -23,7 +23,7 @@ void setup() { // initialize encoder sensor hardware encoder.init(); - encoder.enableInterrupts(doA, doB); + encoder.enableInterrupts(doA, doB); // link the motor to the sensor motor.linkSensor(&encoder); @@ -37,15 +37,15 @@ void setup() { // set control loop type to be used motor.controller = MotionControlType::torque; - // contoller configuration based on the controll type - motor.PID_velocity.P = 0.05; + // contoller configuration based on the controll type + motor.PID_velocity.P = 0.05f; motor.PID_velocity.I = 1; motor.PID_velocity.D = 0; // default voltage_power_supply motor.voltage_limit = 12; - + // velocity low pass filtering time constant - motor.LPF_velocity.Tf = 0.01; + motor.LPF_velocity.Tf = 0.01f; // angle loop controller motor.P_angle.P = 20; @@ -59,7 +59,7 @@ void setup() { motor.useMonitoring(Serial); motor.monitor_downsample = 0; // disable intially motor.monitor_variables = _MON_TARGET | _MON_VEL | _MON_ANGLE; // monitor target velocity and angle - + // current sense init and linking current_sense.init(); motor.linkCurrentSense(¤t_sense); @@ -67,7 +67,7 @@ void setup() { // initialise motor motor.init(); // align encoder and start FOC - motor.initFOC(); + motor.initFOC(); // set the inital target value motor.target = 2; @@ -75,9 +75,9 @@ void setup() { // subscribe motor to the commander command.add('M', doMotor, "motor"); - // Run user commands to configure and the motor (find the full command list in docs.simplefoc.com) + // Run user commands to configure and the motor (find the full command list in docs.simplefoc.com) Serial.println(F("Motor commands sketch | Initial motion control > torque/voltage : target 2V.")); - + _delay(1000); } diff --git a/examples/hardware_specific_examples/Smart_Stepper/smartstepper_control/smartstepper_control.ino b/examples/hardware_specific_examples/Smart_Stepper/smartstepper_control/smartstepper_control.ino new file mode 100644 index 00000000..80f2fe64 --- /dev/null +++ b/examples/hardware_specific_examples/Smart_Stepper/smartstepper_control/smartstepper_control.ino @@ -0,0 +1,61 @@ +/** + * Smart Stepper support with SimpleFOClibrary + */ +#include + +// magnetic sensor instance - SPI +MagneticSensorSPI sensor = MagneticSensorSPI(AS5147_SPI, A2); + +// Stepper motor & driver instance +StepperMotor motor = StepperMotor(50); +int in1[2] = {5, 6}; +int in2[2] = {A4, 7}; +StepperDriver2PWM driver = StepperDriver2PWM(4, in1, 9, in2); + +// instantiate the commander +Commander command = Commander(SerialUSB); +void doMotor(char* cmd) { command.motor(&motor, cmd); } + +void setup() { + + // initialise magnetic sensor hardware + sensor.init(); + // link the motor to the sensor + motor.linkSensor(&sensor); + + // power supply voltage + driver.voltage_power_supply = 12; + driver.init(); + motor.linkDriver(&driver); + + // set motion control loop to be used + motor.controller = MotionControlType::torque; + + // use monitoring with SerialUSB + SerialUSB.begin(115200); + // comment out if not needed + motor.useMonitoring(SerialUSB); + + // initialize motor + motor.init(); + // align sensor and start FOC + motor.initFOC(); + + // add target command M + command.add('M', doMotor, "my motor"); + + SerialUSB.println(F("Motor ready.")); + SerialUSB.println(F("Set the target voltage using Serial terminal:")); + _delay(1000); +} + +void loop() { + // main FOC algorithm function + motor.loopFOC(); + + // Motion control function + motor.move(); + + // user communication + command.run(); +} \ No newline at end of file diff --git a/examples/motion_control/position_motion_control/encoder/angle_control/angle_control.ino b/examples/motion_control/position_motion_control/encoder/angle_control/angle_control.ino index a8e8b997..942f73dc 100644 --- a/examples/motion_control/position_motion_control/encoder/angle_control/angle_control.ino +++ b/examples/motion_control/position_motion_control/encoder/angle_control/angle_control.ino @@ -1,27 +1,27 @@ /** - * + * * Position/angle motion control example * Steps: - * 1) Configure the motor and encoder + * 1) Configure the motor and encoder * 2) Run the code * 3) Set the target angle (in radians) from serial terminal - * - * + * + * * NOTE : * > Arduino UNO example code for running velocity motion control using an encoder with index significantly * > Since Arduino UNO doesn't have enough interrupt pins we have to use software interrupt library PciManager. - * - * > If running this code with Nucleo or Bluepill or any other board which has more than 2 interrupt pins + * + * > If running this code with Nucleo or Bluepill or any other board which has more than 2 interrupt pins * > you can supply doIndex directly to the encoder.enableInterrupts(doA,doB,doIndex) and avoid using PciManger - * + * * > If you don't want to use index pin initialize the encoder class without index pin number: * > For example: * > - Encoder encoder = Encoder(2, 3, 8192); * > and initialize interrupts like this: * > - encoder.enableInterrupts(doA,doB) - * + * * Check the docs.simplefoc.com for more info about the possible encoder configuration. - * + * */ #include // software interrupt library @@ -53,10 +53,10 @@ Commander command = Commander(Serial); void doTarget(char* cmd) { command.scalar(&target_angle, cmd); } void setup() { - + // initialize encoder sensor hardware encoder.init(); - encoder.enableInterrupts(doA, doB); + encoder.enableInterrupts(doA, doB); // software interrupts PciManager.registerListener(&listenerIndex); // link the motor to the sensor @@ -68,7 +68,7 @@ void setup() { driver.init(); // link the motor and the driver motor.linkDriver(&driver); - + // aligning voltage [V] motor.voltage_sensor_align = 3; // index search velocity [rad/s] @@ -77,11 +77,11 @@ void setup() { // set motion control loop to be used motor.controller = MotionControlType::angle; - // contoller configuration + // contoller configuration // default parameters in defaults.h // velocity PI controller parameters - motor.PID_velocity.P = 0.2; + motor.PID_velocity.P = 0.2f; motor.PID_velocity.I = 20; motor.PID_velocity.D = 0; // default voltage_power_supply @@ -89,9 +89,9 @@ void setup() { // jerk control using voltage voltage ramp // default value is 300 volts per sec ~ 0.3V per millisecond motor.PID_velocity.output_ramp = 1000; - + // velocity low pass filtering time constant - motor.LPF_velocity.Tf = 0.01; + motor.LPF_velocity.Tf = 0.01f; // angle P controller motor.P_angle.P = 20; @@ -99,11 +99,11 @@ void setup() { motor.velocity_limit = 4; - // use monitoring with serial + // use monitoring with serial Serial.begin(115200); // comment out if not needed motor.useMonitoring(Serial); - + // initialize motor motor.init(); // align encoder and start FOC @@ -121,7 +121,7 @@ void loop() { // main FOC algorithm function // the faster you run this function the better // Arduino UNO loop ~1kHz - // Bluepill loop ~10kHz + // Bluepill loop ~10kHz motor.loopFOC(); // Motion control function @@ -133,7 +133,7 @@ void loop() { // function intended to be used with serial plotter to monitor motor variables // significantly slowing the execution down!!!! // motor.monitor(); - + // user communication command.run(); } \ No newline at end of file diff --git a/examples/motion_control/position_motion_control/hall_sensor/angle_control/angle_control.ino b/examples/motion_control/position_motion_control/hall_sensor/angle_control/angle_control.ino index 90219b5b..4bd7163f 100644 --- a/examples/motion_control/position_motion_control/hall_sensor/angle_control/angle_control.ino +++ b/examples/motion_control/position_motion_control/hall_sensor/angle_control/angle_control.ino @@ -1,20 +1,20 @@ /** - * + * * Position/angle motion control example * Steps: - * 1) Configure the motor and hall sensor + * 1) Configure the motor and hall sensor * 2) Run the code * 3) Set the target angle (in radians) from serial terminal - * - * + * + * * NOTE : * > This is for Arduino UNO example code for running angle motion control specifically * > Since Arduino UNO doesn't have enough interrupt pins we have to use software interrupt library PciManager. - * - * > If running this code with Nucleo or Bluepill or any other board which has more than 2 interrupt pins + * + * > If running this code with Nucleo or Bluepill or any other board which has more than 2 interrupt pins * > you can supply doIndex directly to the sensr.enableInterrupts(doA,doB,doC) and avoid using PciManger - * - * + * + * */ #include // software interrupt library @@ -46,10 +46,10 @@ Commander command = Commander(Serial); void doTarget(char* cmd) { command.scalar(&target_angle, cmd); } void setup() { - + // initialize sensor hardware sensor.init(); - sensor.enableInterrupts(doA, doB); //, doC); + sensor.enableInterrupts(doA, doB); //, doC); // software interrupts PciManager.registerListener(&listenC); // link the motor to the sensor @@ -71,11 +71,11 @@ void setup() { // set motion control loop to be used motor.controller = MotionControlType::angle; - // contoller configuration + // contoller configuration // default parameters in defaults.h // velocity PI controller parameters - motor.PID_velocity.P = 0.2; + motor.PID_velocity.P = 0.2f; motor.PID_velocity.I = 2; motor.PID_velocity.D = 0; // default voltage_power_supply @@ -83,9 +83,9 @@ void setup() { // jerk control using voltage voltage ramp // default value is 300 volts per sec ~ 0.3V per millisecond motor.PID_velocity.output_ramp = 1000; - + // velocity low pass filtering time constant - motor.LPF_velocity.Tf = 0.01; + motor.LPF_velocity.Tf = 0.01f; // angle P controller motor.P_angle.P = 20; @@ -93,11 +93,11 @@ void setup() { motor.velocity_limit = 4; - // use monitoring with serial + // use monitoring with serial Serial.begin(115200); // comment out if not needed motor.useMonitoring(Serial); - + // initialize motor motor.init(); // align sensor and start FOC @@ -115,7 +115,7 @@ void loop() { // main FOC algorithm function // the faster you run this function the better // Arduino UNO loop ~1kHz - // Bluepill loop ~10kHz + // Bluepill loop ~10kHz motor.loopFOC(); // Motion control function @@ -127,7 +127,7 @@ void loop() { // function intended to be used with serial plotter to monitor motor variables // significantly slowing the execution down!!!! // motor.monitor(); - + // user communication command.run(); } \ No newline at end of file diff --git a/examples/motion_control/position_motion_control/magnetic_sensor/angle_control/angle_control.ino b/examples/motion_control/position_motion_control/magnetic_sensor/angle_control/angle_control.ino index d22289bd..752030c9 100644 --- a/examples/motion_control/position_motion_control/magnetic_sensor/angle_control/angle_control.ino +++ b/examples/motion_control/position_motion_control/magnetic_sensor/angle_control/angle_control.ino @@ -1,11 +1,11 @@ /** - * + * * Position/angle motion control example * Steps: - * 1) Configure the motor and magnetic sensor + * 1) Configure the motor and magnetic sensor * 2) Run the code * 3) Set the target angle (in radians) from serial terminal - * + * */ #include @@ -42,38 +42,38 @@ void setup() { driver.init(); // link the motor and the driver motor.linkDriver(&driver); - + // choose FOC modulation (optional) motor.foc_modulation = FOCModulationType::SpaceVectorPWM; // set motion control loop to be used motor.controller = MotionControlType::angle; - // contoller configuration + // contoller configuration // default parameters in defaults.h // velocity PI controller parameters - motor.PID_velocity.P = 0.2; + motor.PID_velocity.P = 0.2f; motor.PID_velocity.I = 20; motor.PID_velocity.D = 0; // maximal voltage to be set to the motor motor.voltage_limit = 6; - + // velocity low pass filtering time constant // the lower the less filtered - motor.LPF_velocity.Tf = 0.01; + motor.LPF_velocity.Tf = 0.01f; - // angle P controller + // angle P controller motor.P_angle.P = 20; // maximal velocity of the position control motor.velocity_limit = 20; - // use monitoring with serial + // use monitoring with serial Serial.begin(115200); // comment out if not needed motor.useMonitoring(Serial); - + // initialize motor motor.init(); // align sensor and start FOC @@ -93,7 +93,7 @@ void loop() { // main FOC algorithm function // the faster you run this function the better // Arduino UNO loop ~1kHz - // Bluepill loop ~10kHz + // Bluepill loop ~10kHz motor.loopFOC(); // Motion control function @@ -106,7 +106,7 @@ void loop() { // function intended to be used with serial plotter to monitor motor variables // significantly slowing the execution down!!!! // motor.monitor(); - + // user communication command.run(); } \ No newline at end of file diff --git a/examples/motion_control/torque_control/encoder/current_control/current_control.ino b/examples/motion_control/torque_control/encoder/current_control/current_control.ino index ad75b573..8b1ec96b 100644 --- a/examples/motion_control/torque_control/encoder/current_control/current_control.ino +++ b/examples/motion_control/torque_control/encoder/current_control/current_control.ino @@ -1,7 +1,7 @@ /** - * + * * Torque control example using current control loop. - * + * */ #include @@ -25,11 +25,11 @@ float target_current = 0; Commander command = Commander(Serial); void doTarget(char* cmd) { command.scalar(&target_current, cmd); } -void setup() { - +void setup() { + // initialize encoder sensor hardware encoder.init(); - encoder.enableInterrupts(doA, doB); + encoder.enableInterrupts(doA, doB); // link the motor to the sensor motor.linkSensor(&encoder); @@ -46,10 +46,10 @@ void setup() { motor.linkCurrentSense(¤t_sense); // set torque mode: - // TorqueControlType::dc_current + // TorqueControlType::dc_current // TorqueControlType::voltage // TorqueControlType::foc_current - motor.torque_controller = TorqueControlType::foc_current; + motor.torque_controller = TorqueControlType::foc_current; // set motion control loop to be used motor.controller = MotionControlType::torque; @@ -58,17 +58,17 @@ void setup() { motor.PID_current_q.I= 300; motor.PID_current_d.P= 5; motor.PID_current_d.I = 300; - motor.LPF_current_q.Tf = 0.01; - motor.LPF_current_d.Tf = 0.01; + motor.LPF_current_q.Tf = 0.01f; + motor.LPF_current_d.Tf = 0.01f; // foc currnet control parameters (stm/esp/due/teensy) // motor.PID_current_q.P = 5; // motor.PID_current_q.I= 1000; // motor.PID_current_d.P= 5; // motor.PID_current_d.I = 1000; - // motor.LPF_current_q.Tf = 0.002; // 1ms default - // motor.LPF_current_d.Tf = 0.002; // 1ms default + // motor.LPF_current_q.Tf = 0.002f; // 1ms default + // motor.LPF_current_d.Tf = 0.002f; // 1ms default - // use monitoring with serial + // use monitoring with serial Serial.begin(115200); // comment out if not needed motor.useMonitoring(Serial); @@ -91,7 +91,7 @@ void loop() { // main FOC algorithm function // the faster you run this function the better // Arduino UNO loop ~1kHz - // Bluepill loop ~10kHz + // Bluepill loop ~10kHz motor.loopFOC(); // Motion control function diff --git a/examples/motion_control/velocity_motion_control/encoder/velocity_control/velocity_control.ino b/examples/motion_control/velocity_motion_control/encoder/velocity_control/velocity_control.ino index a3974b38..f0f1e3e8 100644 --- a/examples/motion_control/velocity_motion_control/encoder/velocity_control/velocity_control.ino +++ b/examples/motion_control/velocity_motion_control/encoder/velocity_control/velocity_control.ino @@ -1,28 +1,28 @@ /** - * + * * Velocity motion control example * Steps: - * 1) Configure the motor and encoder + * 1) Configure the motor and encoder * 2) Run the code * 3) Set the target velocity (in radians per second) from serial terminal - * - * - * + * + * + * * NOTE : * > Arduino UNO example code for running velocity motion control using an encoder with index significantly * > Since Arduino UNO doesn't have enough interrupt pins we have to use software interrupt library PciManager. - * - * > If running this code with Nucleo or Bluepill or any other board which has more than 2 interrupt pins + * + * > If running this code with Nucleo or Bluepill or any other board which has more than 2 interrupt pins * > you can supply doIndex directly to the encoder.enableInterrupts(doA,doB,doIndex) and avoid using PciManger - * + * * > If you don't want to use index pin initialize the encoder class without index pin number: * > For example: * > - Encoder encoder = Encoder(2, 3, 8192); * > and initialize interrupts like this: * > - encoder.enableInterrupts(doA,doB) - * + * * Check the docs.simplefoc.com for more info about the possible encoder configuration. - * + * */ #include // software interrupt library @@ -59,7 +59,7 @@ void setup() { // initialize encoder sensor hardware encoder.init(); - encoder.enableInterrupts(doA, doB); + encoder.enableInterrupts(doA, doB); // software interrupts PciManager.registerListener(&listenerIndex); // link the motor to the sensor @@ -80,11 +80,11 @@ void setup() { // set motion control loop to be used motor.controller = MotionControlType::velocity; - // contoller configuration + // contoller configuration // default parameters in defaults.h // velocity PI controller parameters - motor.PID_velocity.P = 0.2; + motor.PID_velocity.P = 0.2f; motor.PID_velocity.I = 20; motor.PID_velocity.D = 0; // default voltage_power_supply @@ -92,11 +92,11 @@ void setup() { // jerk control using voltage voltage ramp // default value is 300 volts per sec ~ 0.3V per millisecond motor.PID_velocity.output_ramp = 1000; - + // velocity low pass filtering time constant - motor.LPF_velocity.Tf = 0.01; + motor.LPF_velocity.Tf = 0.01f; - // use monitoring with serial + // use monitoring with serial Serial.begin(115200); // comment out if not needed motor.useMonitoring(Serial); @@ -119,7 +119,7 @@ void loop() { // main FOC algorithm function // the faster you run this function the better // Arduino UNO loop ~1kHz - // Bluepill loop ~10kHz + // Bluepill loop ~10kHz motor.loopFOC(); // Motion control function @@ -131,7 +131,7 @@ void loop() { // function intended to be used with serial plotter to monitor motor variables // significantly slowing the execution down!!!! // motor.monitor(); - + // user communication command.run(); } \ No newline at end of file diff --git a/examples/motion_control/velocity_motion_control/hall_sensor/velocity_control.ino b/examples/motion_control/velocity_motion_control/hall_sensor/velocity_control.ino index 07b70dcd..1d39173a 100644 --- a/examples/motion_control/velocity_motion_control/hall_sensor/velocity_control.ino +++ b/examples/motion_control/velocity_motion_control/hall_sensor/velocity_control.ino @@ -1,20 +1,20 @@ /** - * + * * Velocity motion control example * Steps: - * 1) Configure the motor and sensor + * 1) Configure the motor and sensor * 2) Run the code * 3) Set the target velocity (in radians per second) from serial terminal - * - * - * + * + * + * * NOTE : - * > Specifically for Arduino UNO example code for running velocity motion control using a hall sensor + * > Specifically for Arduino UNO example code for running velocity motion control using a hall sensor * > Since Arduino UNO doesn't have enough interrupt pins we have to use software interrupt library PciManager. - * - * > If running this code with Nucleo or Bluepill or any other board which has more than 2 interrupt pins + * + * > If running this code with Nucleo or Bluepill or any other board which has more than 2 interrupt pins * > you can supply doC directly to the sensor.enableInterrupts(doA,doB,doC) and avoid using PciManger - * + * */ #include // software interrupt library @@ -49,7 +49,7 @@ void setup() { // initialize sensor sensor hardware sensor.init(); - sensor.enableInterrupts(doA, doB); //, doC); + sensor.enableInterrupts(doA, doB); //, doC); // software interrupts PciManager.registerListener(&listenerIndex); // link the motor to the sensor @@ -64,15 +64,15 @@ void setup() { // aligning voltage [V] motor.voltage_sensor_align = 3; - + // set motion control loop to be used motor.controller = MotionControlType::velocity; - // contoller configuration + // contoller configuration // default parameters in defaults.h // velocity PI controller parameters - motor.PID_velocity.P = 0.2; + motor.PID_velocity.P = 0.2f; motor.PID_velocity.I = 2; motor.PID_velocity.D = 0; // default voltage_power_supply @@ -80,11 +80,11 @@ void setup() { // jerk control using voltage voltage ramp // default value is 300 volts per sec ~ 0.3V per millisecond motor.PID_velocity.output_ramp = 1000; - + // velocity low pass filtering time constant - motor.LPF_velocity.Tf = 0.01; + motor.LPF_velocity.Tf = 0.01f; - // use monitoring with serial + // use monitoring with serial Serial.begin(115200); // comment out if not needed motor.useMonitoring(Serial); @@ -107,7 +107,7 @@ void loop() { // main FOC algorithm function // the faster you run this function the better // Arduino UNO loop ~1kHz - // Bluepill loop ~10kHz + // Bluepill loop ~10kHz motor.loopFOC(); // Motion control function @@ -119,7 +119,7 @@ void loop() { // function intended to be used with serial plotter to monitor motor variables // significantly slowing the execution down!!!! // motor.monitor(); - + // user communication command.run(); } \ No newline at end of file diff --git a/examples/motion_control/velocity_motion_control/magnetic_sensor/velocity_control/velocity_control.ino b/examples/motion_control/velocity_motion_control/magnetic_sensor/velocity_control/velocity_control.ino index a16385df..a57fe634 100644 --- a/examples/motion_control/velocity_motion_control/magnetic_sensor/velocity_control/velocity_control.ino +++ b/examples/motion_control/velocity_motion_control/magnetic_sensor/velocity_control/velocity_control.ino @@ -1,12 +1,12 @@ /** - * + * * Velocity motion control example * Steps: - * 1) Configure the motor and magnetic sensor + * 1) Configure the motor and magnetic sensor * 2) Run the code * 3) Set the target velocity (in radians per second) from serial terminal - * - * + * + * * By using the serial terminal set the velocity value you want to motor to obtain * */ @@ -32,7 +32,7 @@ Commander command = Commander(Serial); void doTarget(char* cmd) { command.scalar(&target_velocity, cmd); } void setup() { - + // initialise magnetic sensor hardware sensor.init(); // link the motor to the sensor @@ -48,11 +48,11 @@ void setup() { // set motion control loop to be used motor.controller = MotionControlType::velocity; - // contoller configuration + // contoller configuration // default parameters in defaults.h // velocity PI controller parameters - motor.PID_velocity.P = 0.2; + motor.PID_velocity.P = 0.2f; motor.PID_velocity.I = 20; motor.PID_velocity.D = 0; // default voltage_power_supply @@ -60,13 +60,13 @@ void setup() { // jerk control using voltage voltage ramp // default value is 300 volts per sec ~ 0.3V per millisecond motor.PID_velocity.output_ramp = 1000; - + // velocity low pass filtering - // default 5ms - try different values to see what is the best. + // default 5ms - try different values to see what is the best. // the lower the less filtered - motor.LPF_velocity.Tf = 0.01; + motor.LPF_velocity.Tf = 0.01f; - // use monitoring with serial + // use monitoring with serial Serial.begin(115200); // comment out if not needed motor.useMonitoring(Serial); @@ -88,7 +88,7 @@ void loop() { // main FOC algorithm function // the faster you run this function the better // Arduino UNO loop ~1kHz - // Bluepill loop ~10kHz + // Bluepill loop ~10kHz motor.loopFOC(); // Motion control function @@ -100,7 +100,7 @@ void loop() { // function intended to be used with serial plotter to monitor motor variables // significantly slowing the execution down!!!! // motor.monitor(); - + // user communication command.run(); } \ No newline at end of file diff --git a/examples/motor_commands_serial_examples/encoder/full_control_serial/full_control_serial.ino b/examples/motor_commands_serial_examples/encoder/full_control_serial/full_control_serial.ino index 30d18297..186d257e 100644 --- a/examples/motor_commands_serial_examples/encoder/full_control_serial/full_control_serial.ino +++ b/examples/motor_commands_serial_examples/encoder/full_control_serial/full_control_serial.ino @@ -1,13 +1,13 @@ /** * Comprehensive BLDC motor control example using encoder - * + * * Using serial terminal user can send motor commands and configure the motor and FOC in real-time: * - configure PID controller constants * - change motion control loops * - monitor motor variabels * - set target values - * - check all the configuration values - * + * - check all the configuration values + * * See more info in docs.simplefoc.com/commander_interface */ #include @@ -36,7 +36,7 @@ void setup() { // initialize encoder sensor hardware encoder.init(); - encoder.enableInterrupts(doA, doB); + encoder.enableInterrupts(doA, doB); // link the motor to the sensor motor.linkSensor(&encoder); @@ -53,15 +53,15 @@ void setup() { // set control loop type to be used motor.controller = MotionControlType::torque; - // contoller configuration based on the controll type - motor.PID_velocity.P = 0.2; + // contoller configuration based on the controll type + motor.PID_velocity.P = 0.2f; motor.PID_velocity.I = 20; motor.PID_velocity.D = 0; // default voltage_power_supply motor.voltage_limit = 12; // velocity low pass filtering time constant - motor.LPF_velocity.Tf = 0.01; + motor.LPF_velocity.Tf = 0.01f; // angle loop controller motor.P_angle.P = 20; @@ -87,7 +87,7 @@ void setup() { // Run user commands to configure and the motor (find the full command list in docs.simplefoc.com) Serial.println(F("Motor commands sketch | Initial motion control > torque/voltage : target 2V.")); - + _delay(1000); } diff --git a/examples/motor_commands_serial_examples/hall_sensor/full_control_serial/full_control_serial.ino b/examples/motor_commands_serial_examples/hall_sensor/full_control_serial/full_control_serial.ino index 7a202f94..df7b76ac 100644 --- a/examples/motor_commands_serial_examples/hall_sensor/full_control_serial/full_control_serial.ino +++ b/examples/motor_commands_serial_examples/hall_sensor/full_control_serial/full_control_serial.ino @@ -1,13 +1,13 @@ /** * Comprehensive BLDC motor control example using Hall sensor - * + * * Using serial terminal user can send motor commands and configure the motor and FOC in real-time: * - configure PID controller constants * - change motion control loops * - monitor motor variabels * - set target values - * - check all the configuration values - * + * - check all the configuration values + * * See more info in docs.simplefoc.com/commander_interface */ #include @@ -43,7 +43,7 @@ void setup() { // initialize encoder sensor hardware sensor.init(); - sensor.enableInterrupts(doA, doB); //, doC); + sensor.enableInterrupts(doA, doB); //, doC); // software interrupts PciManager.registerListener(&listenC); // link the motor to the sensor @@ -61,15 +61,15 @@ void setup() { motor.foc_modulation = FOCModulationType::SpaceVectorPWM; // set control loop type to be used motor.controller = MotionControlType::torque; - // contoller configuration based on the controll type - motor.PID_velocity.P = 0.2; + // contoller configuration based on the controll type + motor.PID_velocity.P = 0.2f; motor.PID_velocity.I = 20; motor.PID_velocity.D = 0; // default voltage_power_supply motor.voltage_limit = 12; // velocity low pass filtering time constant - motor.LPF_velocity.Tf = 0.01; + motor.LPF_velocity.Tf = 0.01f; // angle loop controller motor.P_angle.P = 20; @@ -95,7 +95,7 @@ void setup() { // Run user commands to configure and the motor (find the full command list in docs.simplefoc.com) Serial.println(F("Motor commands sketch | Initial motion control > torque/voltage : target 2V.")); - + _delay(1000); } @@ -112,5 +112,3 @@ void loop() { // user communication command.run(); } - - diff --git a/examples/motor_commands_serial_examples/magnetic_sensor/full_control_serial/full_control_serial.ino b/examples/motor_commands_serial_examples/magnetic_sensor/full_control_serial/full_control_serial.ino index 97216a9e..098f6888 100644 --- a/examples/motor_commands_serial_examples/magnetic_sensor/full_control_serial/full_control_serial.ino +++ b/examples/motor_commands_serial_examples/magnetic_sensor/full_control_serial/full_control_serial.ino @@ -1,13 +1,13 @@ /** * Comprehensive BLDC motor control example using magnetic sensor - * + * * Using serial terminal user can send motor commands and configure the motor and FOC in real-time: * - configure PID controller constants * - change motion control loops * - monitor motor variabels * - set target values - * - check all the configuration values - * + * - check all the configuration values + * * See more info in docs.simplefoc.com/commander_interface */ #include @@ -51,15 +51,15 @@ void setup() { // set control loop type to be used motor.controller = MotionControlType::torque; - // contoller configuration based on the control type - motor.PID_velocity.P = 0.2; + // contoller configuration based on the control type + motor.PID_velocity.P = 0.2f; motor.PID_velocity.I = 20; motor.PID_velocity.D = 0; // default voltage_power_supply motor.voltage_limit = 12; // velocity low pass filtering time constant - motor.LPF_velocity.Tf = 0.01; + motor.LPF_velocity.Tf = 0.01f; // angle loop controller motor.P_angle.P = 20; @@ -85,7 +85,7 @@ void setup() { // Run user commands to configure and the motor (find the full command list in docs.simplefoc.com) Serial.println(F("Motor commands sketch | Initial motion control > torque/voltage : target 2V.")); - + _delay(1000); } @@ -98,8 +98,7 @@ void loop() { // velocity, position or voltage // if tatget not set in parameter uses motor.target variable motor.move(); - + // user communication command.run(); } - diff --git a/examples/osc_control_examples/osc_esp32_3pwm/osc_esp32_3pwm.ino b/examples/osc_control_examples/osc_esp32_3pwm/osc_esp32_3pwm.ino index b0a24d25..afb95294 100644 --- a/examples/osc_control_examples/osc_esp32_3pwm/osc_esp32_3pwm.ino +++ b/examples/osc_control_examples/osc_esp32_3pwm/osc_esp32_3pwm.ino @@ -93,11 +93,11 @@ void setup() { driver.init(); motor.linkDriver(&driver); motor.controller = MotionControlType::velocity; - motor.PID_velocity.P = 0.2; + motor.PID_velocity.P = 0.2f; motor.PID_velocity.I = 20; - motor.PID_velocity.D = 0.001; + motor.PID_velocity.D = 0.001f; motor.PID_velocity.output_ramp = 1000; - motor.LPF_velocity.Tf = 0.01; + motor.LPF_velocity.Tf = 0.01f; motor.voltage_limit = 8; //motor.P_angle.P = 20; motor.init(); @@ -107,9 +107,9 @@ void setup() { } // velocity set point variable -float target_velocity = 2.0; +float target_velocity = 2.0f; // angle set point variable -float target_angle = 1.0; +float target_angle = 1.0f; void motorControl(OSCMessage &msg){ diff --git a/examples/osc_control_examples/osc_esp32_fullcontrol/osc_esp32_fullcontrol.ino b/examples/osc_control_examples/osc_esp32_fullcontrol/osc_esp32_fullcontrol.ino index 917f569c..7c6aa4f0 100644 --- a/examples/osc_control_examples/osc_esp32_fullcontrol/osc_esp32_fullcontrol.ino +++ b/examples/osc_control_examples/osc_esp32_fullcontrol/osc_esp32_fullcontrol.ino @@ -30,7 +30,7 @@ WiFiUDP Udp; IPAddress outIp(192,168,1,13); // remote IP (not needed for receive) const unsigned int outPort = 8000; // remote port (not needed for receive) const unsigned int inPort = 8000; // local port to listen for UDP packets (here's where we send the packets) -#define POWER_SUPPLY 7.4 +#define POWER_SUPPLY 7.4f @@ -46,8 +46,8 @@ String m2Prefix("/M2"); // we store seperate set-points for each motor, of course -float set_point1 = 0.0; -float set_point2 = 0.0; +float set_point1 = 0.0f; +float set_point2 = 0.0f; OSCErrorCode error; @@ -135,11 +135,11 @@ void setup() { motor1.linkDriver(&driver1); motor1.foc_modulation = FOCModulationType::SpaceVectorPWM; motor1.controller = MotionControlType::velocity; - motor1.PID_velocity.P = 0.2; + motor1.PID_velocity.P = 0.2f; motor1.PID_velocity.I = 20; - motor1.PID_velocity.D = 0.001; + motor1.PID_velocity.D = 0.001f; motor1.PID_velocity.output_ramp = 1000; - motor1.LPF_velocity.Tf = 0.01; + motor1.LPF_velocity.Tf = 0.01f; motor1.voltage_limit = POWER_SUPPLY; motor1.P_angle.P = 20; motor1.init(); @@ -152,11 +152,11 @@ void setup() { motor2.linkDriver(&driver2); motor2.foc_modulation = FOCModulationType::SpaceVectorPWM; motor2.controller = MotionControlType::velocity; - motor2.PID_velocity.P = 0.2; + motor2.PID_velocity.P = 0.2f; motor2.PID_velocity.I = 20; - motor2.PID_velocity.D = 0.001; + motor2.PID_velocity.D = 0.001f; motor2.PID_velocity.output_ramp = 1000; - motor2.LPF_velocity.Tf = 0.01; + motor2.LPF_velocity.Tf = 0.01f; motor2.voltage_limit = POWER_SUPPLY; motor2.P_angle.P = 20; motor2.init(); diff --git a/examples/utils/calibration/alignment_and_cogging_test/alignment_and_cogging_test.ino b/examples/utils/calibration/alignment_and_cogging_test/alignment_and_cogging_test.ino index 708e2d9e..ca254a5f 100644 --- a/examples/utils/calibration/alignment_and_cogging_test/alignment_and_cogging_test.ino +++ b/examples/utils/calibration/alignment_and_cogging_test/alignment_and_cogging_test.ino @@ -9,7 +9,7 @@ MagneticSensorI2C sensor = MagneticSensorI2C(0x36, 12, 0X0C, 4); /** - * This measures how closely sensor and electrical angle agree and how much your motor is affected by 'cogging'. + * This measures how closely sensor and electrical angle agree and how much your motor is affected by 'cogging'. * It can be used to investigate how much non linearity there is between what we set (electrical angle) and what we read (sensor angle) * This non linearity could be down to magnet placement, coil winding differences or simply that the magnetic field when travelling through a pole pair is not linear * An alignment error of ~10 degrees and cogging of ~4 degrees is normal for small gimbal. @@ -28,9 +28,9 @@ void testAlignmentAndCogging(int direction) { float stDevSum = 0; - float mean = 0.0; - float prev_mean = 0.0; - + float mean = 0.0f; + float prev_mean = 0.0f; + for (int i = 0; i < sample_count; i++) { @@ -39,7 +39,7 @@ void testAlignmentAndCogging(int direction) { motor.move(electricAngle * PI / 180); _delay(5); - // measure + // measure float sensorAngle = (sensor.getAngle() - initialAngle) * 180 / PI; float sensorElectricAngle = sensorAngle * motor.pole_pairs; float electricAngleError = electricAngle - sensorElectricAngle; @@ -87,7 +87,7 @@ void setup() { motor.voltage_sensor_align = 3; motor.foc_modulation = FOCModulationType::SpaceVectorPWM; - + motor.controller = MotionControlType::angle_openloop; motor.voltage_limit=motor.voltage_sensor_align; diff --git a/examples/utils/calibration/find_pole_pair_number/encoder/find_pole_pairs_number/find_pole_pairs_number.ino b/examples/utils/calibration/find_pole_pair_number/encoder/find_pole_pairs_number/find_pole_pairs_number.ino index e6a1d20b..e6187921 100644 --- a/examples/utils/calibration/find_pole_pair_number/encoder/find_pole_pairs_number/find_pole_pairs_number.ino +++ b/examples/utils/calibration/find_pole_pair_number/encoder/find_pole_pairs_number/find_pole_pairs_number.ino @@ -1,17 +1,17 @@ /** * Utility arduino sketch which finds pole pair number of the motor - * + * * To run it just set the correct pin numbers for the BLDC driver and encoder A and B channel as well as the encoder PPR value. - * + * * The program will rotate your motor a specific amount and check how much it moved, and by doing a simple calculation calculate your pole pair number. - * The pole pair number will be outputted to the serial terminal. - * - * If the pole pair number is well estimated your motor will start to spin in voltage mode with 2V target. - * + * The pole pair number will be outputted to the serial terminal. + * + * If the pole pair number is well estimated your motor will start to spin in voltage mode with 2V target. + * * If the code calculates negative pole pair number please invert your encoder A and B channel pins or motor connector. - * - * Try running this code several times to avoid statistical errors. - * > But in general if your motor spins, you have a good pole pairs number. + * + * Try running this code several times to avoid statistical errors. + * > But in general if your motor spins, you have a good pole pairs number. */ #include @@ -31,7 +31,7 @@ void doA(){encoder.handleA();} void doB(){encoder.handleB();} void setup() { - + // initialise encoder hardware encoder.init(); // hardware interrupt enable @@ -57,20 +57,20 @@ void setup() { float pp_search_voltage = 4; // maximum power_supply_voltage/2 float pp_search_angle = 6*M_PI; // search electrical angle to turn - + // move motor to the electrical angle 0 motor.controller = MotionControlType::angle_openloop; motor.voltage_limit=pp_search_voltage; motor.move(0); _delay(1000); - // read the encoder angle + // read the encoder angle float angle_begin = encoder.getAngle(); _delay(50); - + // move the motor slowly to the electrical angle pp_search_angle float motor_angle = 0; while(motor_angle <= pp_search_angle){ - motor_angle += 0.01; + motor_angle += 0.01f; motor.move(motor_angle); } _delay(1000); @@ -93,7 +93,7 @@ void setup() { Serial.print(" = "); Serial.println((pp_search_angle)/(angle_end-angle_begin)); Serial.println(); - + // a bit of monitoring the result if(pp <= 0 ){ @@ -108,7 +108,7 @@ void setup() { Serial.println(F(" - You can also try to adjust the target voltage using serial terminal!")); } - + // set FOC loop to be used motor.controller = MotionControlType::torque; // set the pole pair number to the motor @@ -129,7 +129,7 @@ void loop() { // main FOC algorithm function // the faster you run this function the better // Arduino UNO loop ~1kHz - // Bluepill loop ~10kHz + // Bluepill loop ~10kHz motor.loopFOC(); // Motion control function @@ -137,7 +137,7 @@ void loop() { // this function can be run at much lower frequency than loopFOC() function // You can also use motor.move() and set the motor.target in the code motor.move(target_voltage); - + // communicate with the user serialReceiveUserCommand(); } @@ -146,10 +146,10 @@ void loop() { // utility function enabling serial communication with the user to set the target values // this function can be implemented in serialEvent function as well void serialReceiveUserCommand() { - + // a string to hold incoming data static String received_chars; - + while (Serial.available()) { // get the new byte: char inChar = (char)Serial.read(); @@ -157,13 +157,13 @@ void serialReceiveUserCommand() { received_chars += inChar; // end of user input if (inChar == '\n') { - + // change the motor target target_voltage = received_chars.toFloat(); Serial.print("Target voltage: "); Serial.println(target_voltage); - - // reset the command buffer + + // reset the command buffer received_chars = ""; } } diff --git a/examples/utils/calibration/find_pole_pair_number/magnetic_sensor/find_pole_pairs_number/find_pole_pairs_number.ino b/examples/utils/calibration/find_pole_pair_number/magnetic_sensor/find_pole_pairs_number/find_pole_pairs_number.ino index cc4bd4d0..c2785c55 100644 --- a/examples/utils/calibration/find_pole_pair_number/magnetic_sensor/find_pole_pairs_number/find_pole_pairs_number.ino +++ b/examples/utils/calibration/find_pole_pair_number/magnetic_sensor/find_pole_pairs_number/find_pole_pairs_number.ino @@ -1,17 +1,17 @@ /** * Utility arduino sketch which finds pole pair number of the motor - * + * * To run it just set the correct pin numbers for the BLDC driver and sensor CPR value and chip select pin. - * + * * The program will rotate your motor a specific amount and check how much it moved, and by doing a simple calculation calculate your pole pair number. - * The pole pair number will be outputted to the serial terminal. - * - * If the pole pair number is well estimated your motor will start to spin in voltage mode with 2V target. - * + * The pole pair number will be outputted to the serial terminal. + * + * If the pole pair number is well estimated your motor will start to spin in voltage mode with 2V target. + * * If the code calculates negative pole pair number please invert your motor connector. - * - * Try running this code several times to avoid statistical errors. - * > But in general if your motor spins, you have a good pole pairs number. + * + * Try running this code several times to avoid statistical errors. + * > But in general if your motor spins, you have a good pole pairs number. */ #include @@ -56,20 +56,20 @@ void setup() { float pp_search_voltage = 4; // maximum power_supply_voltage/2 float pp_search_angle = 6*M_PI; // search electrical angle to turn - + // move motor to the electrical angle 0 motor.controller = MotionControlType::angle_openloop; motor.voltage_limit=pp_search_voltage; motor.move(0); _delay(1000); - // read the sensor angle + // read the sensor angle float angle_begin = sensor.getAngle(); _delay(50); - + // move the motor slowly to the electrical angle pp_search_angle float motor_angle = 0; while(motor_angle <= pp_search_angle){ - motor_angle += 0.01; + motor_angle += 0.01f; sensor.getAngle(); // keep track of the overflow motor.move(motor_angle); } @@ -93,7 +93,7 @@ void setup() { Serial.print(F(" = ")); Serial.println((pp_search_angle)/(angle_end-angle_begin)); Serial.println(); - + // a bit of monitoring the result if(pp <= 0 ){ @@ -108,7 +108,7 @@ void setup() { Serial.println(F(" - You can also try to adjust the target voltage using serial terminal!")); } - + // set motion control loop to be used motor.controller = MotionControlType::torque; // set the pole pair number to the motor @@ -129,7 +129,7 @@ void loop() { // main FOC algorithm function // the faster you run this function the better // Arduino UNO loop ~1kHz - // Bluepill loop ~10kHz + // Bluepill loop ~10kHz motor.loopFOC(); // Motion control function @@ -137,7 +137,7 @@ void loop() { // this function can be run at much lower frequency than loopFOC() function // You can also use motor.move() and set the motor.target in the code motor.move(target_voltage); - + // communicate with the user serialReceiveUserCommand(); } @@ -146,10 +146,10 @@ void loop() { // utility function enabling serial communication with the user to set the target values // this function can be implemented in serialEvent function as well void serialReceiveUserCommand() { - + // a string to hold incoming data static String received_chars; - + while (Serial.available()) { // get the new byte: char inChar = (char)Serial.read(); @@ -157,13 +157,13 @@ void serialReceiveUserCommand() { received_chars += inChar; // end of user input if (inChar == '\n') { - + // change the motor target target_voltage = received_chars.toFloat(); Serial.print("Target voltage: "); Serial.println(target_voltage); - - // reset the command buffer + + // reset the command buffer received_chars = ""; } } diff --git a/examples/utils/communication_test/step_dir/step_dir_listener_software_interrupt/step_dir_listener_software_interrupt.ino b/examples/utils/communication_test/step_dir/step_dir_listener_software_interrupt/step_dir_listener_software_interrupt.ino index d723313c..b6f0ea5f 100644 --- a/examples/utils/communication_test/step_dir/step_dir_listener_software_interrupt/step_dir_listener_software_interrupt.ino +++ b/examples/utils/communication_test/step_dir/step_dir_listener_software_interrupt/step_dir_listener_software_interrupt.ino @@ -1,6 +1,6 @@ /** - * A simple example of reading step/dir communication - * - this example uses software interrupts - this code is intended primarily + * A simple example of reading step/dir communication + * - this example uses software interrupts - this code is intended primarily * for Arduino UNO/Mega and similar boards with very limited number of interrupt pins */ @@ -10,11 +10,11 @@ #include -// angle +// angle float received_angle = 0; // StepDirListener( step_pin, dir_pin, counter_to_value) -StepDirListener step_dir = StepDirListener(4, 5, 2.0*_PI/200.0); // receive the angle in radians +StepDirListener step_dir = StepDirListener(4, 5, 2.0f*_PI/200.0); // receive the angle in radians void onStep() { step_dir.handle(); } // If no available hadware interrupt pins use the software interrupt @@ -23,21 +23,21 @@ PciListenerImp listenStep(step_dir.pin_step, onStep); void setup() { Serial.begin(115200); - + // init step and dir pins step_dir.init(); // enable software interrupts PciManager.registerListener(&listenStep); - // attach the variable to be updated on each step (optional) + // attach the variable to be updated on each step (optional) // the same can be done asynchronously by caling step_dir.getValue(); step_dir.attach(&received_angle); - + Serial.println(F("Step/Dir listenning.")); _delay(1000); } void loop() { - Serial.print(received_angle); + Serial.print(received_angle); Serial.print("\t"); Serial.println(step_dir.getValue()); _delay(500); diff --git a/examples/utils/communication_test/step_dir/step_dir_motor_example/step_dir_motor_example.ino b/examples/utils/communication_test/step_dir/step_dir_motor_example/step_dir_motor_example.ino index 932edc81..f8978577 100644 --- a/examples/utils/communication_test/step_dir/step_dir_motor_example/step_dir_motor_example.ino +++ b/examples/utils/communication_test/step_dir/step_dir_motor_example/step_dir_motor_example.ino @@ -19,7 +19,7 @@ void doA() { encoder.handleA(); } void doB() { encoder.handleB(); } // StepDirListener( step_pin, dir_pin, counter_to_value) -StepDirListener step_dir = StepDirListener(A4, A5, 2.0*_PI/200.0); +StepDirListener step_dir = StepDirListener(A4, A5, 2.0f*_PI/200.0); void onStep() { step_dir.handle(); } void setup() { @@ -48,7 +48,7 @@ void setup() { // contoller configuration // default parameters in defaults.h // velocity PI controller parameters - motor.PID_velocity.P = 0.2; + motor.PID_velocity.P = 0.2f; motor.PID_velocity.I = 20; motor.PID_velocity.D = 0; // default voltage_power_supply @@ -58,7 +58,7 @@ void setup() { motor.PID_velocity.output_ramp = 1000; // velocity low pass filtering time constant - motor.LPF_velocity.Tf = 0.01; + motor.LPF_velocity.Tf = 0.01f; // angle P controller motor.P_angle.P = 10; @@ -77,12 +77,12 @@ void setup() { // init step and dir pins step_dir.init(); - // enable interrupts + // enable interrupts step_dir.enableInterrupt(onStep); - // attach the variable to be updated on each step (optional) + // attach the variable to be updated on each step (optional) // the same can be done asynchronously by caling motor.move(step_dir.getValue()); step_dir.attach(&motor.target); - + Serial.println(F("Motor ready.")); Serial.println(F("Listening to step/dir commands!")); _delay(1000); diff --git a/examples/utils/driver_standalone_test/bldc_driver_6pwm_standalone/bldc_driver_6pwm_standalone.ino b/examples/utils/driver_standalone_test/bldc_driver_6pwm_standalone/bldc_driver_6pwm_standalone.ino index 972a74db..478d3974 100644 --- a/examples/utils/driver_standalone_test/bldc_driver_6pwm_standalone/bldc_driver_6pwm_standalone.ino +++ b/examples/utils/driver_standalone_test/bldc_driver_6pwm_standalone/bldc_driver_6pwm_standalone.ino @@ -5,7 +5,7 @@ BLDCDriver6PWM driver = BLDCDriver6PWM(5, 6, 9,10, 3, 11, 8); void setup() { - + // pwm frequency to be used [Hz] // for atmega328 fixed to 32kHz // esp32/stm32/teensy configurable @@ -14,8 +14,8 @@ void setup() { driver.voltage_power_supply = 12; // Max DC voltage allowed - default voltage_power_supply driver.voltage_limit = 12; - // daad_zone [0,1] - default 0.02 - 2% - driver.dead_zone = 0.05; + // daad_zone [0,1] - default 0.02f - 2% + driver.dead_zone = 0.05f; // driver init driver.init(); diff --git a/examples/utils/driver_standalone_test/stepper_driver_2pwm_standalone/stepper_driver_2pwm_standalone.ino b/examples/utils/driver_standalone_test/stepper_driver_2pwm_standalone/stepper_driver_2pwm_standalone.ino index 7289ac79..4627efa9 100644 --- a/examples/utils/driver_standalone_test/stepper_driver_2pwm_standalone/stepper_driver_2pwm_standalone.ino +++ b/examples/utils/driver_standalone_test/stepper_driver_2pwm_standalone/stepper_driver_2pwm_standalone.ino @@ -3,8 +3,10 @@ // Stepper driver instance -// StepperDriver2PWM(pwm1, in1a, in1b, pwm2, in2a, in2b, (en1, en2 optional)) -StepperDriver2PWM driver = StepperDriver2PWM(3, 4, 5, 10 , 9 , 8 , 11, 12); +// StepperDriver2PWM(pwm1, in1, pwm2, in2, (en1, en2 optional)) +int in1[] = {4,5}; +int in2[] = {9,8}; +StepperDriver2PWM driver = StepperDriver2PWM(3, in1, 10 , in2, 11, 12); // StepperDriver2PWM(pwm1, dir1, pwm2, dir2,(en1, en2 optional)) // StepperDriver2PWM driver = StepperDriver2PWM(3, 4, 5, 6, 11, 12); diff --git a/keywords.txt b/keywords.txt index 77152a36..1d8156f5 100644 --- a/keywords.txt +++ b/keywords.txt @@ -18,6 +18,7 @@ StepperDriver KEYWORD1 PIDController KEYWORD1 LowPassFilter KEYWORD1 InlineCurrentSense KEYWORD1 +LowsideCurrentSense KEYWORD1 CurrentSense KEYWORD1 Commander KEYWORD1 StepDirListener KEYWORD1 @@ -48,6 +49,9 @@ LPF_current_q KEYWORD2 LPF_current_d KEYWORD2 P_angle KEYWORD2 LPF_angle KEYWORD2 +lpf_a KEYWORD2 +lpf_b KEYWORD2 +lpf_c KEYWORD2 MotionControlType KEYWORD1 TorqueControlType KEYWORD1 diff --git a/library.properties b/library.properties index af8559f1..0d89185d 100644 --- a/library.properties +++ b/library.properties @@ -1,5 +1,5 @@ name=Simple FOC -version=2.1.1 +version=2.1.2 author=Simplefoc maintainer=Simplefoc sentence=A library demistifying FOC for BLDC motors diff --git a/src/BLDCMotor.cpp b/src/BLDCMotor.cpp index 9c2f89c0..93fcee17 100644 --- a/src/BLDCMotor.cpp +++ b/src/BLDCMotor.cpp @@ -159,7 +159,7 @@ int BLDCMotor::alignSensor() { // find natural direction // move one electrical revolution forward for (int i = 0; i <=500; i++ ) { - float angle = _3PI_2 + _2PI * i / 500.0; + float angle = _3PI_2 + _2PI * i / 500.0f; setPhaseVoltage(voltage_sensor_align, 0, angle); _delay(2); } @@ -167,7 +167,7 @@ int BLDCMotor::alignSensor() { float mid_angle = sensor->getAngle(); // move one electrical revolution backwards for (int i = 500; i >=0; i-- ) { - float angle = _3PI_2 + _2PI * i / 500.0 ; + float angle = _3PI_2 + _2PI * i / 500.0f ; setPhaseVoltage(voltage_sensor_align, 0, angle); _delay(2); } @@ -188,7 +188,7 @@ int BLDCMotor::alignSensor() { // check pole pair number if(monitor_port) monitor_port->print(F("MOT: PP check: ")); float moved = fabs(mid_angle - end_angle); - if( fabs(moved*pole_pairs - _2PI) > 0.5 ) { // 0.5 is arbitrary number it can be lower or higher! + if( fabs(moved*pole_pairs - _2PI) > 0.5f ) { // 0.5f is arbitrary number it can be lower or higher! if(monitor_port) monitor_port->print(F("fail - estimated pp:")); if(monitor_port) monitor_port->println(_2PI/moved,4); }else if(monitor_port) monitor_port->println(F("OK!")); @@ -226,7 +226,7 @@ int BLDCMotor::absoluteZeroSearch() { voltage_limit = voltage_sensor_align; shaft_angle = 0; while(sensor->needsSearch() && shaft_angle < _2PI){ - angleOpenloop(1.5*_2PI); + angleOpenloop(1.5f*_2PI); // call important for some sensors not to loose count // not needed for the search sensor->getAngle(); @@ -313,11 +313,13 @@ void BLDCMotor::move(float new_target) { switch (controller) { case MotionControlType::torque: - if(torque_controller == TorqueControlType::voltage) // if voltage torque control + if(torque_controller == TorqueControlType::voltage){ // if voltage torque control if(!_isset(phase_resistance)) voltage.q = target; else voltage.q = target*phase_resistance; - else + voltage.d = 0; + }else{ current_sp = target; // if current/foc_current torque control + } break; case MotionControlType::angle: // angle set point @@ -568,9 +570,9 @@ float BLDCMotor::velocityOpenloop(float target_velocity){ // get current timestamp unsigned long now_us = _micros(); // calculate the sample time from last call - float Ts = (now_us - open_loop_timestamp) * 1e-6; + float Ts = (now_us - open_loop_timestamp) * 1e-6f; // quick fix for strange cases (micros overflow + timestamp not defined) - if(Ts <= 0 || Ts > 0.5) Ts = 1e-3; + if(Ts <= 0 || Ts > 0.5f) Ts = 1e-3f; // calculate the necessary angle to achieve target velocity shaft_angle = _normalizeAngle(shaft_angle + target_velocity*Ts); @@ -597,9 +599,9 @@ float BLDCMotor::angleOpenloop(float target_angle){ // get current timestamp unsigned long now_us = _micros(); // calculate the sample time from last call - float Ts = (now_us - open_loop_timestamp) * 1e-6; + float Ts = (now_us - open_loop_timestamp) * 1e-6f; // quick fix for strange cases (micros overflow + timestamp not defined) - if(Ts <= 0 || Ts > 0.5) Ts = 1e-3; + if(Ts <= 0 || Ts > 0.5f) Ts = 1e-3f; // calculate the necessary angle to move from current position towards target angle // with maximal velocity (velocity_limit) diff --git a/src/StepperMotor.cpp b/src/StepperMotor.cpp index 183c2c65..ca0716b7 100644 --- a/src/StepperMotor.cpp +++ b/src/StepperMotor.cpp @@ -11,7 +11,7 @@ StepperMotor::StepperMotor(int pp, float _R) // save phase resistance number phase_resistance = _R; - // torque control type is voltage by default + // torque control type is voltage by default // current and foc_current not supported yet torque_controller = TorqueControlType::voltage; } @@ -23,7 +23,7 @@ void StepperMotor::linkDriver(StepperDriver* _driver) { driver = _driver; } -// init hardware pins +// init hardware pins void StepperMotor::init() { if(monitor_port) monitor_port->println(F("MOT: Init")); @@ -37,7 +37,7 @@ void StepperMotor::init() { if(voltage_limit > driver->voltage_limit) voltage_limit = driver->voltage_limit; // constrain voltage for sensor alignment if(voltage_sensor_align > voltage_limit) voltage_sensor_align = voltage_limit; - + // update the controller limits if(_isset(phase_resistance)){ // velocity control loop controls current @@ -52,7 +52,7 @@ void StepperMotor::init() { if(monitor_port) monitor_port->println(F("MOT: Enable driver.")); enable(); _delay(500); - + } @@ -101,14 +101,14 @@ int StepperMotor::initFOC( float zero_electric_offset, Direction _sensor_direct // added the shaft_angle update shaft_angle = sensor->getAngle(); }else if(monitor_port) monitor_port->println(F("MOT: No sensor.")); - + if(exit_flag){ if(monitor_port) monitor_port->println(F("MOT: Ready.")); }else{ if(monitor_port) monitor_port->println(F("MOT: Init FOC failed.")); disable(); } - + return exit_flag; } @@ -116,7 +116,7 @@ int StepperMotor::initFOC( float zero_electric_offset, Direction _sensor_direct int StepperMotor::alignSensor() { int exit_flag = 1; //success if(monitor_port) monitor_port->println(F("MOT: Align sensor.")); - + // if unknown natural direction if(!_isset(sensor_direction)){ // check if sensor needs zero search @@ -127,7 +127,7 @@ int StepperMotor::alignSensor() { // find natural direction // move one electrical revolution forward for (int i = 0; i <=500; i++ ) { - float angle = _3PI_2 + _2PI * i / 500.0; + float angle = _3PI_2 + _2PI * i / 500.0f; setPhaseVoltage(voltage_sensor_align, 0, angle); _delay(2); } @@ -135,14 +135,14 @@ int StepperMotor::alignSensor() { float mid_angle = sensor->getAngle(); // move one electrical revolution backwards for (int i = 500; i >=0; i-- ) { - float angle = _3PI_2 + _2PI * i / 500.0 ; + float angle = _3PI_2 + _2PI * i / 500.0f ; setPhaseVoltage(voltage_sensor_align, 0, angle); _delay(2); } float end_angle = sensor->getAngle(); setPhaseVoltage(0, 0, 0); _delay(200); - // determine the direction the sensor moved + // determine the direction the sensor moved if (mid_angle == end_angle) { if(monitor_port) monitor_port->println(F("MOT: Failed to notice movement")); return 0; // failed calibration @@ -153,10 +153,10 @@ int StepperMotor::alignSensor() { if(monitor_port) monitor_port->println(F("MOT: sensor_direction==CW")); sensor_direction = Direction::CW; } - // check pole pair number + // check pole pair number if(monitor_port) monitor_port->print(F("MOT: PP check: ")); float moved = fabs(mid_angle - end_angle); - if( fabs(moved*pole_pairs - _2PI) > 0.5 ) { // 0.5 is arbitrary number it can be lower or higher! + if( fabs(moved*pole_pairs - _2PI) > 0.5f ) { // 0.5f is arbitrary number it can be lower or higher! if(monitor_port) monitor_port->print(F("fail - estimated pp:")); if(monitor_port) monitor_port->println(_2PI/moved,4); }else if(monitor_port) monitor_port->println(F("OK!")); @@ -166,7 +166,7 @@ int StepperMotor::alignSensor() { // zero electric angle not known if(!_isset(zero_electric_angle)){ // align the electrical phases of the motor and sensor - // set angle -90(270 = 3PI/2) degrees + // set angle -90(270 = 3PI/2) degrees setPhaseVoltage(voltage_sensor_align, 0, _3PI_2); _delay(700); zero_electric_angle = _normalizeAngle(_electricalAngle(sensor_direction*sensor->getAngle(), pole_pairs)); @@ -185,7 +185,7 @@ int StepperMotor::alignSensor() { // Encoder alignment the absolute zero angle // - to the index int StepperMotor::absoluteZeroSearch() { - + if(monitor_port) monitor_port->println(F("MOT: Index search...")); // search the absolute zero with small velocity float limit_vel = velocity_limit; @@ -194,7 +194,7 @@ int StepperMotor::absoluteZeroSearch() { voltage_limit = voltage_sensor_align; shaft_angle = 0; while(sensor->needsSearch() && shaft_angle < _2PI){ - angleOpenloop(1.5*_2PI); + angleOpenloop(1.5f*_2PI); // call important for some sensors not to loose count // not needed for the search sensor->getAngle(); @@ -218,15 +218,15 @@ int StepperMotor::absoluteZeroSearch() { void StepperMotor::loopFOC() { // if open-loop do nothing if( controller==MotionControlType::angle_openloop || controller==MotionControlType::velocity_openloop ) return; - // shaft angle + // shaft angle shaft_angle = shaftAngle(); - + // if disabled do nothing - if(!enabled) return; + if(!enabled) return; electrical_angle = electricalAngle(); - // set the phase voltage - FOC heart function :) + // set the phase voltage - FOC heart function :) setPhaseVoltage(voltage.q, voltage.d, electrical_angle); } @@ -239,7 +239,7 @@ void StepperMotor::move(float new_target) { // get angular velocity shaft_velocity = shaftVelocity(); // if disabled do nothing - if(!enabled) return; + if(!enabled) return; // downsampling (optional) if(motion_cnt++ < motion_downsample) return; motion_cnt = 0; @@ -249,7 +249,7 @@ void StepperMotor::move(float new_target) { switch (controller) { case MotionControlType::torque: if(!_isset(phase_resistance)) voltage.q = target; // if voltage torque control - else voltage.q = target*phase_resistance; + else voltage.q = target*phase_resistance; voltage.d = 0; break; case MotionControlType::angle: @@ -259,7 +259,7 @@ void StepperMotor::move(float new_target) { shaft_velocity_sp = P_angle( shaft_angle_sp - shaft_angle ); // calculate the torque command current_sp = PID_velocity(shaft_velocity_sp - shaft_velocity); // if voltage torque control - // if torque controlled through voltage + // if torque controlled through voltage // use voltage if phase-resistance not provided if(!_isset(phase_resistance)) voltage.q = current_sp; else voltage.q = current_sp*phase_resistance; @@ -270,7 +270,7 @@ void StepperMotor::move(float new_target) { shaft_velocity_sp = target; // calculate the torque command current_sp = PID_velocity(shaft_velocity_sp - shaft_velocity); // if current/foc_current torque control - // if torque controlled through voltage control + // if torque controlled through voltage control // use voltage if phase-resistance not provided if(!_isset(phase_resistance)) voltage.q = current_sp; else voltage.q = current_sp*phase_resistance; @@ -295,12 +295,12 @@ void StepperMotor::move(float new_target) { // Method using FOC to set Uq and Ud to the motor at the optimal angle // Function implementing Sine PWM algorithms // - space vector not implemented yet -// +// // Function using sine approximation // regular sin + cos ~300us (no memory usaage) // approx _sin + _cos ~110us (400Byte ~ 20% of memory) void StepperMotor::setPhaseVoltage(float Uq, float Ud, float angle_el) { - // Sinusoidal PWM modulation + // Sinusoidal PWM modulation // Inverse Park transformation // angle normalization in between 0 and 2pi @@ -323,18 +323,18 @@ float StepperMotor::velocityOpenloop(float target_velocity){ // get current timestamp unsigned long now_us = _micros(); // calculate the sample time from last call - float Ts = (now_us - open_loop_timestamp) * 1e-6; + float Ts = (now_us - open_loop_timestamp) * 1e-6f; // quick fix for strange cases (micros overflow + timestamp not defined) - if(Ts <= 0 || Ts > 0.5) Ts = 1e-3; + if(Ts <= 0 || Ts > 0.5f) Ts = 1e-3f; // calculate the necessary angle to achieve target velocity shaft_angle = _normalizeAngle(shaft_angle + target_velocity*Ts); // for display purposes shaft_velocity = target_velocity; - + // use voltage limit or current limit float Uq = voltage_limit; - if(_isset(phase_resistance)) Uq = current_limit*phase_resistance; + if(_isset(phase_resistance)) Uq = current_limit*phase_resistance; // set the maximal allowed voltage (voltage_limit) with the necessary angle setPhaseVoltage(Uq, 0, _electricalAngle(shaft_angle, pole_pairs)); @@ -352,9 +352,9 @@ float StepperMotor::angleOpenloop(float target_angle){ // get current timestamp unsigned long now_us = _micros(); // calculate the sample time from last call - float Ts = (now_us - open_loop_timestamp) * 1e-6; + float Ts = (now_us - open_loop_timestamp) * 1e-6f; // quick fix for strange cases (micros overflow + timestamp not defined) - if(Ts <= 0 || Ts > 0.5) Ts = 1e-3; + if(Ts <= 0 || Ts > 0.5f) Ts = 1e-3f; // calculate the necessary angle to move from current position towards target angle // with maximal velocity (velocity_limit) @@ -368,12 +368,12 @@ float StepperMotor::angleOpenloop(float target_angle){ // use voltage limit or current limit float Uq = voltage_limit; - if(_isset(phase_resistance)) Uq = current_limit*phase_resistance; + if(_isset(phase_resistance)) Uq = current_limit*phase_resistance; // set the maximal allowed voltage (voltage_limit) with the necessary angle setPhaseVoltage(Uq, 0, _electricalAngle(shaft_angle, pole_pairs)); // save timestamp for next call open_loop_timestamp = now_us; - + return Uq; } \ No newline at end of file diff --git a/src/common/base_classes/BLDCDriver.h b/src/common/base_classes/BLDCDriver.h index 5764aa30..a657d17c 100644 --- a/src/common/base_classes/BLDCDriver.h +++ b/src/common/base_classes/BLDCDriver.h @@ -5,7 +5,7 @@ class BLDCDriver{ public: - + /** Initialise hardware */ virtual int init() = 0; /** Enable hardware */ @@ -14,26 +14,26 @@ class BLDCDriver{ virtual void disable() = 0; long pwm_frequency; //!< pwm frequency value in hertz - float voltage_power_supply; //!< power supply voltage + float voltage_power_supply; //!< power supply voltage float voltage_limit; //!< limiting voltage set to the motor - + float dc_a; //!< currently set duty cycle on phaseA float dc_b; //!< currently set duty cycle on phaseB float dc_c; //!< currently set duty cycle on phaseC - - /** - * Set phase voltages to the harware - * + + /** + * Set phase voltages to the harware + * * @param Ua - phase A voltage * @param Ub - phase B voltage * @param Uc - phase C voltage */ virtual void setPwm(float Ua, float Ub, float Uc) = 0; - /** - * Set phase state, enable/disable - * + /** + * Set phase state, enable/disable + * * @param sc - phase A state : active / disabled ( high impedance ) * @param sb - phase B state : active / disabled ( high impedance ) * @param sa - phase C state : active / disabled ( high impedance ) @@ -41,4 +41,4 @@ class BLDCDriver{ virtual void setPhaseState(int sa, int sb, int sc) = 0; }; -#endif \ No newline at end of file +#endif diff --git a/src/common/base_classes/Sensor.cpp b/src/common/base_classes/Sensor.cpp index fba66907..81e80aa2 100644 --- a/src/common/base_classes/Sensor.cpp +++ b/src/common/base_classes/Sensor.cpp @@ -2,7 +2,7 @@ #include "../foc_utils.h" #include "../time_utils.h" - /** + /** * returns 0 if it does need search for absolute zero * 0 - magnetic sensor (& encoder with index which is found) * 1 - ecoder with index (with index not found yet) @@ -16,15 +16,15 @@ float Sensor::getVelocity(){ // calculate sample time unsigned long now_us = _micros(); - float Ts = (now_us - velocity_calc_timestamp)*1e-6; + float Ts = (now_us - velocity_calc_timestamp)*1e-6f; // quick fix for strange cases (micros overflow) - if(Ts <= 0 || Ts > 0.5) Ts = 1e-3; + if(Ts <= 0 || Ts > 0.5f) Ts = 1e-3f; // current angle float angle_c = getAngle(); // velocity calculation float vel = (angle_c - angle_prev)/Ts; - + // save variables for future pass angle_prev = angle_c; velocity_calc_timestamp = now_us; diff --git a/src/common/defaults.h b/src/common/defaults.h index 71f39098..df651e2a 100644 --- a/src/common/defaults.h +++ b/src/common/defaults.h @@ -1,12 +1,12 @@ // default configuration values // change this file to optimal values for your application -#define DEF_POWER_SUPPLY 12.0 //!< default power supply voltage +#define DEF_POWER_SUPPLY 12.0f //!< default power supply voltage // velocity PI controller params -#define DEF_PID_VEL_P 0.5 //!< default PID controller P value -#define DEF_PID_VEL_I 10.0 //!< default PID controller I value -#define DEF_PID_VEL_D 0.0 //!< default PID controller D value -#define DEF_PID_VEL_RAMP 1000.0 //!< default PID controller voltage ramp value +#define DEF_PID_VEL_P 0.5f //!< default PID controller P value +#define DEF_PID_VEL_I 10.0f //!< default PID controller I value +#define DEF_PID_VEL_D 0.0f //!< default PID controller D value +#define DEF_PID_VEL_RAMP 1000.0f //!< default PID controller voltage ramp value #define DEF_PID_VEL_LIMIT (DEF_POWER_SUPPLY) //!< default PID controller voltage limit // current sensing PID values @@ -14,33 +14,36 @@ // for 16Mhz controllers like Arduino uno and mega #define DEF_PID_CURR_P 2 //!< default PID controller P value #define DEF_PID_CURR_I 100 //!< default PID controller I value -#define DEF_PID_CURR_D 0.0 //!< default PID controller D value -#define DEF_PID_CURR_RAMP 1000.0 //!< default PID controller voltage ramp value +#define DEF_PID_CURR_D 0.0f //!< default PID controller D value +#define DEF_PID_CURR_RAMP 1000.0f //!< default PID controller voltage ramp value #define DEF_PID_CURR_LIMIT (DEF_POWER_SUPPLY) //!< default PID controller voltage limit -#define DEF_CURR_FILTER_Tf 0.01 //!< default velocity filter time constant -#else +#define DEF_CURR_FILTER_Tf 0.01f //!< default velocity filter time constant +#else // for stm32, due, teensy, esp32 and similar #define DEF_PID_CURR_P 3 //!< default PID controller P value -#define DEF_PID_CURR_I 300.0 //!< default PID controller I value -#define DEF_PID_CURR_D 0.0 //!< default PID controller D value +#define DEF_PID_CURR_I 300.0f //!< default PID controller I value +#define DEF_PID_CURR_D 0.0f //!< default PID controller D value #define DEF_PID_CURR_RAMP 0 //!< default PID controller voltage ramp value #define DEF_PID_CURR_LIMIT (DEF_POWER_SUPPLY) //!< default PID controller voltage limit -#define DEF_CURR_FILTER_Tf 0.005 //!< default currnet filter time constant +#define DEF_CURR_FILTER_Tf 0.005f //!< default currnet filter time constant #endif // default current limit values -#define DEF_CURRENT_LIM 0.2 //!< 2Amps current limit by default +#define DEF_CURRENT_LIM 0.2f //!< 2Amps current limit by default // default monitor downsample #define DEF_MON_DOWNSMAPLE 100 //!< default monitor downsample -#define DEF_MOTION_DOWNSMAPLE 0 //!< default motion downsample - disable +#define DEF_MOTION_DOWNSMAPLE 0 //!< default motion downsample - disable // angle P params -#define DEF_P_ANGLE_P 20.0 //!< default P controller P value -#define DEF_VEL_LIM 20.0 //!< angle velocity limit default +#define DEF_P_ANGLE_P 20.0f //!< default P controller P value +#define DEF_VEL_LIM 20.0f //!< angle velocity limit default -// index search -#define DEF_INDEX_SEARCH_TARGET_VELOCITY 1.0 //!< default index search velocity +// index search +#define DEF_INDEX_SEARCH_TARGET_VELOCITY 1.0f //!< default index search velocity // align voltage -#define DEF_VOLTAGE_SENSOR_ALIGN 3.0 //!< default voltage for sensor and motor zero alignemt +#define DEF_VOLTAGE_SENSOR_ALIGN 3.0f //!< default voltage for sensor and motor zero alignemt // low pass filter velocity -#define DEF_VEL_FILTER_Tf 0.005 //!< default velocity filter time constant \ No newline at end of file +#define DEF_VEL_FILTER_Tf 0.005 //!< default velocity filter time constant + +// current sense default parameters +#define DEF_LPF_PER_PHASE_CURRENT_SENSE_Tf 0.0f //!< default currnet sense per phase low pass filter time constant diff --git a/src/common/foc_utils.cpp b/src/common/foc_utils.cpp index 7653f9be..58ac36e5 100644 --- a/src/common/foc_utils.cpp +++ b/src/common/foc_utils.cpp @@ -1,6 +1,6 @@ #include "foc_utils.h" -// int array instead of float array +// int array instead of float array // 4x200 points per 360 deg // 2x storage save (int 2Byte float 4 Byte ) // sin*10000 @@ -13,21 +13,21 @@ const int sine_array[200] = {0,79,158,237,316,395,473,552,631,710,789,867,946,10 // it has to receive an angle in between 0 and 2PI float _sin(float a){ if(a < _PI_2){ - //return sine_array[(int)(199.0*( a / (_PI/2.0)))]; - //return sine_array[(int)(126.6873* a)]; // float array optimized - return 0.0001*sine_array[_round(126.6873* a)]; // int array optimized + //return sine_array[(int)(199.0f*( a / (_PI/2.0)))]; + //return sine_array[(int)(126.6873f* a)]; // float array optimized + return 0.0001f*sine_array[_round(126.6873f* a)]; // int array optimized }else if(a < _PI){ - // return sine_array[(int)(199.0*(1.0 - (a-_PI/2.0) / (_PI/2.0)))]; - //return sine_array[398 - (int)(126.6873*a)]; // float array optimized - return 0.0001*sine_array[398 - _round(126.6873*a)]; // int array optimized + // return sine_array[(int)(199.0f*(1.0f - (a-_PI/2.0) / (_PI/2.0)))]; + //return sine_array[398 - (int)(126.6873f*a)]; // float array optimized + return 0.0001f*sine_array[398 - _round(126.6873f*a)]; // int array optimized }else if(a < _3PI_2){ - // return -sine_array[(int)(199.0*((a - _PI) / (_PI/2.0)))]; - //return -sine_array[-398 + (int)(126.6873*a)]; // float array optimized - return -0.0001*sine_array[-398 + _round(126.6873*a)]; // int array optimized + // return -sine_array[(int)(199.0f*((a - _PI) / (_PI/2.0)))]; + //return -sine_array[-398 + (int)(126.6873f*a)]; // float array optimized + return -0.0001*sine_array[-398 + _round(126.6873f*a)]; // int array optimized } else { - // return -sine_array[(int)(199.0*(1.0 - (a - 3*_PI/2) / (_PI/2.0)))]; - //return -sine_array[796 - (int)(126.6873*a)]; // float array optimized - return -0.0001*sine_array[796 - _round(126.6873*a)]; // int array optimized + // return -sine_array[(int)(199.0f*(1.0f - (a - 3*_PI/2) / (_PI/2.0)))]; + //return -sine_array[796 - (int)(126.6873f*a)]; // float array optimized + return -0.0001*sine_array[796 - _round(126.6873f*a)]; // int array optimized } } @@ -54,7 +54,7 @@ float _electricalAngle(float shaft_angle, int pole_pairs) { return (shaft_angle * pole_pairs); } -// square root approximation function using +// square root approximation function using // https://reprap.org/forum/read.php?147,219210 // https://en.wikipedia.org/wiki/Fast_inverse_square_root float _sqrtApprox(float number) {//low in fat @@ -67,7 +67,7 @@ float _sqrtApprox(float number) {//low in fat y = number; i = * ( long * ) &y; i = 0x5f375a86 - ( i >> 1 ); - y = * ( float * ) &i; + y = * ( float * ) &i; // y = y * ( f - ( x * y * y ) ); // better precision return number * y; } diff --git a/src/common/foc_utils.h b/src/common/foc_utils.h index 30e99b4f..e58fbaa0 100644 --- a/src/common/foc_utils.h +++ b/src/common/foc_utils.h @@ -12,31 +12,31 @@ #define _UNUSED(v) (void) (v) // utility defines -#define _2_SQRT3 1.15470053838 -#define _SQRT3 1.73205080757 -#define _1_SQRT3 0.57735026919 -#define _SQRT3_2 0.86602540378 -#define _SQRT2 1.41421356237 -#define _120_D2R 2.09439510239 -#define _PI 3.14159265359 -#define _PI_2 1.57079632679 -#define _PI_3 1.0471975512 -#define _2PI 6.28318530718 -#define _3PI_2 4.71238898038 -#define _PI_6 0.52359877559 +#define _2_SQRT3 1.15470053838f +#define _SQRT3 1.73205080757f +#define _1_SQRT3 0.57735026919f +#define _SQRT3_2 0.86602540378f +#define _SQRT2 1.41421356237f +#define _120_D2R 2.09439510239f +#define _PI 3.14159265359f +#define _PI_2 1.57079632679f +#define _PI_3 1.0471975512f +#define _2PI 6.28318530718f +#define _3PI_2 4.71238898038f +#define _PI_6 0.52359877559f #define NOT_SET -12345.0 #define _HIGH_IMPEDANCE 0 #define _HIGH_Z _HIGH_IMPEDANCE #define _ACTIVE 1 -// dq current structure +// dq current structure struct DQCurrent_s { float d; float q; }; -// phase current structure +// phase current structure struct PhaseCurrent_s { float a; @@ -54,27 +54,27 @@ struct DQVoltage_s /** * Function approximating the sine calculation by using fixed size array * - execution time ~40us (Arduino UNO) - * + * * @param a angle in between 0 and 2PI */ float _sin(float a); /** * Function approximating cosine calculation by using fixed size array * - execution time ~50us (Arduino UNO) - * + * * @param a angle in between 0 and 2PI */ float _cos(float a); -/** - * normalizing radian angle to [0,2PI] +/** + * normalizing radian angle to [0,2PI] * @param angle - angle to be normalized - */ + */ float _normalizeAngle(float angle); - -/** - * Electrical angle calculation + +/** + * Electrical angle calculation * * @param shaft_angle - shaft angle of the motor * @param pole_pairs - number of pole pairs @@ -84,7 +84,7 @@ float _electricalAngle(float shaft_angle, int pole_pairs); /** * Function approximating square root function * - using fast inverse square root - * + * * @param value - number */ float _sqrtApprox(float value); diff --git a/src/common/pid.cpp b/src/common/pid.cpp index 5290d814..c887c35b 100644 --- a/src/common/pid.cpp +++ b/src/common/pid.cpp @@ -6,9 +6,9 @@ PIDController::PIDController(float P, float I, float D, float ramp, float limit) , D(D) , output_ramp(ramp) // output derivative limit [volts/second] , limit(limit) // output supply limit [volts] - , integral_prev(0.0) - , error_prev(0.0) - , output_prev(0.0) + , integral_prev(0.0f) + , error_prev(0.0f) + , output_prev(0.0f) { timestamp_prev = _micros(); } @@ -17,13 +17,13 @@ PIDController::PIDController(float P, float I, float D, float ramp, float limit) float PIDController::operator() (float error){ // calculate the time from the last call unsigned long timestamp_now = _micros(); - float Ts = (timestamp_now - timestamp_prev) * 1e-6; + float Ts = (timestamp_now - timestamp_prev) * 1e-6f; // quick fix for strange cases (micros overflow) - if(Ts <= 0 || Ts > 0.5) Ts = 1e-3; + if(Ts <= 0 || Ts > 0.5f) Ts = 1e-3f; // u(s) = (P + I/s + Ds)e(s) // Discrete implementations - // proportional part + // proportional part // u_p = P *e(k) float proportional = P * error; // Tustin transform of the integral part @@ -56,4 +56,3 @@ float PIDController::operator() (float error){ timestamp_prev = timestamp_now; return output; } - diff --git a/src/communication/Commander.h b/src/communication/Commander.h index aaa55b11..13777867 100644 --- a/src/communication/Commander.h +++ b/src/communication/Commander.h @@ -24,14 +24,14 @@ typedef void (* CommandCallback)(char*); //!< command callback function pointer /** * Commander class implementing string communication protocol based on IDvalue (ex AB5.321 - command id `A`, sub-command id `B`,value `5.321`) - * - * - This class can be used in combination with HardwareSerial instance which it would read and write + * + * - This class can be used in combination with HardwareSerial instance which it would read and write * or it can be used to parse strings that have been received from the user outside this library * - Commander class implements command protocol for few standard components of the SimpleFOC library * - FOCMotor * - PIDController * - LowPassFilter - * - Commander also provides a very simple command > callback interface that enables user to + * - Commander also provides a very simple command > callback interface that enables user to * attach a callback function to certain command id - see function add() */ class Commander @@ -40,7 +40,7 @@ class Commander /** * Default constructor receiving a serial interface that it uses to output the values to * Also if the function run() is used it uses this serial instance to read the serial for user commands - * + * * @param serial - Serial com port instance * @param eol - the end of line sentinel character * @param echo - echo last typed character (for command line feedback) @@ -49,45 +49,45 @@ class Commander Commander(char eol = '\n', bool echo = false); /** - * Function reading the serial port and firing callbacks that have been added to the commander - * once the user has requested them - when he sends the command - * + * Function reading the serial port and firing callbacks that have been added to the commander + * once the user has requested them - when he sends the command + * * - It has default commands (the letters can be changed in the commands.h file) - * '@' - Verbose mode + * '@' - Verbose mode * '#' - Number of decimal places - * '?' - Scan command - displays all the labels of attached nodes - */ + * '?' - Scan command - displays all the labels of attached nodes + */ void run(); /** - * Function reading the string of user input and firing callbacks that have been added to the commander - * once the user has requested them - when he sends the command - * + * Function reading the string of user input and firing callbacks that have been added to the commander + * once the user has requested them - when he sends the command + * * - It has default commands (the letters can be changed in the commands.h file) - * '@' - Verbose mode + * '@' - Verbose mode * '#' - Number of decimal places * '?' - Scan command - displays all the labels of attached nodes - * + * * @param reader - temporary stream to read user input * @param eol - temporary end of line sentinel - */ + */ void run(Stream &reader, char eol = '\n'); /** - * Function reading the string of user input and firing callbacks that have been added to the commander - * once the user has requested them - when he sends the command - * + * Function reading the string of user input and firing callbacks that have been added to the commander + * once the user has requested them - when he sends the command + * * - It has default commands (the letters can be changed in the commands.h file) - * '@' - Verbose mode + * '@' - Verbose mode * '#' - Number of decimal places * '?' - Scan command - displays all the labels of attached nodes - * + * * @param user_input - string of user inputs - */ + */ void run(char* user_input); /** * Function adding a callback to the coomander withe the command id * @param id - char command letter - * @param onCommand - function pointer void function(char*) + * @param onCommand - function pointer void function(char*) * @param label - string label to be displayed when scan command sent */ void add(char id , CommandCallback onCommand, char* label = nullptr); @@ -101,48 +101,48 @@ class Commander char eol = '\n'; //!< end of line sentinel character bool echo = false; //!< echo last typed character (for command line feedback) /** - * + * * FOC motor (StepperMotor and BLDCMotor) command interface * - It has several paramters (the letters can be changed in the commands.h file) * 'Q' - Q current PID controller & LPF (see function pid and lpf for commands) - * 'D' - D current PID controller & LPF (see function pid and lpf for commands) - * 'V' - Velocity PID controller & LPF (see function pid and lpf for commands) - * 'A' - Angle PID controller & LPF (see function pid and lpf for commands) - * 'L' - Limits + * 'D' - D current PID controller & LPF (see function pid and lpf for commands) + * 'V' - Velocity PID controller & LPF (see function pid and lpf for commands) + * 'A' - Angle PID controller & LPF (see function pid and lpf for commands) + * 'L' - Limits * sub-commands: - * 'C' - Current - * 'U' - Voltage - * 'V' - Velocity - * 'C' - Motion control type config + * 'C' - Current + * 'U' - Voltage + * 'V' - Velocity + * 'C' - Motion control type config * sub-commands: - * 'D' - downsample motiron loop - * '0' - torque - * '1' - velocity - * '2' - angle - * 'T' - Torque control type + * 'D' - downsample motiron loop + * '0' - torque + * '1' - velocity + * '2' - angle + * 'T' - Torque control type * sub-commands: - * '0' - voltage - * '1' - current - * '2' - foc_current - * 'E' - Motor status (enable/disable) + * '0' - voltage + * '1' - current + * '2' - foc_current + * 'E' - Motor status (enable/disable) * sub-commands: - * '0' - enable - * '1' - disable - * 'R' - Motor resistance - * 'S' - Sensor offsets + * '0' - enable + * '1' - disable + * 'R' - Motor resistance + * 'S' - Sensor offsets * sub-commands: - * 'M' - sensor offset - * 'E' - sensor electrical zero - * 'M' - Monitoring control + * 'M' - sensor offset + * 'E' - sensor electrical zero + * 'M' - Monitoring control * sub-commands: - * 'D' - downsample monitoring - * 'C' - clear monitor - * 'S' - set monitoring variables - * 'G' - get variable value - * '' - Target get/set - * + * 'D' - downsample monitoring + * 'C' - clear monitor + * 'S' - set monitoring variables + * 'G' - get variable value + * '' - Target get/set + * * - Each of them can be get by sening the command letter -(ex. 'R' - to get the phase resistance) - * - Each of them can be set by sending 'IdSubidValue' - (ex. SM1.5 for setting sensor zero offset to 1.5) + * - Each of them can be set by sending 'IdSubidValue' - (ex. SM1.5 for setting sensor zero offset to 1.5f) * */ void motor(FOCMotor* motor, char* user_cmd); @@ -170,7 +170,7 @@ class Commander * Float variable scalar command interface * - It only has one property - one float value * - It can be get by sending an empty string '\n' - * - It can be set by sending 'value' - (ex. 0.01 for settin *value=0.01) + * - It can be set by sending 'value' - (ex. 0.01f for settin *value=0.01) */ void scalar(float* value, char* user_cmd); @@ -184,19 +184,19 @@ class Commander // helping variable for serial communication reading char received_chars[MAX_COMMAND_LENGTH] = {0}; //!< so far received user message - waiting for newline int rec_cnt = 0; //!< number of characters receives - + // serial printing functions /** * print the string message only if verbose mode on * @param message - message to be printed */ - void printVerbose(const char* message); + void printVerbose(const char* message); /** - * Print the string message only if verbose mode on + * Print the string message only if verbose mode on * - Function handling the case for strings defined by F macro * @param message - message to be printed */ - void printVerbose(const __FlashStringHelper *message); + void printVerbose(const __FlashStringHelper *message); /** * print the numbers to the serial with desired decimal point number * @param message - number to be printed diff --git a/src/current_sense/InlineCurrentSense.cpp b/src/current_sense/InlineCurrentSense.cpp index 879395d1..43e78b9f 100644 --- a/src/current_sense/InlineCurrentSense.cpp +++ b/src/current_sense/InlineCurrentSense.cpp @@ -12,7 +12,7 @@ InlineCurrentSense::InlineCurrentSense(float _shunt_resistor, float _gain, int _ shunt_resistor = _shunt_resistor; amp_gain = _gain; - volts_to_amps_ratio = 1.0 /_shunt_resistor / _gain; // volts to amps + volts_to_amps_ratio = 1.0f /_shunt_resistor / _gain; // volts to amps // gains for each phase gain_a = volts_to_amps_ratio; gain_b = volts_to_amps_ratio; @@ -72,36 +72,36 @@ int InlineCurrentSense::driverSync(BLDCDriver *driver){ int InlineCurrentSense::driverAlign(BLDCDriver *driver, float voltage){ int exit_flag = 1; if(skip_align) return exit_flag; - + // set phase A active and phases B and C down driver->setPwm(voltage, 0, 0); - _delay(200); + _delay(200); PhaseCurrent_s c = getPhaseCurrents(); // read the current 100 times ( arbitrary number ) for (int i = 0; i < 100; i++) { PhaseCurrent_s c1 = getPhaseCurrents(); - c.a = c.a*0.6 + 0.4*c1.a; - c.b = c.b*0.6 + 0.4*c1.b; - c.c = c.c*0.6 + 0.4*c1.c; + c.a = c.a*0.6 + 0.4f*c1.a; + c.b = c.b*0.6 + 0.4f*c1.b; + c.c = c.c*0.6 + 0.4f*c1.c; _delay(3); } driver->setPwm(0, 0, 0); // align phase A float ab_ratio = fabs(c.a / c.b); float ac_ratio = c.c ? fabs(c.a / c.c) : 0; - if( ab_ratio > 1.5 ){ // should be ~2 + if( ab_ratio > 1.5f ){ // should be ~2 gain_a *= _sign(c.a); - }else if( ab_ratio < 0.7 ){ // should be ~0.5 + }else if( ab_ratio < 0.7f ){ // should be ~0.5 // switch phase A and B int tmp_pinA = pinA; - pinA = pinB; + pinA = pinB; pinB = tmp_pinA; gain_a *= _sign(c.b); exit_flag = 2; // signal that pins have been switched - }else if(_isset(pinC) && ac_ratio < 0.7 ){ // should be ~0.5 + }else if(_isset(pinC) && ac_ratio < 0.7f ){ // should be ~0.5 // switch phase A and C int tmp_pinA = pinA; - pinA = pinC; + pinA = pinC; pinC= tmp_pinA; gain_a *= _sign(c.c); exit_flag = 2;// signal that pins have been switched @@ -109,35 +109,35 @@ int InlineCurrentSense::driverAlign(BLDCDriver *driver, float voltage){ // error in current sense - phase either not measured or bad connection return 0; } - + // set phase B active and phases A and C down driver->setPwm(0, voltage, 0); - _delay(200); + _delay(200); c = getPhaseCurrents(); // read the current 50 times for (int i = 0; i < 100; i++) { PhaseCurrent_s c1 = getPhaseCurrents(); - c.a = c.a*0.6 + 0.4*c1.a; - c.b = c.b*0.6 + 0.4*c1.b; - c.c = c.c*0.6 + 0.4*c1.c; + c.a = c.a*0.6 + 0.4f*c1.a; + c.b = c.b*0.6 + 0.4f*c1.b; + c.c = c.c*0.6 + 0.4f*c1.c; _delay(3); } driver->setPwm(0, 0, 0); float ba_ratio = fabs(c.b/c.a); float bc_ratio = c.c ? fabs(c.b / c.c) : 0; - if( ba_ratio > 1.5 ){ // should be ~2 + if( ba_ratio > 1.5f ){ // should be ~2 gain_b *= _sign(c.b); - }else if( ba_ratio < 0.7 ){ // it should be ~0.5 + }else if( ba_ratio < 0.7f ){ // it should be ~0.5 // switch phase A and B int tmp_pinB = pinB; - pinB = pinA; + pinB = pinA; pinA = tmp_pinB; gain_b *= _sign(c.a); exit_flag = 2; // signal that pins have been switched - }else if(_isset(pinC) && bc_ratio < 0.7 ){ // should be ~0.5 + }else if(_isset(pinC) && bc_ratio < 0.7f ){ // should be ~0.5 // switch phase A and C int tmp_pinB = pinB; - pinB = pinC; + pinB = pinC; pinC = tmp_pinB; gain_b *= _sign(c.c); exit_flag = 2; // signal that pins have been switched @@ -150,7 +150,7 @@ int InlineCurrentSense::driverAlign(BLDCDriver *driver, float voltage){ if(_isset(pinC)){ // set phase B active and phases A and C down driver->setPwm(0, 0, voltage); - _delay(200); + _delay(200); c = getPhaseCurrents(); // read the adc voltage 500 times ( arbitrary number ) for (int i = 0; i < 50; i++) { @@ -170,4 +170,3 @@ int InlineCurrentSense::driverAlign(BLDCDriver *driver, float voltage){ // 4 - success but pins reconfigured and gains inverted return exit_flag; } - diff --git a/src/current_sense/InlineCurrentSense.h b/src/current_sense/InlineCurrentSense.h index da46d104..615c4733 100644 --- a/src/current_sense/InlineCurrentSense.h +++ b/src/current_sense/InlineCurrentSense.h @@ -4,7 +4,9 @@ #include "Arduino.h" #include "../common/foc_utils.h" #include "../common/time_utils.h" +#include "../common/defaults.h" #include "../common/base_classes/CurrentSense.h" +#include "../common/lowpass_filter.h" #include "hardware_api.h" @@ -33,8 +35,13 @@ class InlineCurrentSense: public CurrentSense{ float gain_b; //!< phase B gain float gain_c; //!< phase C gain - private: + // // per phase low pass fileters + // LowPassFilter lpf_a{DEF_LPF_PER_PHASE_CURRENT_SENSE_Tf}; //!< current A low pass filter + // LowPassFilter lpf_b{DEF_LPF_PER_PHASE_CURRENT_SENSE_Tf}; //!< current B low pass filter + // LowPassFilter lpf_c{DEF_LPF_PER_PHASE_CURRENT_SENSE_Tf}; //!< current C low pass filter + private: + // hardware variables int pinA; //!< pin A analog pin for current measurement int pinB; //!< pin B analog pin for current measurement diff --git a/src/current_sense/LowsideCurrentSense.cpp b/src/current_sense/LowsideCurrentSense.cpp index d7cc7168..fc6823a3 100644 --- a/src/current_sense/LowsideCurrentSense.cpp +++ b/src/current_sense/LowsideCurrentSense.cpp @@ -12,7 +12,7 @@ LowsideCurrentSense::LowsideCurrentSense(float _shunt_resistor, float _gain, int shunt_resistor = _shunt_resistor; amp_gain = _gain; - volts_to_amps_ratio = 1.0 /_shunt_resistor / _gain; // volts to amps + volts_to_amps_ratio = 1.0f /_shunt_resistor / _gain; // volts to amps // gains for each phase gain_a = volts_to_amps_ratio; gain_b = volts_to_amps_ratio; @@ -27,30 +27,40 @@ void LowsideCurrentSense::init(){ calibrateOffsets(); } // Function finding zero offsets of the ADC -void LowsideCurrentSense::calibrateOffsets(){ +uint32_t LowsideCurrentSense::calibrateOffsets(const uint32_t calibration_rounds){ + + // find adc offset = zero current voltage offset_ia =0; offset_ib= 0; offset_ic= 0; + uint32_t effective_rounds = 0; float a, b, c; // read the adc voltage 1000 times ( arbitrary number ) - for (int i = 0; i < 1000; i++) { - _readADCVoltagesLowSide(a, b, c); - offset_ia += a; - offset_ib += b; - if(_isset(pinC)) offset_ic += c; - _delay(1); + for (int i = 0; i < calibration_rounds; i++) + { + if(_readADCVoltagesLowSide(a, b, c)) + { + offset_ia += a; + offset_ib += b; + if(_isset(pinC)) offset_ic += c; + effective_rounds++; + } + } + if(effective_rounds > 0) + { + // calculate the mean offsets + offset_ia = offset_ia / effective_rounds; + offset_ib = offset_ib / effective_rounds; + if(_isset(pinC)) offset_ic = offset_ic / effective_rounds; } - // calculate the mean offsets - offset_ia = offset_ia / 1000.0f; - offset_ib = offset_ib / 1000.0f; - if(_isset(pinC)) offset_ic = offset_ic / 1000.0f; + + return effective_rounds; } // read all three phase currents (if possible 2 or 3) PhaseCurrent_s LowsideCurrentSense::getPhaseCurrents(){ PhaseCurrent_s current; - _readADCVoltagesLowSide(current.a, current.b, current.c); current.a = (current.a - offset_ia)*gain_a;// amps current.b = (current.b - offset_ib)*gain_b;// amps diff --git a/src/current_sense/LowsideCurrentSense.h b/src/current_sense/LowsideCurrentSense.h index 2300d5f6..45f68462 100644 --- a/src/current_sense/LowsideCurrentSense.h +++ b/src/current_sense/LowsideCurrentSense.h @@ -4,8 +4,10 @@ #include "Arduino.h" #include "../common/foc_utils.h" #include "../common/time_utils.h" +#include "../common/defaults.h" #include "../common/base_classes/CurrentSense.h" #include "../common/base_classes/FOCMotor.h" +#include "../common/lowpass_filter.h" #include "hardware_api.h" @@ -23,6 +25,10 @@ class LowsideCurrentSense: public CurrentSense{ // CurrentSense interface implementing functions void init() override; + /** + * Function finding zero offsets of the ADC + */ + uint32_t calibrateOffsets(const uint32_t calibration_rounds = 1000); PhaseCurrent_s getPhaseCurrents() override; int driverSync(BLDCDriver *driver) override; int driverAlign(BLDCDriver *driver, float voltage) override; @@ -34,6 +40,11 @@ class LowsideCurrentSense: public CurrentSense{ float gain_b; //!< phase B gain float gain_c; //!< phase C gain + // // per phase low pass fileters + // LowPassFilter lpf_a{DEF_LPF_PER_PHASE_CURRENT_SENSE_Tf}; //!< current A low pass filter + // LowPassFilter lpf_b{DEF_LPF_PER_PHASE_CURRENT_SENSE_Tf}; //!< current B low pass filter + // LowPassFilter lpf_c{DEF_LPF_PER_PHASE_CURRENT_SENSE_Tf}; //!< current C low pass filter + private: // hardware variables @@ -42,17 +53,13 @@ class LowsideCurrentSense: public CurrentSense{ int pinC; //!< pin C analog pin for current measurement // gain variables - double shunt_resistor; //!< Shunt resistor value - double amp_gain; //!< amp gain value - double volts_to_amps_ratio; //!< Volts to amps ratio + float shunt_resistor; //!< Shunt resistor value + float amp_gain; //!< amp gain value + float volts_to_amps_ratio; //!< Volts to amps ratio - /** - * Function finding zero offsets of the ADC - */ - void calibrateOffsets(); - double offset_ia; //!< zero current A voltage value (center of the adc reading) - double offset_ib; //!< zero current B voltage value (center of the adc reading) - double offset_ic; //!< zero current C voltage value (center of the adc reading) + float offset_ia; //!< zero current A voltage value (center of the adc reading) + float offset_ib; //!< zero current B voltage value (center of the adc reading) + float offset_ic; //!< zero current C voltage value (center of the adc reading) }; diff --git a/src/current_sense/hardware_api.h b/src/current_sense/hardware_api.h index b08325f4..852eacd5 100644 --- a/src/current_sense/hardware_api.h +++ b/src/current_sense/hardware_api.h @@ -1,19 +1,18 @@ -#ifndef HARDWARE_UTILS_H -#define HARDWARE_UTILS_H +#pragma once #include "../common/foc_utils.h" #include "../common/time_utils.h" /** * function reading an ADC value and returning the read voltage - * + * * @param pinA - the arduino pin to be read (it has to be ADC pin) */ float _readADCVoltageInline(const int pinA); /** * function reading an ADC value and returning the read voltage - * + * * @param pinA - adc pin A * @param pinB - adc pin B * @param pinC - adc pin C @@ -21,8 +20,8 @@ float _readADCVoltageInline(const int pinA); void _configureADCInline(const int pinA,const int pinB,const int pinC = NOT_SET); /** - * function reading an ADC value and returning the read voltage - * + * function reading an ADC value and returning the read voltage + * * @param pinA - adc pin A * @param pinB - adc pin B * @param pinC - adc pin C @@ -30,14 +29,12 @@ void _configureADCInline(const int pinA,const int pinB,const int pinC = NOT_SET) void _configureADCLowSide(const int pinA,const int pinB,const int pinC = NOT_SET); /** - * function reading an ADC value and returning the read voltage - * - * @param pinA - the arduino pin to be read (it has to be ADC pin) + * function returning 3 most recent voltage readings */ -void _readADCVoltagesLowSide(float & a, float & b, float & c); +bool _readADCVoltagesLowSide(float & a, float & b, float & c); /** * function syncing the Driver with the ADC for the LowSide Sensing */ void _driverSyncLowSide(); -#endif \ No newline at end of file + diff --git a/src/current_sense/hardware_specific/atmega_mcu.cpp b/src/current_sense/hardware_specific/atmega_mcu.cpp new file mode 100644 index 00000000..95917cc0 --- /dev/null +++ b/src/current_sense/hardware_specific/atmega_mcu.cpp @@ -0,0 +1,63 @@ +#include "../hardware_api.h" + +#if defined(__AVR_ATmega328P__) || defined(__AVR_ATmega168__) || defined(__AVR_ATmega328PB__) +#define _ADC_VOLTAGE 5.0f +#define _ADC_RESOLUTION 1024.0f + +// adc counts to voltage conversion ratio +// some optimizing for faster execution +#define _ADC_CONV ( (_ADC_VOLTAGE) / (_ADC_RESOLUTION) ) +int _pinA, _pinB, _pinC = NOT_SET; + +#ifndef cbi + #define cbi(sfr, bit) (_SFR_BYTE(sfr) &= ~_BV(bit)) +#endif +#ifndef sbi + #define sbi(sfr, bit) (_SFR_BYTE(sfr) |= _BV(bit)) +#endif + +// adc counts to voltage conversion ratio +// some optimizing for faster execution +#define _ADC_CONV ( (_ADC_VOLTAGE) / (_ADC_RESOLUTION) ) + +// function reading an ADC value and returning the read voltage +float _readADCVoltageInline(const int pinA){ + uint32_t raw_adc = analogRead(pinA); + return raw_adc * _ADC_CONV; +} +// function reading an ADC value and returning the read voltage +void _configureADCInline(const int pinA,const int pinB,const int pinC){ + pinMode(pinA, INPUT); + pinMode(pinB, INPUT); + if( _isset(pinC) ) pinMode(pinC, INPUT); + + // set hight frequency adc - ADPS2,ADPS1,ADPS0 | 001 (16mhz/2) | 010 ( 16mhz/4 ) | 011 (16mhz/8) | 100(16mhz/16) | 101 (16mhz/32) | 110 (16mhz/64) | 111 (16mhz/128 default) + // set divisor to 8 - adc frequency 16mhz/8 = 2 mhz + // arduino takes 25 conversions per sample so - 2mhz/25 = 80k samples per second - 12.5us per sample + cbi(ADCSRA, ADPS2); + sbi(ADCSRA, ADPS1); + sbi(ADCSRA, ADPS0); +} + + +bool _readADCVoltagesLowSide( float & a, float & b, float & c) +{ + a = _readADCVoltageInline(_pinA); + b = _readADCVoltageInline(_pinB); + if( _isset(_pinC) ) + c = _readADCVoltageInline(_pinC); + return true; +} +// Configure low side for generic mcu +// cannot do much but +void _configureADCLowSide(const int pinA,const int pinB,const int pinC){ + _pinA = pinA; + _pinB = pinB; + if( _isset(pinC) ) _pinC = pinC; + pinMode(pinA, INPUT); + pinMode(pinB, INPUT); + if( _isset(pinC) ) pinMode(pinC, INPUT); + +} + +#endif \ No newline at end of file diff --git a/src/current_sense/hardware_specific/due_mcu.cpp b/src/current_sense/hardware_specific/due_mcu.cpp new file mode 100644 index 00000000..c2fa5860 --- /dev/null +++ b/src/current_sense/hardware_specific/due_mcu.cpp @@ -0,0 +1,46 @@ +#include "../hardware_api.h" + +#if defined(__arm__) && defined(__SAM3X8E__) +#define _ADC_VOLTAGE 3.3f +#define _ADC_RESOLUTION 1024.0f + +// adc counts to voltage conversion ratio +// some optimizing for faster execution +#define _ADC_CONV ( (_ADC_VOLTAGE) / (_ADC_RESOLUTION) ) +int _pinA, _pinB, _pinC = NOT_SET; + +// function reading an ADC value and returning the read voltage +float _readADCVoltageInline(const int pinA){ + uint32_t raw_adc = analogRead(pinA); + return raw_adc * _ADC_CONV; +} +// function reading an ADC value and returning the read voltage +void _configureADCInline(const int pinA,const int pinB,const int pinC){ + pinMode(pinA, INPUT); + pinMode(pinB, INPUT); + if( _isset(pinC) ) pinMode(pinC, INPUT); +} + + +bool _readADCVoltagesLowSide( float & a, float & b, float & c) +{ + a = _readADCVoltageInline(_pinA); + b = _readADCVoltageInline(_pinB); + if( _isset(_pinC) ) + c = _readADCVoltageInline(_pinC); + return true; +} +// Configure low side for generic mcu +// cannot do much but +void _configureADCLowSide(const int pinA,const int pinB,const int pinC){ + _pinA = pinA; + _pinB = pinB; + if( _isset(pinC) ) _pinC = pinC; + pinMode(pinA, INPUT); + pinMode(pinB, INPUT); + if( _isset(pinC) ) pinMode(pinC, INPUT); + +} + + +#endif \ No newline at end of file diff --git a/src/current_sense/hardware_specific/esp32_adc_driver.cpp b/src/current_sense/hardware_specific/esp32_adc_driver.cpp new file mode 100644 index 00000000..01e1ca60 --- /dev/null +++ b/src/current_sense/hardware_specific/esp32_adc_driver.cpp @@ -0,0 +1,225 @@ +#include "esp32_adc_driver.h" + +#ifdef ESP_H + +#include "freertos/FreeRTOS.h" +#include "freertos/task.h" +#include "rom/ets_sys.h" +#include "esp_attr.h" +#include "esp_intr.h" +#include "soc/rtc_io_reg.h" +#include "soc/rtc_cntl_reg.h" +#include "soc/sens_reg.h" + +static uint8_t __analogAttenuation = 3;//11db +static uint8_t __analogWidth = 3;//12 bits +static uint8_t __analogCycles = 8; +static uint8_t __analogSamples = 0;//1 sample +static uint8_t __analogClockDiv = 1; + +// Width of returned answer () +static uint8_t __analogReturnedWidth = 12; + +void __analogSetWidth(uint8_t bits){ + if(bits < 9){ + bits = 9; + } else if(bits > 12){ + bits = 12; + } + __analogReturnedWidth = bits; + __analogWidth = bits - 9; + SET_PERI_REG_BITS(SENS_SAR_START_FORCE_REG, SENS_SAR1_BIT_WIDTH, __analogWidth, SENS_SAR1_BIT_WIDTH_S); + SET_PERI_REG_BITS(SENS_SAR_READ_CTRL_REG, SENS_SAR1_SAMPLE_BIT, __analogWidth, SENS_SAR1_SAMPLE_BIT_S); + + SET_PERI_REG_BITS(SENS_SAR_START_FORCE_REG, SENS_SAR2_BIT_WIDTH, __analogWidth, SENS_SAR2_BIT_WIDTH_S); + SET_PERI_REG_BITS(SENS_SAR_READ_CTRL2_REG, SENS_SAR2_SAMPLE_BIT, __analogWidth, SENS_SAR2_SAMPLE_BIT_S); +} + +void __analogSetCycles(uint8_t cycles){ + __analogCycles = cycles; + SET_PERI_REG_BITS(SENS_SAR_READ_CTRL_REG, SENS_SAR1_SAMPLE_CYCLE, __analogCycles, SENS_SAR1_SAMPLE_CYCLE_S); + SET_PERI_REG_BITS(SENS_SAR_READ_CTRL2_REG, SENS_SAR2_SAMPLE_CYCLE, __analogCycles, SENS_SAR2_SAMPLE_CYCLE_S); +} + +void __analogSetSamples(uint8_t samples){ + if(!samples){ + return; + } + __analogSamples = samples - 1; + SET_PERI_REG_BITS(SENS_SAR_READ_CTRL_REG, SENS_SAR1_SAMPLE_NUM, __analogSamples, SENS_SAR1_SAMPLE_NUM_S); + SET_PERI_REG_BITS(SENS_SAR_READ_CTRL2_REG, SENS_SAR2_SAMPLE_NUM, __analogSamples, SENS_SAR2_SAMPLE_NUM_S); +} + +void __analogSetClockDiv(uint8_t clockDiv){ + if(!clockDiv){ + return; + } + __analogClockDiv = clockDiv; + SET_PERI_REG_BITS(SENS_SAR_READ_CTRL_REG, SENS_SAR1_CLK_DIV, __analogClockDiv, SENS_SAR1_CLK_DIV_S); + SET_PERI_REG_BITS(SENS_SAR_READ_CTRL2_REG, SENS_SAR2_CLK_DIV, __analogClockDiv, SENS_SAR2_CLK_DIV_S); +} + +void __analogSetAttenuation(uint8_t attenuation) +{ + __analogAttenuation = attenuation & 3; + uint32_t att_data = 0; + int i = 10; + while(i--){ + att_data |= __analogAttenuation << (i * 2); + } + WRITE_PERI_REG(SENS_SAR_ATTEN1_REG, att_data & 0xFFFF);//ADC1 has 8 channels + WRITE_PERI_REG(SENS_SAR_ATTEN2_REG, att_data); +} + +void IRAM_ATTR __analogInit(){ + static bool initialized = false; + if(initialized){ + return; + } + + __analogSetAttenuation(__analogAttenuation); + __analogSetCycles(__analogCycles); + __analogSetSamples(__analogSamples + 1);//in samples + __analogSetClockDiv(__analogClockDiv); + __analogSetWidth(__analogWidth + 9);//in bits + + SET_PERI_REG_MASK(SENS_SAR_READ_CTRL_REG, SENS_SAR1_DATA_INV); + SET_PERI_REG_MASK(SENS_SAR_READ_CTRL2_REG, SENS_SAR2_DATA_INV); + + SET_PERI_REG_MASK(SENS_SAR_MEAS_START1_REG, SENS_MEAS1_START_FORCE_M); //SAR ADC1 controller (in RTC) is started by SW + SET_PERI_REG_MASK(SENS_SAR_MEAS_START1_REG, SENS_SAR1_EN_PAD_FORCE_M); //SAR ADC1 pad enable bitmap is controlled by SW + SET_PERI_REG_MASK(SENS_SAR_MEAS_START2_REG, SENS_MEAS2_START_FORCE_M); //SAR ADC2 controller (in RTC) is started by SW + SET_PERI_REG_MASK(SENS_SAR_MEAS_START2_REG, SENS_SAR2_EN_PAD_FORCE_M); //SAR ADC2 pad enable bitmap is controlled by SW + + CLEAR_PERI_REG_MASK(SENS_SAR_MEAS_WAIT2_REG, SENS_FORCE_XPD_SAR_M); //force XPD_SAR=0, use XPD_FSM + SET_PERI_REG_BITS(SENS_SAR_MEAS_WAIT2_REG, SENS_FORCE_XPD_AMP, 0x2, SENS_FORCE_XPD_AMP_S); //force XPD_AMP=0 + + CLEAR_PERI_REG_MASK(SENS_SAR_MEAS_CTRL_REG, 0xfff << SENS_AMP_RST_FB_FSM_S); //clear FSM + SET_PERI_REG_BITS(SENS_SAR_MEAS_WAIT1_REG, SENS_SAR_AMP_WAIT1, 0x1, SENS_SAR_AMP_WAIT1_S); + SET_PERI_REG_BITS(SENS_SAR_MEAS_WAIT1_REG, SENS_SAR_AMP_WAIT2, 0x1, SENS_SAR_AMP_WAIT2_S); + SET_PERI_REG_BITS(SENS_SAR_MEAS_WAIT2_REG, SENS_SAR_AMP_WAIT3, 0x1, SENS_SAR_AMP_WAIT3_S); + while (GET_PERI_REG_BITS2(SENS_SAR_SLAVE_ADDR1_REG, 0x7, SENS_MEAS_STATUS_S) != 0); //wait det_fsm== + + initialized = true; +} + +void __analogSetPinAttenuation(uint8_t pin, uint8_t attenuation) +{ + int8_t channel = digitalPinToAnalogChannel(pin); + if(channel < 0 || attenuation > 3){ + return ; + } + __analogInit(); + if(channel > 7){ + SET_PERI_REG_BITS(SENS_SAR_ATTEN2_REG, 3, attenuation, ((channel - 10) * 2)); + } else { + SET_PERI_REG_BITS(SENS_SAR_ATTEN1_REG, 3, attenuation, (channel * 2)); + } +} + +bool IRAM_ATTR __adcAttachPin(uint8_t pin){ + + int8_t channel = digitalPinToAnalogChannel(pin); + if(channel < 0){ + return false;//not adc pin + } + + int8_t pad = digitalPinToTouchChannel(pin); + if(pad >= 0){ + uint32_t touch = READ_PERI_REG(SENS_SAR_TOUCH_ENABLE_REG); + if(touch & (1 << pad)){ + touch &= ~((1 << (pad + SENS_TOUCH_PAD_OUTEN2_S)) + | (1 << (pad + SENS_TOUCH_PAD_OUTEN1_S)) + | (1 << (pad + SENS_TOUCH_PAD_WORKEN_S))); + WRITE_PERI_REG(SENS_SAR_TOUCH_ENABLE_REG, touch); + } + } else if(pin == 25){ + CLEAR_PERI_REG_MASK(RTC_IO_PAD_DAC1_REG, RTC_IO_PDAC1_XPD_DAC | RTC_IO_PDAC1_DAC_XPD_FORCE); //stop dac1 + } else if(pin == 26){ + CLEAR_PERI_REG_MASK(RTC_IO_PAD_DAC2_REG, RTC_IO_PDAC2_XPD_DAC | RTC_IO_PDAC2_DAC_XPD_FORCE); //stop dac2 + } + + pinMode(pin, ANALOG); + + __analogInit(); + return true; +} + +bool IRAM_ATTR __adcStart(uint8_t pin){ + + int8_t channel = digitalPinToAnalogChannel(pin); + if(channel < 0){ + return false;//not adc pin + } + + if(channel > 9){ + channel -= 10; + CLEAR_PERI_REG_MASK(SENS_SAR_MEAS_START2_REG, SENS_MEAS2_START_SAR_M); + SET_PERI_REG_BITS(SENS_SAR_MEAS_START2_REG, SENS_SAR2_EN_PAD, (1 << channel), SENS_SAR2_EN_PAD_S); + SET_PERI_REG_MASK(SENS_SAR_MEAS_START2_REG, SENS_MEAS2_START_SAR_M); + } else { + CLEAR_PERI_REG_MASK(SENS_SAR_MEAS_START1_REG, SENS_MEAS1_START_SAR_M); + SET_PERI_REG_BITS(SENS_SAR_MEAS_START1_REG, SENS_SAR1_EN_PAD, (1 << channel), SENS_SAR1_EN_PAD_S); + SET_PERI_REG_MASK(SENS_SAR_MEAS_START1_REG, SENS_MEAS1_START_SAR_M); + } + return true; +} + +bool IRAM_ATTR __adcBusy(uint8_t pin){ + + int8_t channel = digitalPinToAnalogChannel(pin); + if(channel < 0){ + return false;//not adc pin + } + + if(channel > 7){ + return (GET_PERI_REG_MASK(SENS_SAR_MEAS_START2_REG, SENS_MEAS2_DONE_SAR) == 0); + } + return (GET_PERI_REG_MASK(SENS_SAR_MEAS_START1_REG, SENS_MEAS1_DONE_SAR) == 0); +} + +uint16_t IRAM_ATTR __adcEnd(uint8_t pin) +{ + + uint16_t value = 0; + int8_t channel = digitalPinToAnalogChannel(pin); + if(channel < 0){ + return 0;//not adc pin + } + if(channel > 7){ + while (GET_PERI_REG_MASK(SENS_SAR_MEAS_START2_REG, SENS_MEAS2_DONE_SAR) == 0); //wait for conversion + value = GET_PERI_REG_BITS2(SENS_SAR_MEAS_START2_REG, SENS_MEAS2_DATA_SAR, SENS_MEAS2_DATA_SAR_S); + } else { + while (GET_PERI_REG_MASK(SENS_SAR_MEAS_START1_REG, SENS_MEAS1_DONE_SAR) == 0); //wait for conversion + value = GET_PERI_REG_BITS2(SENS_SAR_MEAS_START1_REG, SENS_MEAS1_DATA_SAR, SENS_MEAS1_DATA_SAR_S); + } + + // Shift result if necessary + uint8_t from = __analogWidth + 9; + if (from == __analogReturnedWidth) { + return value; + } + if (from > __analogReturnedWidth) { + return value >> (from - __analogReturnedWidth); + } + return value << (__analogReturnedWidth - from); +} + +void __analogReadResolution(uint8_t bits) +{ + if(!bits || bits > 16){ + return; + } + __analogSetWidth(bits); // hadware from 9 to 12 + __analogReturnedWidth = bits; // software from 1 to 16 +} + +uint16_t IRAM_ATTR adcRead(uint8_t pin) +{ + if(!__adcAttachPin(pin) || !__adcStart(pin)){ + return 0; + } + return __adcEnd(pin); +} + +#endif \ No newline at end of file diff --git a/src/current_sense/hardware_specific/esp32_adc_driver.h b/src/current_sense/hardware_specific/esp32_adc_driver.h new file mode 100644 index 00000000..a01eef19 --- /dev/null +++ b/src/current_sense/hardware_specific/esp32_adc_driver.h @@ -0,0 +1,88 @@ + + +#ifndef SIMPLEFOC_ESP32_HAL_ADC_DRIVER_H_ +#define SIMPLEFOC_ESP32_HAL_ADC_DRIVER_H_ + +#include "Arduino.h" + +#ifdef ESP_H +/* + * Get ADC value for pin + * */ +uint16_t adcRead(uint8_t pin); + +/* + * Set the resolution of analogRead return values. Default is 12 bits (range from 0 to 4096). + * If between 9 and 12, it will equal the set hardware resolution, else value will be shifted. + * Range is 1 - 16 + * + * Note: compatibility with Arduino SAM + */ +void __analogReadResolution(uint8_t bits); + +/* + * Sets the sample bits and read resolution + * Default is 12bit (0 - 4095) + * Range is 9 - 12 + * */ +void __analogSetWidth(uint8_t bits); + +/* + * Set number of cycles per sample + * Default is 8 and seems to do well + * Range is 1 - 255 + * */ +void __analogSetCycles(uint8_t cycles); + +/* + * Set number of samples in the range. + * Default is 1 + * Range is 1 - 255 + * This setting splits the range into + * "samples" pieces, which could look + * like the sensitivity has been multiplied + * that many times + * */ +void __analogSetSamples(uint8_t samples); + +/* + * Set the divider for the ADC clock. + * Default is 1 + * Range is 1 - 255 + * */ +void __analogSetClockDiv(uint8_t clockDiv); + +/* + * Set the attenuation for all channels + * Default is 11db + * */ +void __analogSetAttenuation(uint8_t attenuation); + +/* + * Set the attenuation for particular pin + * Default is 11db + * */ +void __analogSetPinAttenuation(uint8_t pin, uint8_t attenuation); + +/* + * Attach pin to ADC (will also clear any other analog mode that could be on) + * */ +bool __adcAttachPin(uint8_t pin); + +/* + * Start ADC conversion on attached pin's bus + * */ +bool __adcStart(uint8_t pin); + +/* + * Check if conversion on the pin's ADC bus is currently running + * */ +bool __adcBusy(uint8_t pin); + +/* + * Get the result of the conversion (will wait if it have not finished) + * */ +uint16_t __adcEnd(uint8_t pin); + +#endif /* SIMPLEFOC_ESP32_HAL_ADC_DRIVER_H_ */ +#endif /* ESP32 */ \ No newline at end of file diff --git a/src/current_sense/hardware_specific/esp32_mcu.cpp b/src/current_sense/hardware_specific/esp32_mcu.cpp index b84204f1..34f3bb72 100644 --- a/src/current_sense/hardware_specific/esp32_mcu.cpp +++ b/src/current_sense/hardware_specific/esp32_mcu.cpp @@ -11,7 +11,7 @@ static mcpwm_dev_t *MCPWM[2] = {&MCPWM0, &MCPWM1}; int a1, a2, a3; //Current readings from internal current sensor amplifiers -int _pinA, _pinB, _pinC; +int _pinA, _pinB, _pinC = NOT_SET; static void IRAM_ATTR isr_handler(void*); byte currentState = 1; @@ -19,16 +19,15 @@ byte currentState = 1; // some optimizing for faster execution #define _ADC_CONV ( (_ADC_VOLTAGE) / (_ADC_RESOLUTION) ) -// function reading an ADC value and returning the read voltage -void _readADCVoltagesLowSide(float & a, float & b, float & c){ - - a = _ADC_CONV * a1; - b = _ADC_CONV * a2; - if(_isset(_pinC)) - c = _ADC_CONV * a3; +bool _readADCVoltagesLowSide( float & a, float & b, float & c) +{ + a = a1 * _ADC_CONV; + b = a2 * _ADC_CONV; + if( _isset(pinC) ) + c = a3 * _ADC_CONV; + return true; } - // function reading an ADC value and returning the read voltage void _configureADCLowSide(const int pinA,const int pinB,const int pinC){ _pinA = pinA; diff --git a/src/current_sense/hardware_specific/generic_mcu.cpp b/src/current_sense/hardware_specific/generic_mcu.cpp index dccd3601..0d28f05d 100644 --- a/src/current_sense/hardware_specific/generic_mcu.cpp +++ b/src/current_sense/hardware_specific/generic_mcu.cpp @@ -1,56 +1,37 @@ #include "../hardware_api.h" - - - -#if defined(__AVR_ATmega328P__) || defined(__AVR_ATmega168__) || defined(__AVR_ATmega328PB__) || defined(__AVR_ATmega2560__) // if mcu is atmega328 or atmega2560 - #define _ADC_VOLTAGE 5.0 - #define _ADC_RESOLUTION 1024.0 - #ifndef cbi - #define cbi(sfr, bit) (_SFR_BYTE(sfr) &= ~_BV(bit)) - #endif - #ifndef sbi - #define sbi(sfr, bit) (_SFR_BYTE(sfr) |= _BV(bit)) - #endif -#elif defined(__arm__) && defined(CORE_TEENSY) // or teensy - #define _ADC_VOLTAGE 3.3 - #define _ADC_RESOLUTION 1024.0 -#elif defined(__arm__) && defined(__SAM3X8E__) // or due - #define _ADC_VOLTAGE 3.3 - #define _ADC_RESOLUTION 1024.0 -#elif defined(ESP_H) // or esp32 - // do nothing implemented in esp32_mcu.h - -#elif defined(_STM32_DEF_) // or stm32 - #define _ADC_VOLTAGE 3.3 - #define _ADC_RESOLUTION 1024.0 -#else - #define _ADC_VOLTAGE 5.0 - #define _ADC_RESOLUTION 1024.0 -#endif - -// adc counts to voltage conversion ratio -// some optimizing for faster execution -#define _ADC_CONV ( (_ADC_VOLTAGE) / (_ADC_RESOLUTION) ) - +__attribute__((weak)) int _pinA, _pinB, _pinC = NOT_SET; // function reading an ADC value and returning the read voltage -float _readADCVoltageInline(const int pinA){ +__attribute__((weak)) float _readADCVoltageInline(const int pinA){ uint32_t raw_adc = analogRead(pinA); - return raw_adc * _ADC_CONV; + return raw_adc * 5.0f/1024.0f; } +// function reading an ADC value and returning the read voltage +__attribute__((weak)) void _configureADCInline(const int pinA,const int pinB,const int pinC){ + pinMode(pinA, INPUT); + pinMode(pinB, INPUT); + if( _isset(pinC) ) pinMode(pinC, INPUT); +} // function reading an ADC value and returning the read voltage -void _configureADCInline(const int pinA,const int pinB,const int pinC){ +__attribute__((weak)) bool _readADCVoltagesLowSide( float & a, float & b, float & c) +{ + a = _readADCVoltageInline(_pinA); + b = _readADCVoltageInline(_pinB); + if( _isset(_pinC) ) + c = _readADCVoltageInline(_pinC); + return true; +} + +// Configure low side for generic mcu +// cannot do much but +__attribute__((weak)) void _configureADCLowSide(const int pinA,const int pinB,const int pinC){ pinMode(pinA, INPUT); pinMode(pinB, INPUT); if( _isset(pinC) ) pinMode(pinC, INPUT); +} + - #if defined(__AVR_ATmega328P__) || defined(__AVR_ATmega168__) || defined(__AVR_ATmega328PB__) || defined(__AVR_ATmega2560__) // if mcu is atmega328 or atmega2560 - // set hight frequency adc - ADPS2,ADPS1,ADPS0 | 001 (16mhz/2) | 010 ( 16mhz/4 ) | 011 (16mhz/8) | 100(16mhz/16) | 101 (16mhz/32) | 110 (16mhz/64) | 111 (16mhz/128 default) - // set divisor to 8 - adc frequency 16mhz/8 = 2 mhz - // arduino takes 25 conversions per sample so - 2mhz/25 = 80k samples per second - 12.5us per sample - cbi(ADCSRA, ADPS2); - sbi(ADCSRA, ADPS1); - sbi(ADCSRA, ADPS0); - #endif -} \ No newline at end of file +// sync driver and the adc +__attribute__((weak)) void _driverSyncLowSide(){ } +__attribute__((weak)) void _startADC3PinConversionLowSide(){ } diff --git a/src/current_sense/hardware_specific/samd21_mcu.cpp b/src/current_sense/hardware_specific/samd21_mcu.cpp index c5364c96..1da6b119 100644 --- a/src/current_sense/hardware_specific/samd21_mcu.cpp +++ b/src/current_sense/hardware_specific/samd21_mcu.cpp @@ -1,11 +1,14 @@ +#include "../hardware_api.h" + +#if defined(_SAMD21_ASYNC_) #include "samd21_mcu.h" -#include "../hardware_api.h" + #include "../../common/hardware_specific/samd_mcu.h" #include static int _pinA, _pinB, _pinC; static uint16_t a_raw = 0xFFFF, b_raw = 0xFFFF, c_raw = 0xFFFF; // updated by adcStopWithDMA when configured in freerunning mode -static SAMDCurrentSenseADCDMA instance; +static SAMD21AsyncCurrentSense instance; /** * function reading an ADC value and returning the read voltage * @@ -26,20 +29,22 @@ void _configureADCLowSide(const int pinA,const int pinB,const int pinC) * * @param pinA - the arduino pin to be read (it has to be ADC pin) */ -void _readADCVoltagesLowSide(float & a, float & b, float & c) +bool _readADCVoltagesLowSide(float & a, float & b, float & c) { static uint64_t last_ts = _micros(); - + bool isNew = false; if(last_ts != instance.timestamp_us) { last_ts = instance.timestamp_us; instance.readResults(a_raw, b_raw, c_raw); + isNew = true; } a = instance.toVolts(a_raw); b = instance.toVolts(b_raw); if(_isset(_pinC)) c = instance.toVolts(c_raw); + return isNew; } /** @@ -47,7 +52,7 @@ void _readADCVoltagesLowSide(float & a, float & b, float & c) */ void _driverSyncLowSide() { - SIMPLEFOC_SAMD_DEBUG_SERIAL.println(F("TODO! _driverSyncLowSide() is not untested, but if you use EVSYS_ID_GEN_TCC_OVF != -1 you are in sync")); + debugPrintln(F("TODO! _driverSyncLowSide() is not untested, but if you use EVSYS_ID_GEN_TCC_OVF != -1 you are in sync")); //TODO: hook with PWM interrupts } @@ -74,17 +79,17 @@ static void ADCsync() { // ADC DMA sequential free running (6) with Interrupts ///////////////// -SAMDCurrentSenseADCDMA * SAMDCurrentSenseADCDMA::getHardwareAPIInstance() +SAMD21AsyncCurrentSense * SAMD21AsyncCurrentSense::getHardwareAPIInstance() { return &instance; } -SAMDCurrentSenseADCDMA::SAMDCurrentSenseADCDMA() +SAMD21AsyncCurrentSense::SAMD21AsyncCurrentSense() { } -void SAMDCurrentSenseADCDMA::init(int pinA, int pinB, int pinC, +void SAMD21AsyncCurrentSense::init(int pinA, int pinB, int pinC, int EVSYS_ID_GEN_TCC_OVF, int pinAREF, float voltageAREF , uint8_t adcBits, uint8_t channelDMA) { @@ -117,7 +122,7 @@ int EVSYS_ID_GEN_TCC_OVF, int pinAREF, float voltageAREF } -uint32_t SAMDCurrentSenseADCDMA::readResults(uint16_t & a, uint16_t & b, uint16_t & c){ +uint32_t SAMD21AsyncCurrentSense::readResults(uint16_t & a, uint16_t & b, uint16_t & c){ a = adcBuffer[ainA]; b = adcBuffer[ainB]; @@ -127,11 +132,11 @@ uint32_t SAMDCurrentSenseADCDMA::readResults(uint16_t & a, uint16_t & b, uint16_ } -float SAMDCurrentSenseADCDMA::toVolts(uint16_t counts) { +float SAMD21AsyncCurrentSense::toVolts(uint16_t counts) { return counts * countsToVolts; } -void SAMDCurrentSenseADCDMA::initPins(){ +void SAMD21AsyncCurrentSense::initPins(){ refsel = ADC_REFCTRL_REFSEL_INTVCC1_Val; if(pinAREF != -1) @@ -170,7 +175,7 @@ void SAMDCurrentSenseADCDMA::initPins(){ } -void SAMDCurrentSenseADCDMA::initADC(){ +void SAMD21AsyncCurrentSense::initADC(){ pinPeripheral(pinA, PIO_ANALOG); pinPeripheral(pinB, PIO_ANALOG); @@ -273,13 +278,13 @@ void SAMDCurrentSenseADCDMA::initADC(){ ADCsync(); } -void SAMDCurrentSenseADCDMA::initDMA() { +void SAMD21AsyncCurrentSense::initDMA() { ::initDMAC(); } DmacDescriptor descriptors[20] __attribute__ ((aligned (16))); -void SAMDCurrentSenseADCDMA::initDMAChannel() { +void SAMD21AsyncCurrentSense::initDMAChannel() { DMAC_CHINTENSET_Type chinset{.reg = 0}; @@ -322,7 +327,7 @@ void SAMDCurrentSenseADCDMA::initDMAChannel() { } -void SAMDCurrentSenseADCDMA::operator()(uint8_t channel, volatile DMAC_CHINTFLAG_Type & flags, volatile DMAC_CHCTRLA_Type & chctrla) +void SAMD21AsyncCurrentSense::operator()(uint8_t channel, volatile DMAC_CHINTFLAG_Type & flags, volatile DMAC_CHCTRLA_Type & chctrla) { chctrla.bit.ENABLE = 0b1; flags.bit.TCMPL = 0b1; @@ -330,7 +335,7 @@ void SAMDCurrentSenseADCDMA::operator()(uint8_t channel, volatile DMAC_CHINTFLAG timestamp_us = _micros(); } -int SAMDCurrentSenseADCDMA::initEVSYS() +int SAMD21AsyncCurrentSense::initEVSYS() { if(::initEVSYS(0, EVSYS_ID_USER_ADC_SYNC, this->EVSYS_ID_GEN_TCC_OVF, EVSYS_CHANNEL_PATH_ASYNCHRONOUS_Val, EVSYS_CHANNEL_EDGSEL_NO_EVT_OUTPUT_Val) != 0) return -1; @@ -353,3 +358,4 @@ int SAMDCurrentSenseADCDMA::initEVSYS() } +#endif \ No newline at end of file diff --git a/src/current_sense/hardware_specific/samd21_mcu.h b/src/current_sense/hardware_specific/samd21_mcu.h index e32addfc..e539f0f7 100644 --- a/src/current_sense/hardware_specific/samd21_mcu.h +++ b/src/current_sense/hardware_specific/samd21_mcu.h @@ -1,9 +1,10 @@ -#ifndef CURRENT_SENSE_SAMD21_H -#define CURRENT_SENSE_SAMD21_H +#pragma once +#include + +#if defined(_SAMD21_) && defined(_SAMD21_ASYNC_) #include -#include #include "../../common/hardware_specific/samd_mcu.h" // #define SIMPLEFOC_SAMD_DEBUG #if !defined(SIMPLEFOC_SAMD_DEBUG_SERIAL) @@ -11,12 +12,12 @@ #endif -class SAMDCurrentSenseADCDMA : public DMACInterruptCallback +class SAMD21AsyncCurrentSense : public DMACInterruptCallback { public: - static SAMDCurrentSenseADCDMA * getHardwareAPIInstance(); - SAMDCurrentSenseADCDMA(); + static SAMD21AsyncCurrentSense * getHardwareAPIInstance(); + SAMD21AsyncCurrentSense(); void init(int pinA, int pinB, int pinC, int EVSYS_ID_GEN_TCC_OVF = -1, int pinAREF = -1, float voltageAREF = 3.3, uint8_t adcBits = 12, uint8_t channelDMA = 0); @@ -62,4 +63,4 @@ class SAMDCurrentSenseADCDMA : public DMACInterruptCallback }; -#endif \ No newline at end of file +#endif diff --git a/src/current_sense/hardware_specific/samd_mcu.cpp b/src/current_sense/hardware_specific/samd_mcu.cpp new file mode 100644 index 00000000..5a6b77e0 --- /dev/null +++ b/src/current_sense/hardware_specific/samd_mcu.cpp @@ -0,0 +1,45 @@ +#include "../hardware_api.h" + +#if (defined(_SAMD21_)||defined(_SAMD51_)||defined(_SAME51_)) && !defined(_SAMD21_ASYNC_) + +#define _ADC_VOLTAGE 3.3f +#define _ADC_RESOLUTION 1024.0f + +// adc counts to voltage conversion ratio +// some optimizing for faster execution +#define _ADC_CONV ( (_ADC_VOLTAGE) / (_ADC_RESOLUTION) ) +int _pinA, _pinB, _pinC = NOT_SET; +// function reading an ADC value and returning the read voltage +float _readADCVoltageInline(const int pinA){ + uint32_t raw_adc = analogRead(pinA); + return raw_adc * _ADC_CONV; +} +// function reading an ADC value and returning the read voltage +void _configureADCInline(const int pinA,const int pinB,const int pinC){ + pinMode(pinA, INPUT); + pinMode(pinB, INPUT); + if( _isset(pinC) ) pinMode(pinC, INPUT); +} + + +bool _readADCVoltagesLowSide( float & a, float & b, float & c) +{ + a = _readADCVoltageInline(_pinA); + b = _readADCVoltageInline(_pinB); + if( _isset(_pinC) ) + c = _readADCVoltageInline(_pinC); + return true; +} +// Configure low side for generic mcu +// cannot do much but +void _configureADCLowSide(const int pinA,const int pinB,const int pinC){ + _pinA = pinA; + _pinB = pinB; + if( _isset(pinC) ) _pinC = pinC; + pinMode(pinA, INPUT); + pinMode(pinB, INPUT); + if( _isset(pinC) ) pinMode(pinC, INPUT); + +} + +#endif \ No newline at end of file diff --git a/src/current_sense/hardware_specific/stm32_mcu.cpp b/src/current_sense/hardware_specific/stm32_mcu.cpp new file mode 100644 index 00000000..4656cb1b --- /dev/null +++ b/src/current_sense/hardware_specific/stm32_mcu.cpp @@ -0,0 +1,47 @@ + +#include "../hardware_api.h" + +#if defined(_STM32_DEF_) + +#define _ADC_VOLTAGE 3.3f +#define _ADC_RESOLUTION 1024.0f + +// adc counts to voltage conversion ratio +// some optimizing for faster execution +#define _ADC_CONV ( (_ADC_VOLTAGE) / (_ADC_RESOLUTION) ) +int _pinA, _pinB, _pinC = NOT_SET; +// function reading an ADC value and returning the read voltage +float _readADCVoltageInline(const int pinA){ + uint32_t raw_adc = analogRead(pinA); + return raw_adc * _ADC_CONV; +} +// function reading an ADC value and returning the read voltage +void _configureADCInline(const int pinA,const int pinB,const int pinC){ + pinMode(pinA, INPUT); + pinMode(pinB, INPUT); + if( _isset(pinC) ) pinMode(pinC, INPUT); +} + + +bool _readADCVoltagesLowSide( float & a, float & b, float & c) +{ + a = _readADCVoltageInline(_pinA); + b = _readADCVoltageInline(_pinB); + if( _isset(_pinC) ) + c = _readADCVoltageInline(_pinC); + return true; +} +// Configure low side for generic mcu +// cannot do much but +void _configureADCLowSide(const int pinA,const int pinB,const int pinC){ + _pinA = pinA; + _pinB = pinB; + if( _isset(pinC) ) _pinC = pinC; + pinMode(pinA, INPUT); + pinMode(pinB, INPUT); + if( _isset(pinC) ) pinMode(pinC, INPUT); + +} + + +#endif \ No newline at end of file diff --git a/src/current_sense/hardware_specific/teensy_mcu.cpp b/src/current_sense/hardware_specific/teensy_mcu.cpp new file mode 100644 index 00000000..5534dd7d --- /dev/null +++ b/src/current_sense/hardware_specific/teensy_mcu.cpp @@ -0,0 +1,45 @@ +#include "../hardware_api.h" + +#if defined(__arm__) && defined(CORE_TEENSY) + +#define _ADC_VOLTAGE 3.3f +#define _ADC_RESOLUTION 1024.0f + +// adc counts to voltage conversion ratio +// some optimizing for faster execution +#define _ADC_CONV ( (_ADC_VOLTAGE) / (_ADC_RESOLUTION) ) +int _pinA, _pinB, _pinC = NOT_SET; + +// function reading an ADC value and returning the read voltage +float _readADCVoltageInline(const int pinA){ + uint32_t raw_adc = analogRead(pinA); + return raw_adc * _ADC_CONV; +} +// function reading an ADC value and returning the read voltage +void _configureADCInline(const int pinA,const int pinB,const int pinC){ + pinMode(pinA, INPUT); + pinMode(pinB, INPUT); + if( _isset(pinC) ) pinMode(pinC, INPUT); +} + +bool _readADCVoltagesLowSide( float & a, float & b, float & c) +{ + a = _readADCVoltageInline(_pinA); + b = _readADCVoltageInline(_pinB); + if( _isset(_pinC) ) + c = _readADCVoltageInline(_pinC); + return true; +} + +// Configure low side for generic mcu +// cannot do much but +void _configureADCLowSide(const int pinA,const int pinB,const int pinC){ + _pinA = pinA; + _pinB = pinB; + if( _isset(pinC) ) _pinC = pinC; + pinMode(pinA, INPUT); + pinMode(pinB, INPUT); + if( _isset(pinC) ) pinMode(pinC, INPUT); + +} +#endif \ No newline at end of file diff --git a/src/drivers/BLDCDriver3PWM.cpp b/src/drivers/BLDCDriver3PWM.cpp index 11215863..7ccd09fa 100644 --- a/src/drivers/BLDCDriver3PWM.cpp +++ b/src/drivers/BLDCDriver3PWM.cpp @@ -39,7 +39,7 @@ void BLDCDriver3PWM::disable() } -// init hardware pins +// init hardware pins int BLDCDriver3PWM::init() { // PWM pins pinMode(pwmA, OUTPUT); @@ -62,7 +62,7 @@ int BLDCDriver3PWM::init() { // Set voltage to the pwm pin -void BLDCDriver3PWM::setPhaseState(int sa, int sb, int sc) { +void BLDCDriver3PWM::setPhaseState(int sa, int sb, int sc) { // disable if needed if( _isset(enableA_pin) && _isset(enableB_pin) && _isset(enableC_pin) ){ digitalWrite(enableA_pin, sa == _HIGH_IMPEDANCE ? LOW : HIGH); @@ -73,16 +73,16 @@ void BLDCDriver3PWM::setPhaseState(int sa, int sb, int sc) { // Set voltage to the pwm pin void BLDCDriver3PWM::setPwm(float Ua, float Ub, float Uc) { - + // limit the voltage in driver Ua = _constrain(Ua, 0.0, voltage_limit); Ub = _constrain(Ub, 0.0, voltage_limit); - Uc = _constrain(Uc, 0.0, voltage_limit); + Uc = _constrain(Uc, 0.0, voltage_limit); // calculate duty cycle // limited in [0,1] - dc_a = _constrain(Ua / voltage_power_supply, 0.0 , 1.0 ); - dc_b = _constrain(Ub / voltage_power_supply, 0.0 , 1.0 ); - dc_c = _constrain(Uc / voltage_power_supply, 0.0 , 1.0 ); + dc_a = _constrain(Ua / voltage_power_supply, 0.0f , 1.0f ); + dc_b = _constrain(Ub / voltage_power_supply, 0.0f , 1.0f ); + dc_c = _constrain(Uc / voltage_power_supply, 0.0f , 1.0f ); // hardware specific writing // hardware specific function - depending on driver and mcu diff --git a/src/drivers/BLDCDriver6PWM.cpp b/src/drivers/BLDCDriver6PWM.cpp index d51eec0a..6039f69e 100644 --- a/src/drivers/BLDCDriver6PWM.cpp +++ b/src/drivers/BLDCDriver6PWM.cpp @@ -15,9 +15,9 @@ BLDCDriver6PWM::BLDCDriver6PWM(int phA_h,int phA_l,int phB_h,int phB_l,int phC_h // default power-supply value voltage_power_supply = DEF_POWER_SUPPLY; voltage_limit = NOT_SET; - + // dead zone initial - 2% - dead_zone = 0.02; + dead_zone = 0.02f; } @@ -39,9 +39,9 @@ void BLDCDriver6PWM::disable() } -// init hardware pins +// init hardware pins int BLDCDriver6PWM::init() { - + // PWM pins pinMode(pwmA_h, OUTPUT); pinMode(pwmB_h, OUTPUT); @@ -55,22 +55,22 @@ int BLDCDriver6PWM::init() { // sanity check for the voltage limit configuration if( !_isset(voltage_limit) || voltage_limit > voltage_power_supply) voltage_limit = voltage_power_supply; - // configure 6pwm + // configure 6pwm // hardware specific function - depending on driver and mcu return _configure6PWM(pwm_frequency, dead_zone, pwmA_h,pwmA_l, pwmB_h,pwmB_l, pwmC_h,pwmC_l); } // Set voltage to the pwm pin -void BLDCDriver6PWM::setPwm(float Ua, float Ub, float Uc) { +void BLDCDriver6PWM::setPwm(float Ua, float Ub, float Uc) { // limit the voltage in driver Ua = _constrain(Ua, 0, voltage_limit); Ub = _constrain(Ub, 0, voltage_limit); - Uc = _constrain(Uc, 0, voltage_limit); + Uc = _constrain(Uc, 0, voltage_limit); // calculate duty cycle // limited in [0,1] - dc_a = _constrain(Ua / voltage_power_supply, 0.0 , 1.0 ); - dc_b = _constrain(Ub / voltage_power_supply, 0.0 , 1.0 ); - dc_c = _constrain(Uc / voltage_power_supply, 0.0 , 1.0 ); + dc_a = _constrain(Ua / voltage_power_supply, 0.0f , 1.0f ); + dc_b = _constrain(Ub / voltage_power_supply, 0.0f , 1.0f ); + dc_c = _constrain(Uc / voltage_power_supply, 0.0f , 1.0f ); // hardware specific writing // hardware specific function - depending on driver and mcu _writeDutyCycle6PWM(dc_a, dc_b, dc_c, dead_zone, pwmA_h,pwmA_l, pwmB_h,pwmB_l, pwmC_h,pwmC_l); @@ -78,6 +78,6 @@ void BLDCDriver6PWM::setPwm(float Ua, float Ub, float Uc) { // Set voltage to the pwm pin -void BLDCDriver6PWM::setPhaseState(int sa, int sb, int sc) { +void BLDCDriver6PWM::setPhaseState(int sa, int sb, int sc) { // TODO implement disabling } diff --git a/src/drivers/StepperDriver2PWM.cpp b/src/drivers/StepperDriver2PWM.cpp index 09dd3e16..a9432b1d 100644 --- a/src/drivers/StepperDriver2PWM.cpp +++ b/src/drivers/StepperDriver2PWM.cpp @@ -1,13 +1,13 @@ #include "StepperDriver2PWM.h" -StepperDriver2PWM::StepperDriver2PWM(int _pwm1, int _in1a, int _in1b, int _pwm2, int _in2a, int _in2b, int en1, int en2){ +StepperDriver2PWM::StepperDriver2PWM(int _pwm1, int* _in1, int _pwm2, int* _in2, int en1, int en2){ // Pin initialization pwm1 = _pwm1; // phase 1 pwm pin number - dir1a = _in1a; // phase 1 INA pin number - dir1b = _in1b; // phase 1 INB pin number + dir1a = _in1[0]; // phase 1 INA pin number + dir1b = _in1[1]; // phase 1 INB pin number pwm2 = _pwm2; // phase 2 pwm pin number - dir2a = _in2a; // phase 2 INA pin number - dir2b = _in2b; // phase 2 INB pin number + dir2a = _in2[0]; // phase 2 INA pin number + dir2b = _in2[1]; // phase 2 INB pin number // enable_pin pin enable_pin1 = en1; @@ -27,7 +27,7 @@ StepperDriver2PWM::StepperDriver2PWM(int _pwm1, int _dir1, int _pwm2, int _dir2, dir2a = _dir2; // phase 2 direction pin // these pins are not used dir1b = NOT_SET; - dir2b = NOT_SET; + dir2b = NOT_SET; // enable_pin pin enable_pin1 = en1; @@ -59,7 +59,7 @@ void StepperDriver2PWM::disable() } -// init hardware pins +// init hardware pins int StepperDriver2PWM::init() { // PWM pins pinMode(pwm1, OUTPUT); @@ -83,22 +83,22 @@ int StepperDriver2PWM::init() { // Set voltage to the pwm pin -void StepperDriver2PWM::setPwm(float Ua, float Ub) { - float duty_cycle1(0.0),duty_cycle2(0.0); +void StepperDriver2PWM::setPwm(float Ua, float Ub) { + float duty_cycle1(0.0f),duty_cycle2(0.0f); // limit the voltage in driver Ua = _constrain(Ua, -voltage_limit, voltage_limit); Ub = _constrain(Ub, -voltage_limit, voltage_limit); // hardware specific writing duty_cycle1 = _constrain(abs(Ua)/voltage_power_supply,0.0,1.0); duty_cycle2 = _constrain(abs(Ub)/voltage_power_supply,0.0,1.0); - + // phase 1 direction digitalWrite(dir1a, Ua >= 0 ? LOW : HIGH); - if( _isset(dir1b) ) digitalWrite(dir1b, Ua <= 0 ? LOW : HIGH); + if( _isset(dir1b) ) digitalWrite(dir1b, Ua >= 0 ? HIGH : LOW ); // phase 2 direction digitalWrite(dir2a, Ub >= 0 ? LOW : HIGH); - if( _isset(dir2b) ) digitalWrite(dir2b, Ub <= 0 ? LOW : HIGH); - + if( _isset(dir2b) ) digitalWrite(dir2b, Ub >= 0 ? HIGH : LOW ); + // write to hardware _writeDutyCycle2PWM(duty_cycle1, duty_cycle2, pwm1, pwm2); } \ No newline at end of file diff --git a/src/drivers/StepperDriver2PWM.h b/src/drivers/StepperDriver2PWM.h index cab796d3..b349af06 100644 --- a/src/drivers/StepperDriver2PWM.h +++ b/src/drivers/StepperDriver2PWM.h @@ -16,15 +16,13 @@ class StepperDriver2PWM: public StepperDriver /** StepperMotor class constructor @param pwm1 PWM1 phase pwm pin - @param in1a IN1A phase dir pin - @param in1b IN1B phase dir pin + @param in1 IN1A phase dir pin @param pwm2 PWM2 phase pwm pin - @param in2a IN2A phase dir pin - @param in2b IN2B phase dir pin + @param in2 IN2A phase dir @param en1 enable pin phase 1 (optional input) @param en2 enable pin phase 2 (optional input) */ - StepperDriver2PWM(int pwm1, int in1a, int in1b, int pwm2, int in2a, int in2b, int en1 = NOT_SET, int en2 = NOT_SET); + StepperDriver2PWM(int pwm1, int* in1, int pwm2, int* in2, int en1 = NOT_SET, int en2 = NOT_SET); /** StepperMotor class constructor diff --git a/src/drivers/StepperDriver4PWM.cpp b/src/drivers/StepperDriver4PWM.cpp index f752ea6e..cfcb7aef 100644 --- a/src/drivers/StepperDriver4PWM.cpp +++ b/src/drivers/StepperDriver4PWM.cpp @@ -37,7 +37,7 @@ void StepperDriver4PWM::disable() } -// init hardware pins +// init hardware pins int StepperDriver4PWM::init() { // PWM pins @@ -59,17 +59,17 @@ int StepperDriver4PWM::init() { // Set voltage to the pwm pin -void StepperDriver4PWM::setPwm(float Ualpha, float Ubeta) { - float duty_cycle1A(0.0),duty_cycle1B(0.0),duty_cycle2A(0.0),duty_cycle2B(0.0); +void StepperDriver4PWM::setPwm(float Ualpha, float Ubeta) { + float duty_cycle1A(0.0f),duty_cycle1B(0.0f),duty_cycle2A(0.0f),duty_cycle2B(0.0f); // limit the voltage in driver Ualpha = _constrain(Ualpha, -voltage_limit, voltage_limit); Ubeta = _constrain(Ubeta, -voltage_limit, voltage_limit); // hardware specific writing if( Ualpha > 0 ) duty_cycle1B = _constrain(abs(Ualpha)/voltage_power_supply,0.0,1.0); - else + else duty_cycle1A = _constrain(abs(Ualpha)/voltage_power_supply,0.0,1.0); - + if( Ubeta > 0 ) duty_cycle2B = _constrain(abs(Ubeta)/voltage_power_supply,0.0,1.0); else diff --git a/src/drivers/hardware_specific/atmega2560_mcu.cpp b/src/drivers/hardware_specific/atmega2560_mcu.cpp index 5236fb03..3d50dc45 100644 --- a/src/drivers/hardware_specific/atmega2560_mcu.cpp +++ b/src/drivers/hardware_specific/atmega2560_mcu.cpp @@ -15,13 +15,13 @@ void _pinHighFrequency(const int pin){ TCCR1B = ((TCCR1B & 0b11111000) | 0x01); // set prescaler to 1 else if (pin == 10 || pin == 9 ) TCCR2B = ((TCCR2B & 0b11111000) | 0x01); // set prescaler to 1 - else if (pin == 5 || pin == 3 || pin == 2) + else if (pin == 5 || pin == 3 || pin == 2) TCCR3B = ((TCCR2B & 0b11111000) | 0x01); // set prescaler to 1 else if (pin == 8 || pin == 7 || pin == 6) TCCR4B = ((TCCR4B & 0b11111000) | 0x01); // set prescaler to 1 else if (pin == 44 || pin == 45 || pin == 46) TCCR5B = ((TCCR5B & 0b11111000) | 0x01); // set prescaler to 1 - + } @@ -46,23 +46,23 @@ void _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const int _pinHighFrequency(pinC); } -// function setting the pwm duty cycle to the hardware +// function setting the pwm duty cycle to the hardware // - Stepper motor - 2PWM setting // - hardware speciffic void _writeDutyCycle2PWM(float dc_a, float dc_b, int pinA, int pinB){ // transform duty cycle from [0,1] to [0,255] - analogWrite(pinA, 255.0*dc_a); - analogWrite(pinB, 255.0*dc_b); + analogWrite(pinA, 255.0f*dc_a); + analogWrite(pinB, 255.0f*dc_b); } -// function setting the pwm duty cycle to the hardware +// function setting the pwm duty cycle to the hardware // - BLDC motor - 3PWM setting // - hardware speciffic void _writeDutyCycle3PWM(float dc_a, float dc_b, float dc_c, int pinA, int pinB, int pinC){ // transform duty cycle from [0,1] to [0,255] - analogWrite(pinA, 255.0*dc_a); - analogWrite(pinB, 255.0*dc_b); - analogWrite(pinC, 255.0*dc_c); + analogWrite(pinA, 255.0f*dc_a); + analogWrite(pinB, 255.0f*dc_b); + analogWrite(pinC, 255.0f*dc_c); } // function setting the high pwm frequency to the supplied pins @@ -74,25 +74,25 @@ void _configure4PWM(long pwm_frequency,const int pin1A, const int pin1B, const i _pinHighFrequency(pin1A); _pinHighFrequency(pin1B); _pinHighFrequency(pin2A); - _pinHighFrequency(pin2B); + _pinHighFrequency(pin2B); } -// function setting the pwm duty cycle to the hardware +// function setting the pwm duty cycle to the hardware // - Stepper motor - 4PWM setting // - hardware speciffic void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, int pin1A, int pin1B, int pin2A, int pin2B){ // transform duty cycle from [0,1] to [0,255] - analogWrite(pin1A, 255.0*dc_1a); - analogWrite(pin1B, 255.0*dc_1b); - analogWrite(pin2A, 255.0*dc_2a); - analogWrite(pin2B, 255.0*dc_2b); + analogWrite(pin1A, 255.0f*dc_1a); + analogWrite(pin1B, 255.0f*dc_1b); + analogWrite(pin2A, 255.0f*dc_2a); + analogWrite(pin2B, 255.0f*dc_2b); } // function configuring pair of high-low side pwm channels, 32khz frequency and center aligned pwm // supports Arudino/ATmega2560 int _configureComplementaryPair(int pinH, int pinL) { - if( (pinH == 4 && pinL == 13 ) || (pinH == 13 && pinL == 4 ) ){ + if( (pinH == 4 && pinL == 13 ) || (pinH == 13 && pinL == 4 ) ){ // configure the pwm phase-corrected mode TCCR0A = ((TCCR0A & 0b11111100) | 0x01); // configure complementary pwm on low side @@ -121,7 +121,7 @@ int _configureComplementaryPair(int pinH, int pinL) { // Configuring PWM frequency, resolution and alignment // - BLDC driver - 6PWM setting // - hardware specific -// supports Arudino/ATmega328 +// supports Arudino/ATmega328 int _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const int pinA_l, const int pinB_h, const int pinB_l, const int pinC_h, const int pinC_l) { // High PWM frequency // - always max 32kHz @@ -132,12 +132,12 @@ int _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const return ret_flag; // returns -1 if not well configured } -// function setting the +// function setting the void _setPwmPair(int pinH, int pinL, float val, int dead_time) { int pwm_h = _constrain(val-dead_time/2,0,255); int pwm_l = _constrain(val+dead_time/2,0,255); - + analogWrite(pinH, pwm_h); if(pwm_l == 255 || pwm_l == 0) digitalWrite(pinL, pwm_l ? LOW : HIGH); @@ -148,7 +148,7 @@ void _setPwmPair(int pinH, int pinL, float val, int dead_time) // Function setting the duty cycle to the pwm pin (ex. analogWrite()) // - BLDC driver - 6PWM setting // - hardware specific -// supports Arudino/ATmega328 +// supports Arudino/ATmega328 void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, float dead_zone, int pinA_h, int pinA_l, int pinB_h, int pinB_l, int pinC_h, int pinC_l){ _setPwmPair(pinA_h, pinA_l, dc_a*255.0, dead_zone*255.0); _setPwmPair(pinB_h, pinB_l, dc_b*255.0, dead_zone*255.0); diff --git a/src/drivers/hardware_specific/atmega328_mcu.cpp b/src/drivers/hardware_specific/atmega328_mcu.cpp index be8dadb1..2c64ee64 100644 --- a/src/drivers/hardware_specific/atmega328_mcu.cpp +++ b/src/drivers/hardware_specific/atmega328_mcu.cpp @@ -1,6 +1,6 @@ #include "../hardware_api.h" -#if defined(__AVR_ATmega328P__) || defined(__AVR_ATmega168__) || defined(__AVR_ATmega328PB__) +#if defined(__AVR_ATmega328P__) || defined(__AVR_ATmega168__) || defined(__AVR_ATmega328PB__) // set pwm frequency to 32KHz void _pinHighFrequency(const int pin){ @@ -12,9 +12,9 @@ void _pinHighFrequency(const int pin){ } if (pin == 9 || pin == 10 ) TCCR1B = ((TCCR1B & 0b11111000) | 0x01); // set prescaler to 1 - if (pin == 3 || pin == 11) + if (pin == 3 || pin == 11) TCCR2B = ((TCCR2B & 0b11111000) | 0x01);// set prescaler to 1 - + } // function setting the high pwm frequency to the supplied pins @@ -42,47 +42,47 @@ void _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const int _pinHighFrequency(pinC); } -// function setting the pwm duty cycle to the hardware +// function setting the pwm duty cycle to the hardware // - Stepper motor - 2PWM setting // - hardware speciffic void _writeDutyCycle2PWM(float dc_a, float dc_b, int pinA, int pinB){ // transform duty cycle from [0,1] to [0,255] - analogWrite(pinA, 255.0*dc_a); - analogWrite(pinB, 255.0*dc_b); + analogWrite(pinA, 255.0f*dc_a); + analogWrite(pinB, 255.0f*dc_b); } -// function setting the pwm duty cycle to the hardware +// function setting the pwm duty cycle to the hardware // - BLDC motor - 3PWM setting // - hardware speciffic void _writeDutyCycle3PWM(float dc_a, float dc_b, float dc_c, int pinA, int pinB, int pinC){ // transform duty cycle from [0,1] to [0,255] - analogWrite(pinA, 255.0*dc_a); - analogWrite(pinB, 255.0*dc_b); - analogWrite(pinC, 255.0*dc_c); + analogWrite(pinA, 255.0f*dc_a); + analogWrite(pinB, 255.0f*dc_b); + analogWrite(pinC, 255.0f*dc_c); } // function setting the high pwm frequency to the supplied pins // - Stepper motor - 4PWM setting // - hardware speciffic -// supports Arudino/ATmega328 +// supports Arudino/ATmega328 void _configure4PWM(long pwm_frequency,const int pin1A, const int pin1B, const int pin2A, const int pin2B) { // High PWM frequency // - always max 32kHz _pinHighFrequency(pin1A); _pinHighFrequency(pin1B); _pinHighFrequency(pin2A); - _pinHighFrequency(pin2B); + _pinHighFrequency(pin2B); } -// function setting the pwm duty cycle to the hardware +// function setting the pwm duty cycle to the hardware // - Stepper motor - 4PWM setting // - hardware speciffic void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, int pin1A, int pin1B, int pin2A, int pin2B){ // transform duty cycle from [0,1] to [0,255] - analogWrite(pin1A, 255.0*dc_1a); - analogWrite(pin1B, 255.0*dc_1b); - analogWrite(pin2A, 255.0*dc_2a); - analogWrite(pin2B, 255.0*dc_2b); + analogWrite(pin1A, 255.0f*dc_1a); + analogWrite(pin1B, 255.0f*dc_1b); + analogWrite(pin2A, 255.0f*dc_2a); + analogWrite(pin2B, 255.0f*dc_2b); } @@ -117,7 +117,7 @@ int _configureComplementaryPair(int pinH, int pinL) { // Configuring PWM frequency, resolution and alignment // - BLDC driver - 6PWM setting // - hardware specific -// supports Arudino/ATmega328 +// supports Arudino/ATmega328 int _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const int pinA_l, const int pinB_h, const int pinB_l, const int pinC_h, const int pinC_l) { _UNUSED(pwm_frequency); _UNUSED(dead_zone); @@ -130,12 +130,12 @@ int _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const return ret_flag; // returns -1 if not well configured } -// function setting the +// function setting the void _setPwmPair(int pinH, int pinL, float val, int dead_time) { int pwm_h = _constrain(val-dead_time/2,0,255); int pwm_l = _constrain(val+dead_time/2,0,255); - + analogWrite(pinH, pwm_h); if(pwm_l == 255 || pwm_l == 0) digitalWrite(pinL, pwm_l ? LOW : HIGH); @@ -146,7 +146,7 @@ void _setPwmPair(int pinH, int pinL, float val, int dead_time) // Function setting the duty cycle to the pwm pin (ex. analogWrite()) // - BLDC driver - 6PWM setting // - hardware specific -// supports Arudino/ATmega328 +// supports Arudino/ATmega328 void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, float dead_zone, int pinA_h, int pinA_l, int pinB_h, int pinB_l, int pinC_h, int pinC_l){ _setPwmPair(pinA_h, pinA_l, dc_a*255.0, dead_zone*255.0); _setPwmPair(pinB_h, pinB_l, dc_b*255.0, dead_zone*255.0); diff --git a/src/drivers/hardware_specific/esp32_mcu.cpp b/src/drivers/hardware_specific/esp32_mcu.cpp index 0bc2d863..c87e3b70 100644 --- a/src/drivers/hardware_specific/esp32_mcu.cpp +++ b/src/drivers/hardware_specific/esp32_mcu.cpp @@ -6,13 +6,13 @@ #include "soc/mcpwm_reg.h" #include "soc/mcpwm_struct.h" -// empty motor slot +// empty motor slot #define _EMPTY_SLOT -20 #define _TAKEN_SLOT -21 // ABI bus frequency - would be better to take it from somewhere // but I did nto find a good exposed variable -#define _MCPWM_FREQ 160e6 +#define _MCPWM_FREQ 160e6f // preferred pwm resolution default #define _PWM_RES_DEF 2048 @@ -70,7 +70,7 @@ typedef struct { } bldc_6pwm_motor_slots_t; // define bldc motor slots array -bldc_3pwm_motor_slots_t esp32_bldc_3pwm_motor_slots[4] = { +bldc_3pwm_motor_slots_t esp32_bldc_3pwm_motor_slots[4] = { {_EMPTY_SLOT, &MCPWM0, MCPWM_UNIT_0, MCPWM_OPR_A, MCPWM0A, MCPWM1A, MCPWM2A}, // 1st motor will be MCPWM0 channel A {_EMPTY_SLOT, &MCPWM0, MCPWM_UNIT_0, MCPWM_OPR_B, MCPWM0B, MCPWM1B, MCPWM2B}, // 2nd motor will be MCPWM0 channel B {_EMPTY_SLOT, &MCPWM1, MCPWM_UNIT_1, MCPWM_OPR_A, MCPWM0A, MCPWM1A, MCPWM2A}, // 3rd motor will be MCPWM1 channel A @@ -78,19 +78,19 @@ bldc_3pwm_motor_slots_t esp32_bldc_3pwm_motor_slots[4] = { }; // define stepper motor slots array -bldc_6pwm_motor_slots_t esp32_bldc_6pwm_motor_slots[2] = { +bldc_6pwm_motor_slots_t esp32_bldc_6pwm_motor_slots[2] = { {_EMPTY_SLOT, &MCPWM0, MCPWM_UNIT_0, MCPWM_OPR_A, MCPWM_OPR_B, MCPWM0A, MCPWM1A, MCPWM2A, MCPWM0B, MCPWM1B, MCPWM2B}, // 1st motor will be on MCPWM0 {_EMPTY_SLOT, &MCPWM1, MCPWM_UNIT_1, MCPWM_OPR_A, MCPWM_OPR_B, MCPWM0A, MCPWM1A, MCPWM2A, MCPWM0B, MCPWM1B, MCPWM2B}, // 1st motor will be on MCPWM0 - }; + }; // define 4pwm stepper motor slots array -stepper_4pwm_motor_slots_t esp32_stepper_4pwm_motor_slots[2] = { +stepper_4pwm_motor_slots_t esp32_stepper_4pwm_motor_slots[2] = { {_EMPTY_SLOT, &MCPWM0, MCPWM_UNIT_0, MCPWM_OPR_A, MCPWM_OPR_B, MCPWM0A, MCPWM1A, MCPWM0B, MCPWM1B}, // 1st motor will be on MCPWM0 {_EMPTY_SLOT, &MCPWM1, MCPWM_UNIT_1, MCPWM_OPR_A, MCPWM_OPR_B, MCPWM0A, MCPWM1A, MCPWM0B, MCPWM1B}, // 1st motor will be on MCPWM1 - }; + }; // define 2pwm stepper motor slots array -stepper_2pwm_motor_slots_t esp32_stepper_2pwm_motor_slots[4] = { +stepper_2pwm_motor_slots_t esp32_stepper_2pwm_motor_slots[4] = { {_EMPTY_SLOT, &MCPWM0, MCPWM_UNIT_0, MCPWM_OPR_A, MCPWM0A, MCPWM1A}, // 1st motor will be MCPWM0 channel A {_EMPTY_SLOT, &MCPWM0, MCPWM_UNIT_0, MCPWM_OPR_B, MCPWM0B, MCPWM1B}, // 2nd motor will be MCPWM0 channel B {_EMPTY_SLOT, &MCPWM1, MCPWM_UNIT_1, MCPWM_OPR_A, MCPWM0A, MCPWM1A}, // 3rd motor will be MCPWM1 channel A @@ -109,12 +109,12 @@ void _configureTimerFrequency(long pwm_frequency, mcpwm_dev_t* mcpwm_num, mcpwm mcpwm_init(mcpwm_unit, MCPWM_TIMER_0, &pwm_config); //Configure PWM0A & PWM0B with above settings mcpwm_init(mcpwm_unit, MCPWM_TIMER_1, &pwm_config); //Configure PWM1A & PWM1B with above settings mcpwm_init(mcpwm_unit, MCPWM_TIMER_2, &pwm_config); //Configure PWM2A & PWM2B with above settings - + if (_isset(dead_zone)){ - // dead zone is configured - float dead_time = (float)(_MCPWM_FREQ / (pwm_frequency)) * dead_zone; - mcpwm_deadtime_enable(mcpwm_unit, MCPWM_TIMER_0, MCPWM_ACTIVE_HIGH_COMPLIMENT_MODE, dead_time/2.0, dead_time/2.0); - mcpwm_deadtime_enable(mcpwm_unit, MCPWM_TIMER_1, MCPWM_ACTIVE_HIGH_COMPLIMENT_MODE, dead_time/2.0, dead_time/2.0); + // dead zone is configured + float dead_time = (float)(_MCPWM_FREQ / (pwm_frequency)) * dead_zone; + mcpwm_deadtime_enable(mcpwm_unit, MCPWM_TIMER_0, MCPWM_ACTIVE_HIGH_COMPLIMENT_MODE, dead_time/2.0, dead_time/2.0); + mcpwm_deadtime_enable(mcpwm_unit, MCPWM_TIMER_1, MCPWM_ACTIVE_HIGH_COMPLIMENT_MODE, dead_time/2.0, dead_time/2.0); mcpwm_deadtime_enable(mcpwm_unit, MCPWM_TIMER_2, MCPWM_ACTIVE_HIGH_COMPLIMENT_MODE, dead_time/2.0, dead_time/2.0); } _delay(100); @@ -128,21 +128,21 @@ void _configureTimerFrequency(long pwm_frequency, mcpwm_dev_t* mcpwm_num, mcpwm // calculate prescaler and period // step 1: calculate the prescaler using the default pwm resolution // prescaler = bus_freq / (pwm_freq * default_pwm_res) - 1 - int prescaler = ceil((double)_MCPWM_FREQ / (double)_PWM_RES_DEF / 2.0 / (double)pwm_frequency) - 1; + int prescaler = ceil((double)_MCPWM_FREQ / (double)_PWM_RES_DEF / 2.0f / (double)pwm_frequency) - 1; // constrain prescaler - prescaler = _constrain(prescaler, 0, 128); + prescaler = _constrain(prescaler, 0, 128); // now calculate the real resolution timer period necessary (pwm resolution) // pwm_res = bus_freq / (pwm_freq * (prescaler + 1)) - int resolution_corrected = (double)_MCPWM_FREQ / 2.0 / (double)pwm_frequency / (double)(prescaler + 1); + int resolution_corrected = (double)_MCPWM_FREQ / 2.0f / (double)pwm_frequency / (double)(prescaler + 1); // if pwm resolution too low lower the prescaler - if(resolution_corrected < _PWM_RES_MIN && prescaler > 0 ) - resolution_corrected = (double)_MCPWM_FREQ / 2.0 / (double)pwm_frequency / (double)(--prescaler + 1); + if(resolution_corrected < _PWM_RES_MIN && prescaler > 0 ) + resolution_corrected = (double)_MCPWM_FREQ / 2.0f / (double)pwm_frequency / (double)(--prescaler + 1); resolution_corrected = _constrain(resolution_corrected, _PWM_RES_MIN, _PWM_RES_MAX); // set prescaler mcpwm_num->timer[0].period.prescale = prescaler; mcpwm_num->timer[1].period.prescale = prescaler; - mcpwm_num->timer[2].period.prescale = prescaler; + mcpwm_num->timer[2].period.prescale = prescaler; _delay(1); //set period mcpwm_num->timer[0].period.period = resolution_corrected; @@ -152,13 +152,13 @@ void _configureTimerFrequency(long pwm_frequency, mcpwm_dev_t* mcpwm_num, mcpwm mcpwm_num->timer[0].period.upmethod = 0; mcpwm_num->timer[1].period.upmethod = 0; mcpwm_num->timer[2].period.upmethod = 0; - _delay(1); - // _delay(1); + _delay(1); + // _delay(1); //restart the timers mcpwm_start(mcpwm_unit, MCPWM_TIMER_0); mcpwm_start(mcpwm_unit, MCPWM_TIMER_1); mcpwm_start(mcpwm_unit, MCPWM_TIMER_2); - _delay(1); + _delay(1); // Cast here because MCPWM_SELECT_SYNC_INT0 (1) is not defined // in the default Espressif MCPWM headers. The correct const may be used // when https://github.com/espressif/esp-idf/issues/5429 is resolved. @@ -174,15 +174,15 @@ void _configureTimerFrequency(long pwm_frequency, mcpwm_dev_t* mcpwm_num, mcpwm // function setting the high pwm frequency to the supplied pins // - Stepper motor - 2PWM setting // - hardware speciffic -// supports Arudino/ATmega328, STM32 and ESP32 +// supports Arudino/ATmega328, STM32 and ESP32 void _configure2PWM(long pwm_frequency,const int pinA, const int pinB) { if(!pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; // default frequency 25hz - else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 40kHz max + else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 40kHz max stepper_2pwm_motor_slots_t m_slot = {}; // determine which motor are we connecting - // and set the appropriate configuration parameters + // and set the appropriate configuration parameters int slot_num; for(slot_num = 0; slot_num < 4; slot_num++){ if(esp32_stepper_2pwm_motor_slots[slot_num].pin1pwm == _EMPTY_SLOT){ // put the new motor in the first empty slot @@ -191,8 +191,8 @@ void _configure2PWM(long pwm_frequency,const int pinA, const int pinB) { break; } } - - // disable all the slots with the same MCPWM + + // disable all the slots with the same MCPWM // disable 3pwm bldc motor which would go in the same slot esp32_bldc_3pwm_motor_slots[slot_num].pinA = _TAKEN_SLOT; if( slot_num < 2 ){ @@ -219,15 +219,15 @@ void _configure2PWM(long pwm_frequency,const int pinA, const int pinB) { // function setting the high pwm frequency to the supplied pins // - BLDC motor - 3PWM setting // - hardware speciffic -// supports Arudino/ATmega328, STM32 and ESP32 +// supports Arudino/ATmega328, STM32 and ESP32 void _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC) { if(!pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; // default frequency 25hz - else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 40kHz max + else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 40kHz max bldc_3pwm_motor_slots_t m_slot = {}; // determine which motor are we connecting - // and set the appropriate configuration parameters + // and set the appropriate configuration parameters int slot_num; for(slot_num = 0; slot_num < 4; slot_num++){ if(esp32_bldc_3pwm_motor_slots[slot_num].pinA == _EMPTY_SLOT){ // put the new motor in the first empty slot @@ -236,7 +236,7 @@ void _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const int break; } } - // disable all the slots with the same MCPWM + // disable all the slots with the same MCPWM // disable 2pwm steppr motor which would go in the same slot esp32_stepper_2pwm_motor_slots[slot_num].pin1pwm = _TAKEN_SLOT; if( slot_num < 2 ){ @@ -266,10 +266,10 @@ void _configure3PWM(long pwm_frequency,const int pinA, const int pinB, const int // - hardware speciffic void _configure4PWM(long pwm_frequency,const int pinA, const int pinB, const int pinC, const int pinD) { if(!pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = _PWM_FREQUENCY; // default frequency 25hz - else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 40kHz max + else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 40kHz max stepper_4pwm_motor_slots_t m_slot = {}; // determine which motor are we connecting - // and set the appropriate configuration parameters + // and set the appropriate configuration parameters int slot_num; for(slot_num = 0; slot_num < 2; slot_num++){ if(esp32_stepper_4pwm_motor_slots[slot_num].pin1A == _EMPTY_SLOT){ // put the new motor in the first empty slot @@ -278,7 +278,7 @@ void _configure4PWM(long pwm_frequency,const int pinA, const int pinB, const int break; } } - // disable all the slots with the same MCPWM + // disable all the slots with the same MCPWM if( slot_num == 0 ){ // slots 0 and 1 of the 3pwm bldc esp32_bldc_3pwm_motor_slots[0].pinA = _TAKEN_SLOT; @@ -297,7 +297,7 @@ void _configure4PWM(long pwm_frequency,const int pinA, const int pinB, const int esp32_stepper_2pwm_motor_slots[3].pin1pwm = _TAKEN_SLOT; // slot 1 of the 6pwm bldc esp32_bldc_6pwm_motor_slots[1].pinAH = _TAKEN_SLOT; - } + } // configure pins mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_1a, pinA); @@ -314,7 +314,7 @@ void _configure4PWM(long pwm_frequency,const int pinA, const int pinB, const int // - hardware speciffic // ESP32 uses MCPWM void _writeDutyCycle2PWM(float dc_a, float dc_b, int pinA, int pinB){ - // determine which motor slot is the motor connected to + // determine which motor slot is the motor connected to for(int i = 0; i < 4; i++){ if(esp32_stepper_2pwm_motor_slots[i].pin1pwm == pinA){ // if motor slot found // se the PWM on the slot timers @@ -331,7 +331,7 @@ void _writeDutyCycle2PWM(float dc_a, float dc_b, int pinA, int pinB){ // - hardware speciffic // ESP32 uses MCPWM void _writeDutyCycle3PWM(float dc_a, float dc_b, float dc_c, int pinA, int pinB, int pinC){ - // determine which motor slot is the motor connected to + // determine which motor slot is the motor connected to for(int i = 0; i < 4; i++){ if(esp32_bldc_3pwm_motor_slots[i].pinA == pinA){ // if motor slot found // se the PWM on the slot timers @@ -349,7 +349,7 @@ void _writeDutyCycle3PWM(float dc_a, float dc_b, float dc_c, int pinA, int pinB // - hardware speciffic // ESP32 uses MCPWM void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, int pin1A, int pin1B, int pin2A, int pin2B){ - // determine which motor slot is the motor connected to + // determine which motor slot is the motor connected to for(int i = 0; i < 2; i++){ if(esp32_stepper_4pwm_motor_slots[i].pin1A == pin1A){ // if motor slot found // se the PWM on the slot timers @@ -367,12 +367,12 @@ void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, in // - BLDC driver - 6PWM setting // - hardware specific int _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const int pinA_l, const int pinB_h, const int pinB_l, const int pinC_h, const int pinC_l){ - + if(!pwm_frequency || !_isset(pwm_frequency) ) pwm_frequency = 20000; // default frequency 20khz - centered pwm has twice lower frequency else pwm_frequency = _constrain(pwm_frequency, 0, _PWM_FREQUENCY_MAX); // constrain to 40kHz max - centered pwm has twice lower frequency bldc_6pwm_motor_slots_t m_slot = {}; // determine which motor are we connecting - // and set the appropriate configuration parameters + // and set the appropriate configuration parameters int slot_num; for(slot_num = 0; slot_num < 2; slot_num++){ if(esp32_bldc_6pwm_motor_slots[slot_num].pinAH == _EMPTY_SLOT){ // put the new motor in the first empty slot @@ -384,7 +384,7 @@ int _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const // if no slots available if(slot_num >= 2) return -1; - // disable all the slots with the same MCPWM + // disable all the slots with the same MCPWM if( slot_num == 0 ){ // slots 0 and 1 of the 3pwm bldc esp32_bldc_3pwm_motor_slots[0].pinA = _TAKEN_SLOT; @@ -397,7 +397,7 @@ int _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const esp32_bldc_3pwm_motor_slots[3].pinA = _TAKEN_SLOT; // slot 1 of the 6pwm bldc esp32_stepper_4pwm_motor_slots[1].pin1A = _TAKEN_SLOT; - } + } // configure pins mcpwm_gpio_init(m_slot.mcpwm_unit, m_slot.mcpwm_ah, pinA_h); @@ -409,7 +409,7 @@ int _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const // configure the timer _configureTimerFrequency(pwm_frequency, m_slot.mcpwm_num, m_slot.mcpwm_unit, dead_zone); - // return + // return return 0; } @@ -417,7 +417,7 @@ int _configure6PWM(long pwm_frequency, float dead_zone, const int pinA_h, const // - BLDC driver - 6PWM setting // - hardware specific void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, float dead_zone, int pinA_h, int pinA_l, int pinB_h, int pinB_l, int pinC_h, int pinC_l){ - // determine which motor slot is the motor connected to + // determine which motor slot is the motor connected to for(int i = 0; i < 2; i++){ if(esp32_bldc_6pwm_motor_slots[i].pinAH == pinA_h){ // if motor slot found // se the PWM on the slot timers diff --git a/src/drivers/hardware_specific/generic_mcu.cpp b/src/drivers/hardware_specific/generic_mcu.cpp index 6549de07..5cc8bc4b 100644 --- a/src/drivers/hardware_specific/generic_mcu.cpp +++ b/src/drivers/hardware_specific/generic_mcu.cpp @@ -1,5 +1,10 @@ #include "../hardware_api.h" +// if the mcu doen't have defiend analogWrite +#if defined(ESP_H) + __attribute__((weak)) void analogWrite(uint8_t pin, int value){ }; +#endif + // function setting the high pwm frequency to the supplied pins // - Stepper motor - 2PWM setting // - hardware speciffic @@ -53,34 +58,34 @@ __attribute__((weak)) int _configure6PWM(long pwm_frequency, float dead_zone, co } -// function setting the pwm duty cycle to the hardware +// function setting the pwm duty cycle to the hardware // - Stepper motor - 2PWM setting // - hardware speciffic __attribute__((weak)) void _writeDutyCycle2PWM(float dc_a, float dc_b, int pinA, int pinB){ // transform duty cycle from [0,1] to [0,255] - analogWrite(pinA, 255.0*dc_a); - analogWrite(pinB, 255.0*dc_b); + analogWrite(pinA, 255.0f*dc_a); + analogWrite(pinB, 255.0f*dc_b); } -// function setting the pwm duty cycle to the hardware +// function setting the pwm duty cycle to the hardware // - BLDC motor - 3PWM setting // - hardware speciffic __attribute__((weak)) void _writeDutyCycle3PWM(float dc_a, float dc_b, float dc_c, int pinA, int pinB, int pinC){ // transform duty cycle from [0,1] to [0,255] - analogWrite(pinA, 255.0*dc_a); - analogWrite(pinB, 255.0*dc_b); - analogWrite(pinC, 255.0*dc_c); + analogWrite(pinA, 255.0f*dc_a); + analogWrite(pinB, 255.0f*dc_b); + analogWrite(pinC, 255.0f*dc_c); } -// function setting the pwm duty cycle to the hardware +// function setting the pwm duty cycle to the hardware // - Stepper motor - 4PWM setting // - hardware speciffic __attribute__((weak)) void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, int pin1A, int pin1B, int pin2A, int pin2B){ // transform duty cycle from [0,1] to [0,255] - analogWrite(pin1A, 255.0*dc_1a); - analogWrite(pin1B, 255.0*dc_1b); - analogWrite(pin2A, 255.0*dc_2a); - analogWrite(pin2B, 255.0*dc_2b); + analogWrite(pin1A, 255.0f*dc_1a); + analogWrite(pin1B, 255.0f*dc_1b); + analogWrite(pin2A, 255.0f*dc_2a); + analogWrite(pin2B, 255.0f*dc_2b); } @@ -100,4 +105,3 @@ __attribute__((weak)) void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc _UNUSED(pinC_l); return; } - diff --git a/src/drivers/hardware_specific/rp2040_mcu.cpp b/src/drivers/hardware_specific/rp2040_mcu.cpp index 097b7a6d..7be8faaf 100644 --- a/src/drivers/hardware_specific/rp2040_mcu.cpp +++ b/src/drivers/hardware_specific/rp2040_mcu.cpp @@ -147,7 +147,7 @@ void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, in inline float swDti(float val, float dt) { float ret = dt+val; - if (ret>1.0) ret = 1.0; + if (ret>1.0) ret = 1.0f; return ret; } diff --git a/src/drivers/hardware_specific/samd_mcu.cpp b/src/drivers/hardware_specific/samd_mcu.cpp index 5345aa70..edec2866 100644 --- a/src/drivers/hardware_specific/samd_mcu.cpp +++ b/src/drivers/hardware_specific/samd_mcu.cpp @@ -704,7 +704,7 @@ void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, float dead_zone, i if (tcc1->tcc.chaninfo!=tcc2->tcc.chaninfo) { // low-side on a different pin of same TCC - do dead-time in software... float ls = dc_a+(dead_zone*(SIMPLEFOC_SAMD_PWM_RESOLUTION-1)); - if (ls>1.0) ls = 1.0; // no off-time is better than too-short dead-time + if (ls>1.0) ls = 1.0f; // no off-time is better than too-short dead-time writeSAMDDutyCycle(tcc1->tcc.chaninfo, dc_a); writeSAMDDutyCycle(tcc2->tcc.chaninfo, ls); } @@ -715,7 +715,7 @@ void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, float dead_zone, i tcc2 = getTccPinConfiguration(pinB_l); if (tcc1->tcc.chaninfo!=tcc2->tcc.chaninfo) { float ls = dc_b+(dead_zone*(SIMPLEFOC_SAMD_PWM_RESOLUTION-1)); - if (ls>1.0) ls = 1.0; // no off-time is better than too-short dead-time + if (ls>1.0) ls = 1.0f; // no off-time is better than too-short dead-time writeSAMDDutyCycle(tcc1->tcc.chaninfo, dc_b); writeSAMDDutyCycle(tcc2->tcc.chaninfo, ls); } @@ -726,7 +726,7 @@ void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, float dead_zone, i tcc2 = getTccPinConfiguration(pinC_l); if (tcc1->tcc.chaninfo!=tcc2->tcc.chaninfo) { float ls = dc_c+(dead_zone*(SIMPLEFOC_SAMD_PWM_RESOLUTION-1)); - if (ls>1.0) ls = 1.0; // no off-time is better than too-short dead-time + if (ls>1.0) ls = 1.0f; // no off-time is better than too-short dead-time writeSAMDDutyCycle(tcc1->tcc.chaninfo, dc_c); writeSAMDDutyCycle(tcc2->tcc.chaninfo, ls); } @@ -866,5 +866,3 @@ void printTCCConfiguration(tccConfiguration& info) { #endif #endif - - diff --git a/src/drivers/hardware_specific/stm32_mcu.cpp b/src/drivers/hardware_specific/stm32_mcu.cpp index 3f213711..b0ac00e7 100644 --- a/src/drivers/hardware_specific/stm32_mcu.cpp +++ b/src/drivers/hardware_specific/stm32_mcu.cpp @@ -27,12 +27,12 @@ void _setPwm(int ulPin, uint32_t value, int resolution) } -// init pin pwm +// init pin pwm HardwareTimer* _initPinPWM(uint32_t PWM_freq, int ulPin) { PinName pin = digitalPinToPinName(ulPin); TIM_TypeDef *Instance = (TIM_TypeDef *)pinmap_peripheral(pin, PinMap_PWM); - + uint32_t index = get_timer_index(Instance); if (HardwareTimer_Handle[index] == NULL) { HardwareTimer_Handle[index]->__this = new HardwareTimer((TIM_TypeDef *)pinmap_peripheral(pin, PinMap_PWM)); @@ -61,7 +61,7 @@ HardwareTimer* _initPinPWMLow(uint32_t PWM_freq, int ulPin) PinName pin = digitalPinToPinName(ulPin); TIM_TypeDef *Instance = (TIM_TypeDef *)pinmap_peripheral(pin, PinMap_PWM); uint32_t index = get_timer_index(Instance); - + if (HardwareTimer_Handle[index] == NULL) { HardwareTimer_Handle[index]->__this = new HardwareTimer((TIM_TypeDef *)pinmap_peripheral(pin, PinMap_PWM)); HardwareTimer_Handle[index]->handle.Init.CounterMode = TIM_COUNTERMODE_CENTERALIGNED3; @@ -129,17 +129,17 @@ HardwareTimer* _initHardware6PWMInterface(uint32_t PWM_freq, float dead_zone, in PinName wlPinName = digitalPinToPinName(pinC_l); TIM_TypeDef *Instance = (TIM_TypeDef *)pinmap_peripheral(uhPinName, PinMap_PWM); - + uint32_t index = get_timer_index(Instance); - + if (HardwareTimer_Handle[index] == NULL) { HardwareTimer_Handle[index]->__this = new HardwareTimer((TIM_TypeDef *)pinmap_peripheral(uhPinName, PinMap_PWM)); HardwareTimer_Handle[index]->handle.Init.CounterMode = TIM_COUNTERMODE_CENTERALIGNED3; HAL_TIM_Base_Init(&(HardwareTimer_Handle[index]->handle)); - ((HardwareTimer *)HardwareTimer_Handle[index]->__this)->setOverflow(PWM_freq, HERTZ_FORMAT); + ((HardwareTimer *)HardwareTimer_Handle[index]->__this)->setOverflow(PWM_freq, HERTZ_FORMAT); } HardwareTimer *HT = (HardwareTimer *)(HardwareTimer_Handle[index]->__this); - + HT->setMode(STM_PIN_CHANNEL(pinmap_function(uhPinName, PinMap_PWM)), TIMER_OUTPUT_COMPARE_PWM1, uhPinName); HT->setMode(STM_PIN_CHANNEL(pinmap_function(ulPinName, PinMap_PWM)), TIMER_OUTPUT_COMPARE_PWM1, ulPinName); HT->setMode(STM_PIN_CHANNEL(pinmap_function(vhPinName, PinMap_PWM)), TIMER_OUTPUT_COMPARE_PWM1, vhPinName); @@ -148,20 +148,20 @@ HardwareTimer* _initHardware6PWMInterface(uint32_t PWM_freq, float dead_zone, in HT->setMode(STM_PIN_CHANNEL(pinmap_function(wlPinName, PinMap_PWM)), TIMER_OUTPUT_COMPARE_PWM1, wlPinName); // dead time is set in nanoseconds - uint32_t dead_time_ns = (float)(1e9/PWM_freq)*dead_zone; + uint32_t dead_time_ns = (float)(1e9f/PWM_freq)*dead_zone; uint32_t dead_time = __LL_TIM_CALC_DEADTIME(SystemCoreClock, LL_TIM_GetClockDivision(HT->getHandle()->Instance), dead_time_ns); LL_TIM_OC_SetDeadTime(HT->getHandle()->Instance, dead_time); // deadtime is non linear! LL_TIM_CC_EnableChannel(HT->getHandle()->Instance, LL_TIM_CHANNEL_CH1 | LL_TIM_CHANNEL_CH1N | LL_TIM_CHANNEL_CH2 | LL_TIM_CHANNEL_CH2N | LL_TIM_CHANNEL_CH3 | LL_TIM_CHANNEL_CH3N); HT->pause(); HT->refresh(); - HT->resume(); + HT->resume(); return HT; } // returns 0 if each pair of pwm channels has same channel -// returns 1 all the channels belong to the same timer - hardware inverted channels +// returns 1 all the channels belong to the same timer - hardware inverted channels // returns -1 if neither - avoid configuring - error!!! int _interfaceType(const int pinA_h, const int pinA_l, const int pinB_h, const int pinB_l, const int pinC_h, const int pinC_l){ PinName nameAH = digitalPinToPinName(pinA_h); @@ -177,7 +177,7 @@ int _interfaceType(const int pinA_h, const int pinA_l, const int pinB_h, const int tim5 = get_timer_index((TIM_TypeDef *)pinmap_peripheral(nameCH, PinMap_PWM)); int tim6 = get_timer_index((TIM_TypeDef *)pinmap_peripheral(nameCL, PinMap_PWM)); if(tim1 == tim2 && tim2==tim3 && tim3==tim4 && tim4==tim5 && tim5==tim6) - return _HARDWARE_6PWM; // hardware 6pwm interface - only on timer + return _HARDWARE_6PWM; // hardware 6pwm interface - only on timer else if(tim1 == tim2 && tim3==tim4 && tim5==tim6) return _SOFTWARE_6PWM; // software 6pwm interface - each pair of high-low side same timer else @@ -230,7 +230,7 @@ void _configure4PWM(long pwm_frequency,const int pinA, const int pinB, const int HardwareTimer* HT1 = _initPinPWM(pwm_frequency, pinA); HardwareTimer* HT2 = _initPinPWM(pwm_frequency, pinB); HardwareTimer* HT3 = _initPinPWM(pwm_frequency, pinC); - HardwareTimer* HT4 = _initPinPWM(pwm_frequency, pinD); + HardwareTimer* HT4 = _initPinPWM(pwm_frequency, pinD); // allign the timers _alignPWMTimers(HT1, HT2, HT3, HT4); } @@ -315,11 +315,11 @@ void _writeDutyCycle6PWM(float dc_a, float dc_b, float dc_c, float dead_zone, i _setPwm(pinC_h, _PWM_RANGE*dc_c, _PWM_RESOLUTION); break; case _SOFTWARE_6PWM: - _setPwm(pinA_l, _constrain(dc_a + dead_zone/2, 0, 1)*_PWM_RANGE, _PWM_RESOLUTION); + _setPwm(pinA_l, _constrain(dc_a + dead_zone/2, 0, 1)*_PWM_RANGE, _PWM_RESOLUTION); _setPwm(pinA_h, _constrain(dc_a - dead_zone/2, 0, 1)*_PWM_RANGE, _PWM_RESOLUTION); - _setPwm(pinB_l, _constrain(dc_b + dead_zone/2, 0, 1)*_PWM_RANGE, _PWM_RESOLUTION); + _setPwm(pinB_l, _constrain(dc_b + dead_zone/2, 0, 1)*_PWM_RANGE, _PWM_RESOLUTION); _setPwm(pinB_h, _constrain(dc_b - dead_zone/2, 0, 1)*_PWM_RANGE, _PWM_RESOLUTION); - _setPwm(pinC_l, _constrain(dc_c + dead_zone/2, 0, 1)*_PWM_RANGE, _PWM_RESOLUTION); + _setPwm(pinC_l, _constrain(dc_c + dead_zone/2, 0, 1)*_PWM_RANGE, _PWM_RESOLUTION); _setPwm(pinC_h, _constrain(dc_c - dead_zone/2, 0, 1)*_PWM_RANGE, _PWM_RESOLUTION); break; } diff --git a/src/drivers/hardware_specific/teensy_mcu.cpp b/src/drivers/hardware_specific/teensy_mcu.cpp index 5f70db4f..67bb0ad1 100644 --- a/src/drivers/hardware_specific/teensy_mcu.cpp +++ b/src/drivers/hardware_specific/teensy_mcu.cpp @@ -1,4 +1,4 @@ -#include "../hardware_api.h" +#include "../hardware_api.h" #if defined(__arm__) && defined(CORE_TEENSY) @@ -8,7 +8,7 @@ // configure High PWM frequency void _setHighFrequency(const long freq, const int pin){ analogWrite(pin, 0); - analogWriteFrequency(pin, freq); + analogWriteFrequency(pin, freq); } @@ -42,25 +42,25 @@ void _configure4PWM(long pwm_frequency,const int pinA, const int pinB, const int _setHighFrequency(pwm_frequency, pinA); _setHighFrequency(pwm_frequency, pinB); _setHighFrequency(pwm_frequency, pinC); - _setHighFrequency(pwm_frequency, pinD); + _setHighFrequency(pwm_frequency, pinD); } -// function setting the pwm duty cycle to the hardware +// function setting the pwm duty cycle to the hardware // - Stepper motor - 2PWM setting // - hardware speciffic void _writeDutyCycle2PWM(float dc_a, float dc_b, int pinA, int pinB){ // transform duty cycle from [0,1] to [0,255] - analogWrite(pinA, 255.0*dc_a); - analogWrite(pinB, 255.0*dc_b); + analogWrite(pinA, 255.0f*dc_a); + analogWrite(pinB, 255.0f*dc_b); } // function setting the pwm duty cycle to the hardware // - BLDC motor - 3PWM setting // - hardware speciffic void _writeDutyCycle3PWM(float dc_a, float dc_b, float dc_c, int pinA, int pinB, int pinC){ // transform duty cycle from [0,1] to [0,255] - analogWrite(pinA, 255.0*dc_a); - analogWrite(pinB, 255.0*dc_b); - analogWrite(pinC, 255.0*dc_c); + analogWrite(pinA, 255.0f*dc_a); + analogWrite(pinB, 255.0f*dc_b); + analogWrite(pinC, 255.0f*dc_c); } // function setting the pwm duty cycle to the hardware @@ -68,10 +68,10 @@ void _writeDutyCycle3PWM(float dc_a, float dc_b, float dc_c, int pinA, int pinB // - hardware speciffic void _writeDutyCycle4PWM(float dc_1a, float dc_1b, float dc_2a, float dc_2b, int pin1A, int pin1B, int pin2A, int pin2B){ // transform duty cycle from [0,1] to [0,255] - analogWrite(pin1A, 255.0*dc_1a); - analogWrite(pin1B, 255.0*dc_1b); - analogWrite(pin2A, 255.0*dc_2a); - analogWrite(pin2B, 255.0*dc_2b); + analogWrite(pin1A, 255.0f*dc_1a); + analogWrite(pin1B, 255.0f*dc_1b); + analogWrite(pin2A, 255.0f*dc_2a); + analogWrite(pin2B, 255.0f*dc_2b); } #endif \ No newline at end of file diff --git a/src/sensors/Encoder.cpp b/src/sensors/Encoder.cpp index 001330ba..4d110d7d 100644 --- a/src/sensors/Encoder.cpp +++ b/src/sensors/Encoder.cpp @@ -112,12 +112,12 @@ float Encoder::getVelocity(){ // timestamp long timestamp_us = _micros(); // sampling time calculation - float Ts = (timestamp_us - prev_timestamp_us) * 1e-6; + float Ts = (timestamp_us - prev_timestamp_us) * 1e-6f; // quick fix for strange cases (micros overflow) - if(Ts <= 0 || Ts > 0.5) Ts = 1e-3; + if(Ts <= 0 || Ts > 0.5f) Ts = 1e-3f; // time from last impulse - float Th = (timestamp_us - pulse_timestamp) * 1e-6; + float Th = (timestamp_us - pulse_timestamp) * 1e-6f; long dN = pulse_counter - prev_pulse_counter; // Pulse per second calculation (Eq.3.) @@ -129,8 +129,8 @@ float Encoder::getVelocity(){ float dt = Ts + prev_Th - Th; pulse_per_second = (dN != 0 && dt > Ts/2) ? dN / dt : pulse_per_second; - // if more than 0.05 passed in between impulses - if ( Th > 0.1) pulse_per_second = 0; + // if more than 0.05f passed in between impulses + if ( Th > 0.1f) pulse_per_second = 0; // velocity calculation float velocity = pulse_per_second / ((float)cpr) * (_2PI); diff --git a/src/sensors/HallSensor.cpp b/src/sensors/HallSensor.cpp index 2ceb22cf..a7a9f005 100644 --- a/src/sensors/HallSensor.cpp +++ b/src/sensors/HallSensor.cpp @@ -7,14 +7,14 @@ - pp - pole pairs */ HallSensor::HallSensor(int _hallA, int _hallB, int _hallC, int _pp){ - + // hardware pins pinA = _hallA; pinB = _hallB; pinC = _hallC; // hall has 6 segments per electrical revolution - cpr = _pp * 6; + cpr = _pp * 6; // extern pullup as default pullup = Pullup::USE_EXTERN; @@ -40,12 +40,12 @@ void HallSensor::handleC() { /** * Updates the state and sector following an interrupt - */ + */ void HallSensor::updateState() { long new_pulse_timestamp = _micros(); int8_t new_hall_state = C_active + (B_active << 1) + (A_active << 2); - + // glitch avoidance #1 - sometimes we get an interrupt but pins haven't changed if (new_hall_state == hall_state) { return; @@ -74,7 +74,7 @@ void HallSensor::updateState() { } else { pulse_diff = 0; } - + pulse_timestamp = new_pulse_timestamp; total_interrupts++; old_direction = direction; @@ -87,7 +87,7 @@ void HallSensor::updateState() { * ... // for debug or call driver directly? * } * sensor.attachSectorCallback(onSectorChange); - */ + */ void HallSensor::attachSectorCallback(void (*_onSectorChange)(int sector)) { onSectorChange = _onSectorChange; } @@ -107,12 +107,12 @@ float HallSensor::getVelocity(){ if (pulse_diff == 0 || ((_micros() - pulse_timestamp) > pulse_diff) ) { // last velocity isn't accurate if too old return 0; } else { - return direction * (_2PI / cpr) / (pulse_diff / 1000000.0); + return direction * (_2PI / cpr) / (pulse_diff / 1000000.0f); } } -// HallSensor initialisation of the hardware pins +// HallSensor initialisation of the hardware pins // and calculation variables void HallSensor::init(){ // initialise the electrical rotations to 0 @@ -134,7 +134,7 @@ void HallSensor::init(){ B_active = digitalRead(pinB); C_active = digitalRead(pinC); updateState(); - + pulse_timestamp = _micros(); } @@ -149,4 +149,3 @@ void HallSensor::enableInterrupts(void (*doA)(), void(*doB)(), void(*doC)()){ if(doB != nullptr) attachInterrupt(digitalPinToInterrupt(pinB), doB, CHANGE); if(doC != nullptr) attachInterrupt(digitalPinToInterrupt(pinC), doC, CHANGE); } - diff --git a/src/sensors/MagneticSensorAnalog.cpp b/src/sensors/MagneticSensorAnalog.cpp index 35f6d73c..f67d432b 100644 --- a/src/sensors/MagneticSensorAnalog.cpp +++ b/src/sensors/MagneticSensorAnalog.cpp @@ -6,7 +6,7 @@ * @param _max_raw_count the largest value read. whilst you might expect it to be 2^10 = 1023 it is often ~ 1020. Note: For ESP32 (with 12bit ADC the value will be nearer 4096) */ MagneticSensorAnalog::MagneticSensorAnalog(uint8_t _pinAnalog, int _min_raw_count, int _max_raw_count){ - + pinAnalog = _pinAnalog; cpr = _max_raw_count - _min_raw_count; @@ -26,30 +26,30 @@ void MagneticSensorAnalog::init(){ // velocity calculation init angle_prev = 0; - velocity_calc_timestamp = _micros(); + velocity_calc_timestamp = _micros(); // full rotations tracking number full_rotation_offset = 0; - raw_count_prev = getRawCount(); + raw_count_prev = getRawCount(); } // Shaft angle calculation // angle is in radians [rad] float MagneticSensorAnalog::getAngle(){ // raw data from the sensor - raw_count = getRawCount(); + raw_count = getRawCount(); int delta = raw_count - raw_count_prev; // if overflow happened track it as full rotation - if(abs(delta) > (0.8*cpr) ) full_rotation_offset += delta > 0 ? -_2PI : _2PI; - + if(abs(delta) > (0.8f*cpr) ) full_rotation_offset += delta > 0 ? -_2PI : _2PI; + float angle = full_rotation_offset + ( (float) (raw_count) / (float)cpr) * _2PI; - // calculate velocity here + // calculate velocity here long now = _micros(); - float Ts = ( now - velocity_calc_timestamp)*1e-6; + float Ts = ( now - velocity_calc_timestamp)*1e-6f; // quick fix for strange cases (micros overflow) - if(Ts <= 0 || Ts > 0.5) Ts = 1e-3; + if(Ts <= 0 || Ts > 0.5f) Ts = 1e-3f; velocity = (angle - angle_prev)/Ts; // save variables for future pass diff --git a/src/sensors/MagneticSensorI2C.cpp b/src/sensors/MagneticSensorI2C.cpp index c9029f6e..56d202dc 100644 --- a/src/sensors/MagneticSensorI2C.cpp +++ b/src/sensors/MagneticSensorI2C.cpp @@ -19,21 +19,21 @@ MagneticSensorI2CConfig_s AS5048_I2C = { // MagneticSensorI2C(uint8_t _chip_address, float _cpr, uint8_t _angle_register_msb) // @param _chip_address I2C chip address -// @param _bit_resolution bit resolution of the sensor +// @param _bit_resolution bit resolution of the sensor // @param _angle_register_msb angle read register // @param _bits_used_msb number of used bits in msb MagneticSensorI2C::MagneticSensorI2C(uint8_t _chip_address, int _bit_resolution, uint8_t _angle_register_msb, int _bits_used_msb){ // chip I2C address - chip_address = _chip_address; + chip_address = _chip_address; // angle read register of the magnetic sensor angle_register_msb = _angle_register_msb; // register maximum value (counts per revolution) cpr = pow(2, _bit_resolution); - + // depending on the sensor architecture there are different combinations of // LSB and MSB register used bits // AS5600 uses 0..7 LSB and 8..11 MSB - // AS5048 uses 0..5 LSB and 6..13 MSB + // AS5048 uses 0..5 LSB and 6..13 MSB // used bits in LSB lsb_used = _bit_resolution - _bits_used_msb; // extraction masks @@ -43,13 +43,13 @@ MagneticSensorI2C::MagneticSensorI2C(uint8_t _chip_address, int _bit_resolution, } MagneticSensorI2C::MagneticSensorI2C(MagneticSensorI2CConfig_s config){ - chip_address = config.chip_address; + chip_address = config.chip_address; // angle read register of the magnetic sensor angle_register_msb = config.angle_register; // register maximum value (counts per revolution) cpr = pow(2, config.bit_resolution); - + int bits_used_msb = config.data_start_bit - 7; lsb_used = config.bit_resolution - bits_used_msb; // extraction masks @@ -61,36 +61,36 @@ MagneticSensorI2C::MagneticSensorI2C(MagneticSensorI2CConfig_s config){ void MagneticSensorI2C::init(TwoWire* _wire){ wire = _wire; - + // I2C communication begin wire->begin(); // velocity calculation init angle_prev = 0; - velocity_calc_timestamp = _micros(); + velocity_calc_timestamp = _micros(); // full rotations tracking number full_rotation_offset = 0; - angle_data_prev = getRawCount(); + angle_data_prev = getRawCount(); } // Shaft angle calculation // angle is in radians [rad] float MagneticSensorI2C::getAngle(){ // raw data from the sensor - float angle_data = getRawCount(); + float angle_data = getRawCount(); - // tracking the number of rotations - // in order to expand angle range form [0,2PI] + // tracking the number of rotations + // in order to expand angle range form [0,2PI] // to basically infinity float d_angle = angle_data - angle_data_prev; // if overflow happened track it as full rotation - if(abs(d_angle) > (0.8*cpr) ) full_rotation_offset += d_angle > 0 ? -_2PI : _2PI; + if(abs(d_angle) > (0.8f*cpr) ) full_rotation_offset += d_angle > 0 ? -_2PI : _2PI; // save the current angle value for the next steps // in order to know if overflow happened angle_data_prev = angle_data; - // return the full angle - // (number of full rotations)*2PI + current sensor angle + // return the full angle + // (number of full rotations)*2PI + current sensor angle return (full_rotation_offset + ( angle_data / (float)cpr) * _2PI) ; } @@ -98,15 +98,15 @@ float MagneticSensorI2C::getAngle(){ float MagneticSensorI2C::getVelocity(){ // calculate sample time unsigned long now_us = _micros(); - float Ts = (now_us - velocity_calc_timestamp)*1e-6; + float Ts = (now_us - velocity_calc_timestamp)*1e-6f; // quick fix for strange cases (micros overflow) - if(Ts <= 0 || Ts > 0.5) Ts = 1e-3; + if(Ts <= 0 || Ts > 0.5f) Ts = 1e-3f; // current angle float angle_c = getAngle(); // velocity calculation float vel = (angle_c - angle_prev)/Ts; - + // save variables for future pass angle_prev = angle_c; velocity_calc_timestamp = now_us; @@ -119,7 +119,7 @@ int MagneticSensorI2C::getRawCount(){ return (int)MagneticSensorI2C::read(angle_register_msb); } -// I2C functions +// I2C functions /* * Read a register from the sensor * Takes the address of the register as a uint8_t @@ -133,14 +133,14 @@ int MagneticSensorI2C::read(uint8_t angle_reg_msb) { wire->beginTransmission(chip_address); wire->write(angle_reg_msb); wire->endTransmission(false); - + // read the data msb and lsb wire->requestFrom(chip_address, (uint8_t)2); for (byte i=0; i < 2; i++) { readArray[i] = wire->read(); } - // depending on the sensor architecture there are different combinations of + // depending on the sensor architecture there are different combinations of // LSB and MSB register used bits // AS5600 uses 0..7 LSB and 8..11 MSB // AS5048 uses 0..5 LSB and 6..13 MSB @@ -151,7 +151,7 @@ int MagneticSensorI2C::read(uint8_t angle_reg_msb) { /* * Checks whether other devices have locked the bus. Can clear SDA locks. -* This should be called before sensor.init() on devices that suffer i2c slaves locking sda +* This should be called before sensor.init() on devices that suffer i2c slaves locking sda * e.g some stm32 boards with AS5600 chips * Takes the sda_pin and scl_pin * Returns 0 for OK, 1 for other master and 2 for unfixable sda locked LOW @@ -160,7 +160,7 @@ int MagneticSensorI2C::checkBus(byte sda_pin, byte scl_pin) { pinMode(scl_pin, INPUT_PULLUP); pinMode(sda_pin, INPUT_PULLUP); - delay(250); + delay(250); if (digitalRead(scl_pin) == LOW) { // Someone else has claimed master!"); @@ -179,15 +179,15 @@ int MagneticSensorI2C::checkBus(byte sda_pin, byte scl_pin) { } pinMode(sda_pin, INPUT); delayMicroseconds(20); - if (digitalRead(sda_pin) == LOW) { + if (digitalRead(sda_pin) == LOW) { // SDA still blocked - return 2; - } + return 2; + } _delay(1000); } // SDA is clear (HIGH) pinMode(sda_pin, INPUT); pinMode(scl_pin, INPUT); - - return 0; + + return 0; } diff --git a/src/sensors/MagneticSensorPWM.cpp b/src/sensors/MagneticSensorPWM.cpp index da4ebf89..713bcc2a 100644 --- a/src/sensors/MagneticSensorPWM.cpp +++ b/src/sensors/MagneticSensorPWM.cpp @@ -13,7 +13,7 @@ MagneticSensorPWM::MagneticSensorPWM(uint8_t _pinPWM, int _min_raw_count, int _m cpr = _max_raw_count - _min_raw_count; min_raw_count = _min_raw_count; max_raw_count = _max_raw_count; - + // define if the sensor uses interrupts is_interrupt_based = false; @@ -23,7 +23,7 @@ MagneticSensorPWM::MagneticSensorPWM(uint8_t _pinPWM, int _min_raw_count, int _m void MagneticSensorPWM::init(){ - + // initial hardware pinMode(pinPWM, INPUT); @@ -36,23 +36,23 @@ void MagneticSensorPWM::init(){ raw_count_prev = getRawCount(); } -// get current angle (rad) +// get current angle (rad) float MagneticSensorPWM::getAngle(){ // raw data from sensor raw_count = getRawCount(); - + int delta = raw_count - raw_count_prev; // if overflow happened track it as full rotation - if(abs(delta) > (0.8*cpr) ) full_rotation_offset += delta > 0 ? -_2PI : _2PI; + if(abs(delta) > (0.8f*cpr) ) full_rotation_offset += delta > 0 ? -_2PI : _2PI; float angle = full_rotation_offset + ( (float) (raw_count) / (float)cpr) * _2PI; // calculate velocity here long now = _micros(); - float Ts = (now - velocity_calc_timestamp)*1e-6; + float Ts = (now - velocity_calc_timestamp)*1e-6f; // quick fix for strange cases (micros overflow) - if(Ts <= 0 || Ts > 0.5) Ts = 1e-3; + if(Ts <= 0 || Ts > 0.5f) Ts = 1e-3f; velocity = (angle - angle_prev)/Ts; // save variables for future pass @@ -80,7 +80,7 @@ int MagneticSensorPWM::getRawCount(){ void MagneticSensorPWM::handlePWM() { // unsigned long now_us = ticks(); unsigned long now_us = _micros(); - + // if falling edge, calculate the pulse length if (!digitalRead(pinPWM)) pulse_length_us = now_us - last_call_us; @@ -95,8 +95,6 @@ void MagneticSensorPWM::enableInterrupt(void (*doPWM)()){ // declare it's interrupt based is_interrupt_based = true; - #if !defined(__AVR_ATmega328P__) && !defined(__AVR_ATmega168__) && !defined(__AVR_ATmega328PB__) && !defined(__AVR_ATmega2560__) // if mcu is not atmega328 && if mcu is not atmega2560 - // enable interrupts on pwm input pin - attachInterrupt(digitalPinToInterrupt(pinPWM), doPWM, CHANGE); - #endif + // enable interrupts on pwm input pin + attachInterrupt(digitalPinToInterrupt(pinPWM), doPWM, CHANGE); } \ No newline at end of file diff --git a/src/sensors/MagneticSensorSPI.cpp b/src/sensors/MagneticSensorSPI.cpp index b3d82dee..a6e1a1b1 100644 --- a/src/sensors/MagneticSensorSPI.cpp +++ b/src/sensors/MagneticSensorSPI.cpp @@ -8,7 +8,7 @@ MagneticSensorSPIConfig_s AS5147_SPI = { .clock_speed = 1000000, .bit_resolution = 14, .angle_register = 0x3FFF, - .data_start_bit = 13, + .data_start_bit = 13, .command_rw_bit = 14, .command_parity_bit = 15 }; @@ -22,19 +22,19 @@ MagneticSensorSPIConfig_s MA730_SPI = { .clock_speed = 1000000, .bit_resolution = 14, .angle_register = 0x0000, - .data_start_bit = 15, + .data_start_bit = 15, .command_rw_bit = 0, // not required .command_parity_bit = 0 // parity not implemented }; // MagneticSensorSPI(int cs, float _bit_resolution, int _angle_register) -// cs - SPI chip select pin +// cs - SPI chip select pin // _bit_resolution sensor resolution bit number // _angle_register - (optional) angle read register - default 0x3FFF MagneticSensorSPI::MagneticSensorSPI(int cs, float _bit_resolution, int _angle_register){ - - chip_select_pin = cs; + + chip_select_pin = cs; // angle read register of the magnetic sensor angle_register = _angle_register ? _angle_register : DEF_ANGLE_REGISTER; // register maximum value (counts per revolution) @@ -42,14 +42,14 @@ MagneticSensorSPI::MagneticSensorSPI(int cs, float _bit_resolution, int _angle_r spi_mode = SPI_MODE1; clock_speed = 1000000; bit_resolution = _bit_resolution; - + command_parity_bit = 15; // for backwards compatibilty command_rw_bit = 14; // for backwards compatibilty data_start_bit = 13; // for backwards compatibilty } MagneticSensorSPI::MagneticSensorSPI(MagneticSensorSPIConfig_s config, int cs){ - chip_select_pin = cs; + chip_select_pin = cs; // angle read register of the magnetic sensor angle_register = config.angle_register ? config.angle_register : DEF_ANGLE_REGISTER; // register maximum value (counts per revolution) @@ -57,7 +57,7 @@ MagneticSensorSPI::MagneticSensorSPI(MagneticSensorSPIConfig_s config, int cs){ spi_mode = config.spi_mode; clock_speed = config.clock_speed; bit_resolution = config.bit_resolution; - + command_parity_bit = config.command_parity_bit; // for backwards compatibilty command_rw_bit = config.command_rw_bit; // for backwards compatibilty data_start_bit = config.data_start_bit; // for backwards compatibilty @@ -72,43 +72,40 @@ void MagneticSensorSPI::init(SPIClass* _spi){ //setup pins pinMode(chip_select_pin, OUTPUT); - + //SPI has an internal SPI-device counter, it is possible to call "begin()" from different devices spi->begin(); -#ifndef ESP_H // if not ESP32 board - spi->setBitOrder(MSBFIRST); // Set the SPI_1 bit order - spi->setDataMode(spi_mode) ; - spi->setClockDivider(SPI_CLOCK_DIV8); -#endif + // do any architectures need to set the clock divider for SPI? Why was this in the code? + //spi->setClockDivider(SPI_CLOCK_DIV8); digitalWrite(chip_select_pin, HIGH); // velocity calculation init angle_prev = 0; - velocity_calc_timestamp = _micros(); + velocity_calc_timestamp = _micros(); // full rotations tracking number full_rotation_offset = 0; - angle_data_prev = getRawCount(); + angle_data_prev = getRawCount(); } // Shaft angle calculation // angle is in radians [rad] float MagneticSensorSPI::getAngle(){ // raw data from the sensor - float angle_data = getRawCount(); + float angle_data = getRawCount(); - // tracking the number of rotations - // in order to expand angle range form [0,2PI] + // tracking the number of rotations + // in order to expand angle range form [0,2PI] // to basically infinity float d_angle = angle_data - angle_data_prev; // if overflow happened track it as full rotation - if(abs(d_angle) > (0.8*cpr) ) full_rotation_offset += d_angle > 0 ? -_2PI : _2PI; + if(abs(d_angle) > (0.8f*cpr) ) full_rotation_offset += d_angle > 0 ? -_2PI : _2PI; // save the current angle value for the next steps // in order to know if overflow happened angle_data_prev = angle_data; - // return the full angle - // (number of full rotations)*2PI + current sensor angle + // return the full angle + // (number of full rotations)*2PI + current sensor angle return full_rotation_offset + ( angle_data / (float)cpr) * _2PI; } @@ -116,9 +113,9 @@ float MagneticSensorSPI::getAngle(){ float MagneticSensorSPI::getVelocity(){ // calculate sample time unsigned long now_us = _micros(); - float Ts = (now_us - velocity_calc_timestamp)*1e-6; + float Ts = (now_us - velocity_calc_timestamp)*1e-6f; // quick fix for strange cases (micros overflow) - if(Ts <= 0 || Ts > 0.5) Ts = 1e-3; + if(Ts <= 0 || Ts > 0.5f) Ts = 1e-3f; // current angle float angle_c = getAngle(); @@ -137,7 +134,7 @@ int MagneticSensorSPI::getRawCount(){ return (int)MagneticSensorSPI::read(angle_register); } -// SPI functions +// SPI functions /** * Utility function used to calculate even parity of word */ @@ -170,36 +167,26 @@ word MagneticSensorSPI::read(word angle_register){ command |= ((word)spiCalcEvenParity(command) << command_parity_bit); } -#if !defined(_STM32_DEF_) // if not stm chips //SPI - begin transaction spi->beginTransaction(settings); -#endif //Send the command digitalWrite(chip_select_pin, LOW); - digitalWrite(chip_select_pin, LOW); spi->transfer16(command); digitalWrite(chip_select_pin,HIGH); - digitalWrite(chip_select_pin,HIGH); - #if defined( ESP_H ) // if ESP32 board - delayMicroseconds(50); + delayMicroseconds(50); // why do we need to delay 50us on ESP32? In my experience no extra delays are needed, on any of the architectures I've tested... #else - delayMicroseconds(10); + delayMicroseconds(1); // delay 1us, the minimum time possible in plain arduino. 350ns is the required time for AMS sensors, 80ns for MA730, MA702 #endif - + //Now read the response digitalWrite(chip_select_pin, LOW); - digitalWrite(chip_select_pin, LOW); word register_value = spi->transfer16(0x00); digitalWrite(chip_select_pin, HIGH); - digitalWrite(chip_select_pin,HIGH); -#if !defined(_STM32_DEF_) // if not stm chips //SPI - end transaction spi->endTransaction(); -#endif - register_value = register_value >> (1 + data_start_bit - bit_resolution); //this should shift data to the rightmost bits of the word const static word data_mask = 0xFFFF >> (16 - bit_resolution); From 1a9c7a746f886252222c9f42f7d1e88c9440c751 Mon Sep 17 00:00:00 2001 From: maxime Date: Tue, 8 Jun 2021 12:35:37 -0600 Subject: [PATCH 13/15] fix merge error --- src/current_sense/hardware_specific/samd21_mcu.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/src/current_sense/hardware_specific/samd21_mcu.cpp b/src/current_sense/hardware_specific/samd21_mcu.cpp index f21b5474..f1c35d0e 100644 --- a/src/current_sense/hardware_specific/samd21_mcu.cpp +++ b/src/current_sense/hardware_specific/samd21_mcu.cpp @@ -298,7 +298,6 @@ void SAMD21AsyncCurrentSense::initDMAChannel() { chctrlb.bit.EVACT = DMAC_CHCTRLB_EVACT_NOACT_Val; //only used if EVIE is set chctrlb.bit.TRIGSRC = ADC_DMAC_ID_RESRDY; chctrlb.bit.TRIGACT = DMAC_CHCTRLB_TRIGACT_BEAT_Val; //block, beat or transaction - DMAC->CHCTRLB.reg = chctrlb.reg; for(uint32_t i = firstAIN; i < lastAIN + 1; i++) { From 8067de4fa7045d618a9c1a44dc6760a2e7191181 Mon Sep 17 00:00:00 2001 From: maxime Date: Wed, 9 Jun 2021 13:37:17 -0600 Subject: [PATCH 14/15] SAMD21AdvancedSPI: now allows to mix and match sercom/sercom_alt --- .../hardware_specific/samd21_AdvancedSPI.cpp | 56 +++++++++++-------- 1 file changed, 32 insertions(+), 24 deletions(-) diff --git a/src/common/hardware_specific/samd21_AdvancedSPI.cpp b/src/common/hardware_specific/samd21_AdvancedSPI.cpp index add333be..469f7deb 100644 --- a/src/common/hardware_specific/samd21_AdvancedSPI.cpp +++ b/src/common/hardware_specific/samd21_AdvancedSPI.cpp @@ -35,6 +35,22 @@ SAMDAdvancedSPI::SAMDAdvancedSPI(SercomChannel sercomChannel, uint8_t pinMOSI, u } +bool getPad(const SamdPinDefinition * pinDef, SercomChannel sercomChannel, SercomPad & pad, EPioType & peripheral) +{ + if(pinDef->sercom == sercomChannel) + { + pad = pinDef->sercom_pad; + peripheral = EPioType::PIO_SERCOM; + return true; + } + if(pinDef->sercom_alt == sercomChannel) + { + pad = pinDef->sercom_pad_alt; + peripheral = EPioType::PIO_SERCOM_ALT; + return true; + } + return false; +} int SAMDAdvancedSPI::init(SercomSpiClockMode spi_mode, uint32_t clock_speed) { debugPrint(F("MOSI: ")); @@ -72,6 +88,7 @@ int SAMDAdvancedSPI::init(SercomSpiClockMode spi_mode, uint32_t clock_speed) if(!hardwareSS) { pinMode(pinSS, OUTPUT); + pinPeripheral(pinSS, EPioType::PIO_OUTPUT); digitalWrite(pinSS, HIGH); isSoftwareSSLow = false; } @@ -113,41 +130,32 @@ int SAMDAdvancedSPI::init(SercomSpiClockMode spi_mode, uint32_t clock_speed) return -1; } - SercomPad MOSI_pad, MISO_pad, SCK_pad, SS_pad; - EPioType peripheral; + SercomPad MOSI_pad, MISO_pad, SCK_pad, SS_pad = NO_PAD; + EPioType peripheralMOSI, peripheralMISO, peripheralSCK, peripheralSS; + - if(defMOSI->sercom == sercomChannel && defMISO->sercom == sercomChannel && defSCK->sercom == sercomChannel && (ctrlb.bit.MSSEN == 0 || defSS->sercom == sercomChannel)) + if( getPad(defMOSI, sercomChannel, MOSI_pad, peripheralMOSI ) + && getPad(defMISO, sercomChannel, MISO_pad, peripheralMISO ) + && getPad(defSCK, sercomChannel, SCK_pad , peripheralSCK ) + && (ctrlb.bit.MSSEN == 0 || getPad(defSS, sercomChannel, SS_pad , peripheralSS ))) { - MOSI_pad = defMOSI->sercom_pad; - MISO_pad = defMISO->sercom_pad; - SCK_pad = defSCK->sercom_pad; - SS_pad = defSS->sercom_pad; - peripheral = EPioType::PIO_SERCOM; - debugPrintf_P(PSTR("Using sercom %d\n\r"), defMOSI->sercom); + pinPeripheral(pinMOSI, peripheralMOSI); + pinPeripheral(pinMISO, peripheralMISO); + pinPeripheral(pinSCK, peripheralSCK); + if(ctrlb.bit.MSSEN) + pinPeripheral(pinSS, peripheralSS); } - else if(defMOSI->sercom_alt == sercomChannel && defMISO->sercom_alt == sercomChannel && defSCK->sercom_alt == sercomChannel && (ctrlb.bit.MSSEN == 0 || defSS->sercom_alt == sercomChannel)) - { - MOSI_pad = defMOSI->sercom_pad_alt; - MISO_pad = defMISO->sercom_pad_alt; - SCK_pad = defSCK->sercom_pad_alt; - SS_pad = defSS->sercom_pad_alt; - peripheral = EPioType::PIO_SERCOM_ALT; - - debugPrintf_P(PSTR("Using sercom alt %d\n\r"), defMOSI->sercom_alt); - } else { debugPrint(F("SPI sercom CHANNEL configuration error!")); + debugPrintf_P(PSTR("desired sercom %d, got [peripheral, pad]: MOSI [%d, %d], MISO [%d, %d], SCK [%d, %d], SS [%d, %d]\n\r"), sercomChannel, peripheralMOSI, MOSI_pad, peripheralMISO, MISO_pad, peripheralSCK, SCK_pad, peripheralSS, SS_pad); + return -1; } - pinPeripheral(pinMOSI, peripheral); - pinPeripheral(pinMISO, peripheral); - pinPeripheral(pinSCK, peripheral); - pinPeripheral(pinSS, ctrlb.bit.MSSEN ? peripheral : EPioType::PIO_OUTPUT); - debugPrintf_P(PSTR("Using sercom pads MOSI[%d], MISO[%d], SCK[%d], SS[%d]\n\r"), MOSI_pad, MISO_pad, SCK_pad, ctrlb.bit.MSSEN ? SS_pad : -1); + debugPrintf_P(PSTR("Using sercom [peripheral, pad]: MOSI [%d, %d], MISO [%d, %d], SCK [%d, %d], SS [%d, %d]\n\r"), peripheralMOSI, MOSI_pad, peripheralMISO, MISO_pad, peripheralSCK, SCK_pad, peripheralSS, SS_pad); if(MOSI_pad == PAD_0 && SCK_pad == PAD_1 && (ctrlb.bit.MSSEN ? (SS_pad == PAD_2 && MISO_pad == PAD_3) : (MISO_pad == PAD_3 || MISO_pad == PAD_2))) From 56c0fec0fc512469497c6797ef7fdbcb3c343444 Mon Sep 17 00:00:00 2001 From: maxime Date: Wed, 9 Jun 2021 13:37:57 -0600 Subject: [PATCH 15/15] SAMD21AsyncCurrentSense DIV4 was way too noisy --- .../hardware_specific/samd21_mcu.cpp | 20 +++++++++++-------- 1 file changed, 12 insertions(+), 8 deletions(-) diff --git a/src/current_sense/hardware_specific/samd21_mcu.cpp b/src/current_sense/hardware_specific/samd21_mcu.cpp index f1c35d0e..1a8e1195 100644 --- a/src/current_sense/hardware_specific/samd21_mcu.cpp +++ b/src/current_sense/hardware_specific/samd21_mcu.cpp @@ -157,29 +157,33 @@ void SAMD21AsyncCurrentSense::initPins(){ pinMode(pinA, INPUT); pinMode(pinB, INPUT); - + pinPeripheral(pinA, PIO_ANALOG); + pinPeripheral(pinB, PIO_ANALOG); ainA = getSamdPinDefinition(pinA)->adc_channel; - ainB = getSamdPinDefinition(pinB)->adc_channel; + ainC = ainB = getSamdPinDefinition(pinB)->adc_channel; firstAIN = min(ainA, ainB); lastAIN = max(ainA, ainB); if( _isset(pinC) ) { ainC = getSamdPinDefinition(pinC)->adc_channel; pinMode(pinC, INPUT); + pinPeripheral(pinC, PIO_ANALOG); firstAIN = min(firstAIN, ainC); lastAIN = max(lastAIN, ainC); } + oneBeforeFirstAIN = firstAIN - 1; //hack to discard noisy first readout bufferSize = lastAIN - oneBeforeFirstAIN + 1; + debugPrintf(PSTR("initPins() Using pin%d -> ain%d, pin%d -> ain%d, pin%d -> ain%d\n\r"), pinA, ainA, pinB, ainB, pinC, ainC); + } void SAMD21AsyncCurrentSense::initADC(){ - pinPeripheral(pinA, PIO_ANALOG); - pinPeripheral(pinB, PIO_ANALOG); - pinPeripheral(pinC, PIO_ANALOG); + + ADC->CTRLA.bit.ENABLE = 0x00; // Disable ADC @@ -206,13 +210,13 @@ void SAMD21AsyncCurrentSense::initADC(){ uint32_t linearity = (*((uint32_t *) ADC_FUSES_LINEARITY_0_ADDR) & ADC_FUSES_LINEARITY_0_Msk) >> ADC_FUSES_LINEARITY_0_Pos; linearity |= ((*((uint32_t *) ADC_FUSES_LINEARITY_1_ADDR) & ADC_FUSES_LINEARITY_1_Msk) >> ADC_FUSES_LINEARITY_1_Pos) << 5; ADC->CALIB.reg = ADC_CALIB_BIAS_CAL(bias) | ADC_CALIB_LINEARITY_CAL(linearity); - + ADCsync(); /* Configure reference */ ADC_REFCTRL_Type refctrl{.reg = 0}; // refctrl.bit.REFCOMP = 1; /* enable reference compensation */ refctrl.bit.REFSEL = refsel; ADC->REFCTRL.reg = refctrl.reg; - + ADCsync(); /* Bits 19:16 – INPUTSCAN[3:0]: Number of Input Channels Included in Scan This register gives the number of input sources included in the pin scan. The number of input sources included is @@ -257,7 +261,7 @@ void SAMD21AsyncCurrentSense::initADC(){ ADC_CTRLB_Type ctrlb{.reg = 0}; /* ADC clock is 8MHz / 4 = 2MHz */ - ctrlb.bit.PRESCALER = ADC_CTRLB_PRESCALER_DIV4_Val; + ctrlb.bit.PRESCALER = ADC_CTRLB_PRESCALER_DIV8_Val; ctrlb.bit.RESSEL = ADC_CTRLB_RESSEL_12BIT_Val; ctrlb.bit.CORREN = 0; ctrlb.bit.FREERUN = 0;