diff --git a/src/common/hardware_specific/samd21.cpp b/src/common/hardware_specific/samd21.cpp new file mode 100644 index 00000000..0b6e6d8e --- /dev/null +++ b/src/common/hardware_specific/samd21.cpp @@ -0,0 +1,381 @@ +#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}; + } +} + + +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_P(PSTR("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_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_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_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_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; + + /* 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() +{ + 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 ; + + 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}; + +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, DMACInterruptCallback * interrupt_handler, bool force) +{ + + if(channel > 11) + { + debugPrintf_P(PSTR("FATAL: initDMAChannel() channel %d out of bounds (max %d)\n\r"), channel, 12); + return -1; + } + + if(g_DMACChannelInitialized[channel] && !force) + { + 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; + + // 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; + debugPrintf_P(PSTR("initDMAChannel() inittializing channel %d\n\r"), channel); + + + 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_P(PSTR("Error in adcToDMATransfer() [DMAC] Bad channel number %d"), channel); + } +} + + +void DMAC_Handler() { + 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)(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 +{ + 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(F("initTCCHandler() Error: illegal nullptr handler")); + return nullptr; + } + + if(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; + } + + if(TCC_Handlers[tccn].count < MAX_HANDLERS) + { + + 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(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_P(PSTR("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 new file mode 100644 index 00000000..469f7deb --- /dev/null +++ b/src/common/hardware_specific/samd21_AdvancedSPI.cpp @@ -0,0 +1,261 @@ +#include "samd21_AdvancedSPI.h" +#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) +{ + +} + +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: ")); + defMOSI = getSamdPinDefinition(pinMOSI); + debugPrint(F("MISO: ")); + defMISO = getSamdPinDefinition(pinMISO); + debugPrint(F("SCK : ")); + defSCK = getSamdPinDefinition(pinSCK); + debugPrint(F("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); + pinPeripheral(pinSS, EPioType::PIO_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(F("SPI sercom CLOCK configuration error!")); + return -1; + } + + SercomPad MOSI_pad, MISO_pad, SCK_pad, SS_pad = NO_PAD; + EPioType peripheralMOSI, peripheralMISO, peripheralSCK, peripheralSS; + + + 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 ))) + { + + pinPeripheral(pinMOSI, peripheralMOSI); + pinPeripheral(pinMISO, peripheralMISO); + pinPeripheral(pinSCK, peripheralSCK); + if(ctrlb.bit.MSSEN) + pinPeripheral(pinSS, peripheralSS); + } + 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; + } + + + 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))) + { + 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(F("SPI sercom PADS configuration invalid!")); + return -1; + } + + 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; + + + ctrla.bit.ENABLE = 0b1; //***enables the SPI*** + + sercomCfg.sercom->SPI.CTRLA.reg = ctrla.reg; + while(sercomCfg.sercom->SPI.SYNCBUSY.bit.ENABLE); + + + return 0; + +} + +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) +{ + 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); +} \ 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..0b4fff70 --- /dev/null +++ b/src/common/hardware_specific/samd21_AdvancedSPI.h @@ -0,0 +1,43 @@ +#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..6b0fa15a --- /dev/null +++ b/src/common/hardware_specific/samd_mcu.cpp @@ -0,0 +1,83 @@ +#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; + } +} + +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) + { + debugPrint(F("getSamdPinDefinition() : pin < 0")); + return nullptr; + } + if((uint32_t)arduinoPin > (PINS_COUNT-1)) + { + debugPrintf_P(PSTR("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_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 new file mode 100644 index 00000000..e6e6eb13 --- /dev/null +++ b/src/common/hardware_specific/samd_mcu.h @@ -0,0 +1,213 @@ +#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[]; // *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 +template +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...) sprintf(buffer, args); debugPrint(buffer); +#define debugPrintf_P(args...) sprintf_P(buffer, args); debugPrint(buffer); +#else +#define debugPrintf(args...) ; +#define debugPrint(arg) ; +#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(); +class DMACInterruptCallback +{ + public: + 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); +void trigDMACChannel(uint8_t channel); +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/LowsideCurrentSense.cpp b/src/current_sense/LowsideCurrentSense.cpp index 18aefd49..102df319 100644 --- a/src/current_sense/LowsideCurrentSense.cpp +++ b/src/current_sense/LowsideCurrentSense.cpp @@ -27,34 +27,44 @@ void LowsideCurrentSense::init(){ calibrateOffsets(); } // Function finding zero offsets of the ADC -void LowsideCurrentSense::calibrateOffsets(){ - const int calibration_rounds = 1000; - +uint32_t LowsideCurrentSense::calibrateOffsets(const uint32_t calibration_rounds){ + + // find adc offset = zero current voltage - offset_ia = 0; - offset_ib = 0; - offset_ic = 0; + 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 < calibration_rounds; i++) { - _startADC3PinConversionLowSide(); - offset_ia += _readADCVoltageLowSide(pinA); - offset_ib += _readADCVoltageLowSide(pinB); - if(_isset(pinC)) offset_ic += _readADCVoltageLowSide(pinC); - _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++; + } } - // calculate the mean offsets - offset_ia = offset_ia / calibration_rounds; - offset_ib = offset_ib / calibration_rounds; - if(_isset(pinC)) offset_ic = offset_ic / calibration_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; + } + + return effective_rounds; } // 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 + _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/LowsideCurrentSense.h b/src/current_sense/LowsideCurrentSense.h index 9618b572..45f68462 100644 --- a/src/current_sense/LowsideCurrentSense.h +++ b/src/current_sense/LowsideCurrentSense.h @@ -25,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; @@ -49,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 f715ef9e..a4a8dc6a 100644 --- a/src/current_sense/hardware_api.h +++ b/src/current_sense/hardware_api.h @@ -1,5 +1,4 @@ -#ifndef HARDWARE_UTILS_H -#define HARDWARE_UTILS_H +#pragma once #include "../common/foc_utils.h" #include "../common/time_utils.h" @@ -29,14 +28,10 @@ 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 - * - * @param pinA - the arduino pin to be read (it has to be ADC pin) + * function returning 3 most recent voltage readings */ -float _readADCVoltageLowSide(const int pinA); +bool _readADCVoltagesLowSide(float & a, float & b, float & c); /** * function syncing the Driver with the ADC for the LowSide Sensing @@ -44,6 +39,3 @@ float _readADCVoltageLowSide(const int pinA); void _driverSyncLowSide(); -void _startADC3PinConversionLowSide(); - -#endif diff --git a/src/current_sense/hardware_specific/atmega_mcu.cpp b/src/current_sense/hardware_specific/atmega_mcu.cpp index 75265c03..93a116b9 100644 --- a/src/current_sense/hardware_specific/atmega_mcu.cpp +++ b/src/current_sense/hardware_specific/atmega_mcu.cpp @@ -1,13 +1,13 @@ #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)) @@ -40,16 +40,24 @@ void _configureADCInline(const int pinA,const int pinB,const int pinC){ } -// function reading an ADC value and returning the read voltage -float _readADCVoltageLowSide(const int pinA){ - return _readADCVoltageInline(pinA); +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 +#endif diff --git a/src/current_sense/hardware_specific/due_mcu.cpp b/src/current_sense/hardware_specific/due_mcu.cpp index ef450b35..a010f0b3 100644 --- a/src/current_sense/hardware_specific/due_mcu.cpp +++ b/src/current_sense/hardware_specific/due_mcu.cpp @@ -1,13 +1,13 @@ #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){ @@ -22,17 +22,25 @@ void _configureADCInline(const int pinA,const int pinB,const int pinC){ } -// function reading an ADC value and returning the read voltage -float _readADCVoltageLowSide(const int pinA){ - return _readADCVoltageInline(pinA); +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 +#endif diff --git a/src/current_sense/hardware_specific/esp32_mcu.cpp b/src/current_sense/hardware_specific/esp32_mcu.cpp index c880c56a..44c74b3c 100644 --- a/src/current_sense/hardware_specific/esp32_mcu.cpp +++ b/src/current_sense/hardware_specific/esp32_mcu.cpp @@ -1,6 +1,6 @@ #include "../hardware_api.h" -#ifdef ESP_H +#if defined(ESP_H) #include "driver/mcpwm.h" #include "soc/mcpwm_reg.h" @@ -16,7 +16,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; @@ -24,15 +24,13 @@ 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 -float _readADCVoltageLowSide(const int pin){ - uint32_t raw_adc; - - 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; +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 diff --git a/src/current_sense/hardware_specific/generic_mcu.cpp b/src/current_sense/hardware_specific/generic_mcu.cpp index ecc0b79b..0d28f05d 100644 --- a/src/current_sense/hardware_specific/generic_mcu.cpp +++ b/src/current_sense/hardware_specific/generic_mcu.cpp @@ -1,5 +1,5 @@ #include "../hardware_api.h" - +__attribute__((weak)) int _pinA, _pinB, _pinC = NOT_SET; // function reading an ADC value and returning the read voltage __attribute__((weak)) float _readADCVoltageInline(const int pinA){ uint32_t raw_adc = analogRead(pinA); @@ -14,8 +14,13 @@ __attribute__((weak)) void _configureADCInline(const int pinA,const int pinB,co } // function reading an ADC value and returning the read voltage -__attribute__((weak)) float _readADCVoltageLowSide(const int pinA){ - return _readADCVoltageInline(pinA); +__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 diff --git a/src/current_sense/hardware_specific/samd21_mcu.cpp b/src/current_sense/hardware_specific/samd21_mcu.cpp index af2c0048..1a8e1195 100644 --- a/src/current_sense/hardware_specific/samd21_mcu.cpp +++ b/src/current_sense/hardware_specific/samd21_mcu.cpp @@ -1,13 +1,14 @@ -#ifdef _SAMD21_ - -#include "samd21_mcu.h" #include "../hardware_api.h" +#if defined(_SAMD21_ASYNC_) + +#include "samd21_mcu.h" -static bool freeRunning = false; +#include "../../common/hardware_specific/samd_mcu.h" +#include 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; +static uint16_t a_raw = 0xFFFF, b_raw = 0xFFFF, c_raw = 0xFFFF; // updated by adcStopWithDMA when configured in freerunning mode +static SAMD21AsyncCurrentSense instance; /** * function reading an ADC value and returning the read voltage * @@ -20,31 +21,30 @@ 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 * * @param pinA - the arduino pin to be read (it has to be ADC pin) */ -float _readADCVoltageLowSide(const int pinA) +bool _readADCVoltagesLowSide(float & a, float & b, float & c) { - instance.readResults(a, b, c); - - if(pinA == _pinA) - return instance.toVolts(a); - if(pinA == _pinB) - return instance.toVolts(b); - if(pinA == _pinC) - return instance.toVolts(c); - - return NAN; + 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; } /** @@ -52,8 +52,7 @@ float _readADCVoltageLowSide(const int pinA) */ void _driverSyncLowSide() { - SIMPLEFOC_SAMD_DEBUG_SERIAL.println(F("TODO! _driverSyncLowSide() is not implemented")); - instance.startADCScan(); + 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 } @@ -68,15 +67,9 @@ 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); /** -<<<<<<< HEAD - * @brief ADC sync wait -======= * @brief ADC sync wait ->>>>>>> dev * @retval void */ static __inline__ void ADCsync() __attribute__((always_inline, unused)); @@ -86,23 +79,19 @@ static void ADCsync() { // ADC DMA sequential free running (6) with Interrupts ///////////////// -<<<<<<< HEAD -SAMDCurrentSenseADCDMA * SAMDCurrentSenseADCDMA::getHardwareAPIInstance() -{ - -======= -SAMDCurrentSenseADCDMA * SAMDCurrentSenseADCDMA::getHardwareAPIInstance() +SAMD21AsyncCurrentSense * SAMD21AsyncCurrentSense::getHardwareAPIInstance() { ->>>>>>> dev return &instance; } -SAMDCurrentSenseADCDMA::SAMDCurrentSenseADCDMA() +SAMD21AsyncCurrentSense::SAMD21AsyncCurrentSense() { } -void SAMDCurrentSenseADCDMA::init(int pinA, int pinB, int pinC, int pinAREF, float voltageAREF, uint32_t adcBits, uint32_t channelDMA) +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) { this->pinA = pinA; this->pinB = pinB; @@ -111,96 +100,123 @@ void SAMDCurrentSenseADCDMA::init(int pinA, int pinB, int pinC, int pinAREF, flo 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] = 42; initPins(); + if(this->EVSYS_ID_GEN_TCC_OVF != -1) + { + 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; + initDMA(); + + initDMAChannel(); + } initADC(); - initDMA(); - startADCScan(); //so we have something to read next time we call readResults() + } -void SAMDCurrentSenseADCDMA::startADCScan(){ - adcToDMATransfer(adcBuffer + oneBeforeFirstAIN, BufferSize); - adcStartWithDMA(); -} +uint32_t SAMD21AsyncCurrentSense::readResults(uint16_t & a, uint16_t & b, uint16_t & c){ -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; 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 } -float SAMDCurrentSenseADCDMA::toVolts(uint16_t counts) { +float SAMD21AsyncCurrentSense::toVolts(uint16_t counts) { return counts * countsToVolts; } -void SAMDCurrentSenseADCDMA::initPins(){ -<<<<<<< HEAD - -======= +void SAMD21AsyncCurrentSense::initPins(){ ->>>>>>> dev - pinMode(pinAREF, INPUT); + refsel = ADC_REFCTRL_REFSEL_INTVCC1_Val; + if(pinAREF != -1) + { + debugPrintf(PSTR("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(PSTR("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; + pinPeripheral(pinA, PIO_ANALOG); + pinPeripheral(pinB, PIO_ANALOG); + ainA = getSamdPinDefinition(pinA)->adc_channel; + ainC = ainB = getSamdPinDefinition(pinB)->adc_channel; firstAIN = min(ainA, ainB); lastAIN = max(ainA, ainB); -<<<<<<< HEAD - if( _isset(pinC) ) -======= if( _isset(pinC) ) ->>>>>>> dev { - uint32_t ainC = g_APinDescription[pinC].ulADCChannelNumber; + 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; + 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 SAMDCurrentSenseADCDMA::initADC(){ +void SAMD21AsyncCurrentSense::initADC(){ -<<<<<<< HEAD - analogRead(pinA); // do some pin init pinPeripheral() - analogRead(pinB); // do some pin init pinPeripheral() - analogRead(pinC); // do some pin init pinPeripheral() - ADC->CTRLA.bit.ENABLE = 0x00; // Disable ADC - ADCsync(); - //ADC->REFCTRL.bit.REFSEL = ADC_REFCTRL_REFSEL_INTVCC0_Val; // 2.2297f V Supply VDDANA -======= - analogRead(pinA); // do some pin init pinPeripheral() - analogRead(pinB); // do some pin init pinPeripheral() - analogRead(pinC); // do some pin init pinPeripheral() + + + ADC->CTRLA.bit.ENABLE = 0x00; // Disable ADC + + /* Turn on the digital interface clock */ + PM->APBCMASK.reg |= PM_APBCMASK_ADC; - ADC->CTRLA.bit.ENABLE = 0x00; // Disable 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 ->>>>>>> dev - 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 + + /* 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; + 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 @@ -214,24 +230,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 @@ -241,110 +240,125 @@ void SAMDCurrentSenseADCDMA::initADC(){ 0x1C DAC DAC output 0x1D-0x1F Reserved */ - ADC->INPUTCTRL.bit.MUXPOS = oneBeforeFirstAIN; - ADCsync(); -<<<<<<< HEAD - ADC->INPUTCTRL.bit.INPUTSCAN = lastAIN; // so the adc will scan from oneBeforeFirstAIN to lastAIN (inclusive) -======= - ADC->INPUTCTRL.bit.INPUTSCAN = lastAIN; // so the adc will scan from oneBeforeFirstAIN to lastAIN (inclusive) ->>>>>>> dev + ADC_INPUTCTRL_Type inputctrl{.reg = 0}; + 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) + 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_DIV4_Val + // 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_DIV8_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->CTRLA.bit.ENABLE = 0x01; + ADCsync(); } +void SAMD21AsyncCurrentSense::initDMA() { + ::initDMAC(); +} -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)); -<<<<<<< HEAD +DmacDescriptor descriptors[20] __attribute__ ((aligned (16))); - DMAC->CHCTRLB.reg = DMAC_CHCTRLB_LVL(0) - | DMAC_CHCTRLB_TRIGSRC(ADC_DMAC_ID_RESRDY) -======= - - DMAC->CHCTRLB.reg = DMAC_CHCTRLB_LVL(0) - | DMAC_CHCTRLB_TRIGSRC(ADC_DMAC_ID_RESRDY) ->>>>>>> dev - | 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; -} +void SAMD21AsyncCurrentSense::initDMAChannel() { + DMAC_CHINTENSET_Type chinset{.reg = 0}; -int iii = 0; + chinset.bit.TCMPL = 0b1; -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!"); - // } + // 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 + 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 */ + 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 adcStartWithDMA(void){ - ADCsync(); +void SAMD21AsyncCurrentSense::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; - ADCsync(); - ADC->SWTRIG.bit.FLUSH = 1; - ADCsync(); -<<<<<<< HEAD - ADC->CTRLA.bit.ENABLE = 0x01; -======= - ADC->CTRLA.bit.ENABLE = 0x01; ->>>>>>> dev - ADCsync(); + timestamp_us = _micros(); } -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; +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; + + uint16_t EVGEN_DMAC = 0; + switch(channelDMA) + { + 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: + 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) + return -1; + return 0; } -#endif \ No newline at end of file +#endif diff --git a/src/current_sense/hardware_specific/samd21_mcu.h b/src/current_sense/hardware_specific/samd21_mcu.h index c0cec74a..e539f0f7 100644 --- a/src/current_sense/hardware_specific/samd21_mcu.h +++ b/src/current_sense/hardware_specific/samd21_mcu.h @@ -1,67 +1,66 @@ -#ifdef _SAMD21_ +#pragma once -#ifndef CURRENT_SENSE_SAMD21_H -#define CURRENT_SENSE_SAMD21_H +#include +#if defined(_SAMD21_) && defined(_SAMD21_ASYNC_) + +#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 -#include - typedef struct { - uint16_t btctrl; - uint16_t btcnt; - uint32_t srcaddr; - uint32_t dstaddr; - uint32_t descaddr; - } dmacdescriptor ; - -class SAMDCurrentSenseADCDMA +class SAMD21AsyncCurrentSense : public DMACInterruptCallback { 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(); - bool readResults(uint16_t & a, uint16_t & b, uint16_t & c); + 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); + + uint32_t readResults(uint16_t & a, uint16_t & b, uint16_t & c); + + void operator()(uint8_t channel, volatile DMAC_CHINTFLAG_Type &, volatile DMAC_CHCTRLA_Type &) override; + float toVolts(uint16_t counts); + + uint16_t adcBuffer[20]; + int EVSYS_ID_GEN_TCC_OVF; + uint64_t timestamp_us; private: - void adcToDMATransfer(void *rxdata, uint32_t hwords); void initPins(); void initADC(); void initDMA(); + void initDMAChannel(); + int initEVSYS(); + uint32_t oneBeforeFirstAIN; // hack to discard first noisy readout uint32_t firstAIN; uint32_t lastAIN; - uint32_t BufferSize = 0; - - uint16_t adcBuffer[20]; - - - uint32_t pinA; - uint32_t pinB; - uint32_t pinC; - uint32_t pinAREF; - uint32_t channelDMA; // DMA channel + uint32_t ainA; + uint32_t ainB; + uint32_t ainC; + uint32_t refsel; + uint32_t bufferSize = 0; + + + int pinA; + int pinB; + int pinC; + int pinAREF; + uint8_t channelDMA; // DMA channel bool freeRunning; float voltageAREF; float maxCountsADC; float countsToVolts; - - dmacdescriptor descriptor_section[12] __attribute__ ((aligned (16))); - dmacdescriptor descriptor __attribute__ ((aligned (16))); }; #endif - - - -#endif \ No newline at end of file diff --git a/src/current_sense/hardware_specific/samd_mcu.cpp b/src/current_sense/hardware_specific/samd_mcu.cpp index 550d7bee..28133582 100644 --- a/src/current_sense/hardware_specific/samd_mcu.cpp +++ b/src/current_sense/hardware_specific/samd_mcu.cpp @@ -1,6 +1,6 @@ #include "../hardware_api.h" -#if defined(_SAMD21_)||defined(_SAMD51_)||defined(_SAME51_) +#if (defined(_SAMD21_)||defined(_SAMD51_)||defined(_SAME51_)) && !defined(_SAMD21_ASYNC_) #define _ADC_VOLTAGE 3.3f #define _ADC_RESOLUTION 1024.0f @@ -8,7 +8,7 @@ // 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); @@ -22,16 +22,24 @@ void _configureADCInline(const int pinA,const int pinB,const int pinC){ } -// function reading an ADC value and returning the read voltage -float _readADCVoltageLowSide(const int pinA){ - return _readADCVoltageInline(pinA); +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 +#endif diff --git a/src/current_sense/hardware_specific/stm32_mcu.cpp b/src/current_sense/hardware_specific/stm32_mcu.cpp index 19833e11..32ae8434 100644 --- a/src/current_sense/hardware_specific/stm32_mcu.cpp +++ b/src/current_sense/hardware_specific/stm32_mcu.cpp @@ -9,7 +9,7 @@ // 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); @@ -23,17 +23,25 @@ void _configureADCInline(const int pinA,const int pinB,const int pinC){ } -// function reading an ADC value and returning the read voltage -float _readADCVoltageLowSide(const int pinA){ - return _readADCVoltageInline(pinA); +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 +#endif diff --git a/src/current_sense/hardware_specific/teensy_mcu.cpp b/src/current_sense/hardware_specific/teensy_mcu.cpp index 606ce618..9b6a4932 100644 --- a/src/current_sense/hardware_specific/teensy_mcu.cpp +++ b/src/current_sense/hardware_specific/teensy_mcu.cpp @@ -8,6 +8,7 @@ // 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){ @@ -21,17 +22,24 @@ void _configureADCInline(const int pinA,const int pinB,const int pinC){ if( _isset(pinC) ) pinMode(pinC, INPUT); } - -// function reading an ADC value and returning the read voltage -float _readADCVoltageLowSide(const int pinA){ - return _readADCVoltageInline(pinA); +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 +} +#endif diff --git a/src/drivers/hardware_specific/samd21_mcu.cpp b/src/drivers/hardware_specific/samd21_mcu.cpp index ab41b745..d987f838 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_ @@ -176,7 +177,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; @@ -196,6 +197,7 @@ void configureTCC(tccConfiguration& tccConfig, long pwm_frequency, bool negate, tccConfigured[tccConfig.tcc.tccn] = true; + if (tccConfig.tcc.tccn>=TCC_INST_NUM) { Tc* tc = (Tc*)GetTC(tccConfig.tcc.chaninfo); // disable @@ -229,7 +231,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_DSTOP; + 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 +262,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); @@ -304,7 +321,6 @@ void configureTCC(tccConfiguration& tccConfig, long pwm_frequency, bool negate, - void writeSAMDDutyCycle(int chaninfo, float dc) { uint8_t tccn = GetTCNumber(chaninfo); uint8_t chan = GetTCChannelNumber(chaninfo); @@ -333,4 +349,6 @@ void writeSAMDDutyCycle(int chaninfo, float dc) { + + #endif 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.cpp b/src/drivers/hardware_specific/samd_mcu.cpp index e1d38a80..edec2866 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 7faf7442..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); + +/* + @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); 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; //!INTFLAG.bit.OVF = 0b1; + // debugPrintln("\n\r*low*"); + if(!pinLow) + { + 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::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(); + } + + } +} + +word SAMDMagneticSensorSPI::makeSPICommand() +{ + 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 + + return command; +} + +word SAMDMagneticSensorSPI::extractResult(word register_value) +{ + 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::init(SAMDAdvancedSPI* _spi) +{ + + 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) +{ + 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(); + delayMicroseconds(10); + + word register_value = spi->transfer16(command); + + return extractResult(register_value); +} + + + +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 = bufferSize; + SPIdescriptors[0].DSTADDR.reg = (uint32_t)&spi->sercomCfg.sercom->SPI.DATA.reg; + SPIdescriptors[0].SRCADDR.reg = ((uint32_t)&transmitBuffer[0]) + bufferSize; + + + ::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 = bufferSize; + SPIdescriptors[1].DSTADDR.reg = ((uint32_t)&receiveBuffer[0]) + bufferSize; + SPIdescriptors[1].SRCADDR.reg = (uint32_t) &spi->sercomCfg.sercom->SPI.DATA.reg; + + ::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 new file mode 100644 index 00000000..e3274258 --- /dev/null +++ b/src/sensors/hardware_specific/SAMDMagneticSensorSPI.h @@ -0,0 +1,43 @@ +#pragma once +#include +#include +#include "../MagneticSensor.h" +#include "../MagneticSensorSPI.h" +#include "../../common/hardware_specific/samd21_AdvancedSPI.h" + +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()(uint8_t channel, volatile DMAC_CHINTFLAG_Type &, volatile DMAC_CHCTRLA_Type &) override; + + private: + void initDMA(); + word makeSPICommand(); + word extractResult(word register_value); + MagneticSensorSPIConfig_s config; + + SAMDAdvancedSPI* spi; + word command; + int8_t tccN, dmaTX, dmaRX; + bool pinLow; + + 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