diff --git a/README.md b/README.md index 2242ed53..a7c4a639 100644 --- a/README.md +++ b/README.md @@ -33,6 +33,8 @@ Therefore this is an attempt to: > - decimal places (settable through commander) > - bugfix - current sense align - added offset exchange when exchanging pins > - bugfix - trapezoid 150 fixed +> - bugfix - MagneticSensorPWM bugfixes +> - current sensing support for STM32L4xx series thanks to @triple6 (discord) > - phase disable in 6pwm mode > - stm32 - software and hardware 6pwm > - atmega328 diff --git a/src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_hal.cpp b/src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_hal.cpp new file mode 100644 index 00000000..bfec2175 --- /dev/null +++ b/src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_hal.cpp @@ -0,0 +1,266 @@ +#include "stm32l4_hal.h" + +#if defined(STM32L4xx) + +#include "../../../../communication/SimpleFOCDebug.h" + +#define SIMPLEFOC_STM32_DEBUG + +ADC_HandleTypeDef hadc; + +int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* driver_params) +{ + ADC_InjectionConfTypeDef sConfigInjected; + + // check if all pins belong to the same ADC + ADC_TypeDef* adc_pin1 = (ADC_TypeDef*)pinmap_peripheral(analogInputToPinName(cs_params->pins[0]), PinMap_ADC); + ADC_TypeDef* adc_pin2 = (ADC_TypeDef*)pinmap_peripheral(analogInputToPinName(cs_params->pins[1]), PinMap_ADC); + ADC_TypeDef* adc_pin3 = _isset(cs_params->pins[2]) ? (ADC_TypeDef*)pinmap_peripheral(analogInputToPinName(cs_params->pins[2]), PinMap_ADC) : nullptr; + if ( (adc_pin1 != adc_pin2) || ( (adc_pin3) && (adc_pin1 != adc_pin3) )){ +#ifdef SIMPLEFOC_STM32_DEBUG + SIMPLEFOC_DEBUG("STM32-CS: ERR: Analog pins dont belong to the same ADC!"); +#endif + return -1; + } + + + /**Configure the global features of the ADC (Clock, Resolution, Data Alignment and number of conversion) + */ + hadc.Instance = (ADC_TypeDef *)pinmap_peripheral(analogInputToPinName(cs_params->pins[0]), PinMap_ADC); + + if(hadc.Instance == ADC1) { +#ifdef __HAL_RCC_ADC1_CLK_ENABLE + __HAL_RCC_ADC1_CLK_ENABLE(); +#endif +#ifdef __HAL_RCC_ADC12_CLK_ENABLE + __HAL_RCC_ADC12_CLK_ENABLE(); +#endif + } +#ifdef ADC2 + else if (hadc.Instance == ADC2) { +#ifdef __HAL_RCC_ADC2_CLK_ENABLE + __HAL_RCC_ADC2_CLK_ENABLE(); +#endif +#ifdef __HAL_RCC_ADC12_CLK_ENABLE + __HAL_RCC_ADC12_CLK_ENABLE(); +#endif + } +#endif +#ifdef ADC3 + else if (hadc.Instance == ADC3) { +#ifdef __HAL_RCC_ADC3_CLK_ENABLE + __HAL_RCC_ADC3_CLK_ENABLE(); +#endif +#ifdef __HAL_RCC_ADC34_CLK_ENABLE + __HAL_RCC_ADC34_CLK_ENABLE(); +#endif +#if defined(ADC345_COMMON) + __HAL_RCC_ADC345_CLK_ENABLE(); +#endif + } +#endif +#ifdef ADC4 + else if (hadc.Instance == ADC4) { +#ifdef __HAL_RCC_ADC4_CLK_ENABLE + __HAL_RCC_ADC4_CLK_ENABLE(); +#endif +#ifdef __HAL_RCC_ADC34_CLK_ENABLE + __HAL_RCC_ADC34_CLK_ENABLE(); +#endif +#if defined(ADC345_COMMON) + __HAL_RCC_ADC345_CLK_ENABLE(); +#endif + } +#endif +#ifdef ADC5 + else if (hadc.Instance == ADC5) { +#if defined(ADC345_COMMON) + __HAL_RCC_ADC345_CLK_ENABLE(); +#endif + } +#endif + else{ +#ifdef SIMPLEFOC_STM32_DEBUG + SIMPLEFOC_DEBUG("STM32-CS: ERR: Pin does not belong to any ADC!"); +#endif + return -1; // error not a valid ADC instance + } + +#ifdef SIMPLEFOC_STM32_DEBUG + SIMPLEFOC_DEBUG("STM32-CS: Using ADC: ", _adcToIndex(&hadc)+1); +#endif + + hadc.Init.ClockPrescaler = ADC_CLOCK_SYNC_PCLK_DIV4; + hadc.Init.Resolution = ADC_RESOLUTION_12B; + hadc.Init.ScanConvMode = ADC_SCAN_ENABLE; + hadc.Init.ContinuousConvMode = DISABLE; + hadc.Init.LowPowerAutoWait = DISABLE; + hadc.Init.DiscontinuousConvMode = DISABLE; + hadc.Init.ExternalTrigConvEdge = ADC_EXTERNALTRIGCONVEDGE_NONE; + hadc.Init.ExternalTrigConv = ADC_SOFTWARE_START; // for now + hadc.Init.DataAlign = ADC_DATAALIGN_RIGHT; + hadc.Init.NbrOfConversion = 2; + hadc.Init.DMAContinuousRequests = DISABLE; + hadc.Init.EOCSelection = ADC_EOC_SINGLE_CONV; + hadc.Init.Overrun = ADC_OVR_DATA_PRESERVED; + if ( HAL_ADC_Init(&hadc) != HAL_OK){ +#ifdef SIMPLEFOC_STM32_DEBUG + SIMPLEFOC_DEBUG("STM32-CS: ERR: cannot init ADC!"); +#endif + return -1; + } + + /**Configures for the selected ADC injected channel its corresponding rank in the sequencer and its sample time + */ + sConfigInjected.InjectedNbrOfConversion = _isset(cs_params->pins[2]) ? 3 : 2; + sConfigInjected.InjectedSamplingTime = ADC_SAMPLETIME_2CYCLES_5; + sConfigInjected.ExternalTrigInjecConvEdge = ADC_EXTERNALTRIGINJECCONV_EDGE_RISING; + sConfigInjected.AutoInjectedConv = DISABLE; + sConfigInjected.InjectedSingleDiff = ADC_SINGLE_ENDED; + sConfigInjected.InjectedDiscontinuousConvMode = DISABLE; + sConfigInjected.InjectedOffsetNumber = ADC_OFFSET_NONE; + sConfigInjected.InjectedOffset = 0; + sConfigInjected.InjecOversamplingMode = DISABLE; + sConfigInjected.QueueInjectedContext = DISABLE; + + // automating TRGO flag finding - hardware specific + uint8_t tim_num = 0; + while(driver_params->timers[tim_num] != NP && tim_num < 6){ + uint32_t trigger_flag = _timerToInjectedTRGO(driver_params->timers[tim_num++]); + if(trigger_flag == _TRGO_NOT_AVAILABLE) continue; // timer does not have valid trgo for injected channels + + // if the code comes here, it has found the timer available + // timer does have trgo flag for injected channels + sConfigInjected.ExternalTrigInjecConv = trigger_flag; + + // this will be the timer with which the ADC will sync + cs_params->timer_handle = driver_params->timers[tim_num-1]; + // done + break; + } + if( cs_params->timer_handle == NP ){ + // not possible to use these timers for low-side current sense +#ifdef SIMPLEFOC_STM32_DEBUG + SIMPLEFOC_DEBUG("STM32-CS: ERR: cannot sync any timer to injected channels!"); +#endif + return -1; + } + + + // first channel + sConfigInjected.InjectedRank = ADC_INJECTED_RANK_1; + sConfigInjected.InjectedChannel = _getADCChannel(analogInputToPinName(cs_params->pins[0])); + if (HAL_ADCEx_InjectedConfigChannel(&hadc, &sConfigInjected) != HAL_OK){ +#ifdef SIMPLEFOC_STM32_DEBUG + SIMPLEFOC_DEBUG("STM32-CS: ERR: cannot init injected channel: ", (int)_getADCChannel(analogInputToPinName(cs_params->pins[0])) ); +#endif + return -1; + } + + // second channel + sConfigInjected.InjectedRank = ADC_INJECTED_RANK_2; + sConfigInjected.InjectedChannel = _getADCChannel(analogInputToPinName(cs_params->pins[1])); + if (HAL_ADCEx_InjectedConfigChannel(&hadc, &sConfigInjected) != HAL_OK){ +#ifdef SIMPLEFOC_STM32_DEBUG + SIMPLEFOC_DEBUG("STM32-CS: ERR: cannot init injected channel: ", (int)_getADCChannel(analogInputToPinName(cs_params->pins[1]))) ; +#endif + return -1; + } + + // third channel - if exists + if(_isset(cs_params->pins[2])){ + sConfigInjected.InjectedRank = ADC_INJECTED_RANK_3; + sConfigInjected.InjectedChannel = _getADCChannel(analogInputToPinName(cs_params->pins[2])); + if (HAL_ADCEx_InjectedConfigChannel(&hadc, &sConfigInjected) != HAL_OK){ +#ifdef SIMPLEFOC_STM32_DEBUG + SIMPLEFOC_DEBUG("STM32-CS: ERR: cannot init injected channel: ", (int)_getADCChannel(analogInputToPinName(cs_params->pins[2]))) ; +#endif + return -1; + } + } + + + + if(hadc.Instance == ADC1) { + // enable interrupt + HAL_NVIC_SetPriority(ADC1_2_IRQn, 0, 0); + HAL_NVIC_EnableIRQ(ADC1_2_IRQn); + } +#ifdef ADC2 + else if (hadc.Instance == ADC2) { + // enable interrupt + HAL_NVIC_SetPriority(ADC1_2_IRQn, 0, 0); + HAL_NVIC_EnableIRQ(ADC1_2_IRQn); + } +#endif +#ifdef ADC3 + else if (hadc.Instance == ADC3) { + // enable interrupt + HAL_NVIC_SetPriority(ADC3_IRQn, 0, 0); + HAL_NVIC_EnableIRQ(ADC3_IRQn); + } +#endif +#ifdef ADC4 + else if (hadc.Instance == ADC4) { + // enable interrupt + HAL_NVIC_SetPriority(ADC4_IRQn, 0, 0); + HAL_NVIC_EnableIRQ(ADC4_IRQn); + } +#endif +#ifdef ADC5 + else if (hadc.Instance == ADC5) { + // enable interrupt + HAL_NVIC_SetPriority(ADC5_IRQn, 0, 0); + HAL_NVIC_EnableIRQ(ADC5_IRQn); + } +#endif + + cs_params->adc_handle = &hadc; + return 0; +} + +void _adc_gpio_init(Stm32CurrentSenseParams* cs_params, const int pinA, const int pinB, const int pinC) +{ + uint8_t cnt = 0; + if(_isset(pinA)){ + pinmap_pinout(analogInputToPinName(pinA), PinMap_ADC); + cs_params->pins[cnt++] = pinA; + } + if(_isset(pinB)){ + pinmap_pinout(analogInputToPinName(pinB), PinMap_ADC); + cs_params->pins[cnt++] = pinB; + } + if(_isset(pinC)){ + pinmap_pinout(analogInputToPinName(pinC), PinMap_ADC); + cs_params->pins[cnt] = pinC; + } +} + +extern "C" { + void ADC1_2_IRQHandler(void) + { + HAL_ADC_IRQHandler(&hadc); + } +#ifdef ADC3 + void ADC3_IRQHandler(void) + { + HAL_ADC_IRQHandler(&hadc); + } +#endif + +#ifdef ADC4 + void ADC4_IRQHandler(void) + { + HAL_ADC_IRQHandler(&hadc); + } +#endif + +#ifdef ADC5 + void ADC5_IRQHandler(void) + { + HAL_ADC_IRQHandler(&hadc); + } +#endif +} + +#endif \ No newline at end of file diff --git a/src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_hal.h b/src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_hal.h new file mode 100644 index 00000000..0317b74b --- /dev/null +++ b/src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_hal.h @@ -0,0 +1,19 @@ +#ifndef STM32L4_LOWSIDE_HAL +#define STM32L4_LOWSIDE_HAL + +#include "Arduino.h" + +#if defined(STM32L4xx) + +#include "stm32l4xx_hal.h" +#include "../../../../common/foc_utils.h" +#include "../../../../drivers/hardware_specific/stm32/stm32_mcu.h" +#include "../stm32_mcu.h" +#include "stm32l4_utils.h" + +int _adc_init(Stm32CurrentSenseParams* cs_params, const STM32DriverParams* driver_params); +void _adc_gpio_init(Stm32CurrentSenseParams* cs_params, const int pinA, const int pinB, const int pinC); + +#endif + +#endif \ No newline at end of file diff --git a/src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_mcu.cpp b/src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_mcu.cpp new file mode 100644 index 00000000..edac6414 --- /dev/null +++ b/src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_mcu.cpp @@ -0,0 +1,98 @@ +#include "../../../hardware_api.h" + +#if defined(STM32L4xx) + +#include "../../../../common/foc_utils.h" +#include "../../../../drivers/hardware_api.h" +#include "../../../../drivers/hardware_specific/stm32/stm32_mcu.h" +#include "../../../hardware_api.h" +#include "../stm32_mcu.h" +#include "stm32l4_hal.h" +#include "stm32l4_utils.h" +#include "Arduino.h" + + +#define _ADC_VOLTAGE_L4 3.3f +#define _ADC_RESOLUTION_L4 4096.0f + + +// array of values of 4 injected channels per adc instance (5) +uint32_t adc_val[5][4]={0}; +// does adc interrupt need a downsample - per adc (5) +bool needs_downsample[5] = {1}; +// downsampling variable - per adc (5) +uint8_t tim_downsample[5] = {0}; + + +void* _configureADCLowSide(const void* driver_params, const int pinA, const int pinB, const int pinC){ + + Stm32CurrentSenseParams* cs_params= new Stm32CurrentSenseParams { + .pins={(int)NOT_SET, (int)NOT_SET, (int)NOT_SET}, + .adc_voltage_conv = (_ADC_VOLTAGE_L4) / (_ADC_RESOLUTION_L4) + }; + _adc_gpio_init(cs_params, pinA,pinB,pinC); + if(_adc_init(cs_params, (STM32DriverParams*)driver_params) != 0) return SIMPLEFOC_CURRENT_SENSE_INIT_FAILED; + return cs_params; +} + + +void _driverSyncLowSide(void* _driver_params, void* _cs_params){ + STM32DriverParams* driver_params = (STM32DriverParams*)_driver_params; + Stm32CurrentSenseParams* cs_params = (Stm32CurrentSenseParams*)_cs_params; + + // if compatible timer has not been found + if (cs_params->timer_handle == NULL) return; + + // stop all the timers for the driver + _stopTimers(driver_params->timers, 6); + + // if timer has repetition counter - it will downsample using it + // and it does not need the software downsample + if( IS_TIM_REPETITION_COUNTER_INSTANCE(cs_params->timer_handle->getHandle()->Instance) ){ + // adjust the initial timer state such that the trigger + // - for DMA transfer aligns with the pwm peaks instead of throughs. + // - for interrupt based ADC transfer + // - only necessary for the timers that have repetition counters + cs_params->timer_handle->getHandle()->Instance->CR1 |= TIM_CR1_DIR; + cs_params->timer_handle->getHandle()->Instance->CNT = cs_params->timer_handle->getHandle()->Instance->ARR; + // remember that this timer has repetition counter - no need to downasmple + needs_downsample[_adcToIndex(cs_params->adc_handle)] = 0; + } + + // set the trigger output event + LL_TIM_SetTriggerOutput(cs_params->timer_handle->getHandle()->Instance, LL_TIM_TRGO_UPDATE); + // start the adc + HAL_ADCEx_InjectedStart_IT(cs_params->adc_handle); + // restart all the timers of the driver + _startTimers(driver_params->timers, 6); +} + + +// function reading an ADC value and returning the read voltage +float _readADCVoltageLowSide(const int pin, const void* cs_params){ + for(int i=0; i < 3; i++){ + if( pin == ((Stm32CurrentSenseParams*)cs_params)->pins[i]) // found in the buffer + return adc_val[_adcToIndex(((Stm32CurrentSenseParams*)cs_params)->adc_handle)][i] * ((Stm32CurrentSenseParams*)cs_params)->adc_voltage_conv; + } + return 0; +} + + +extern "C" { + void HAL_ADCEx_InjectedConvCpltCallback(ADC_HandleTypeDef *AdcHandle){ + // calculate the instance + int adc_index = _adcToIndex(AdcHandle); + + // if the timer han't repetition counter - downsample two times + if( needs_downsample[adc_index] && tim_downsample[adc_index]++ > 0) { + tim_downsample[adc_index] = 0; + return; + } + + adc_val[adc_index][0]=HAL_ADCEx_InjectedGetValue(AdcHandle, ADC_INJECTED_RANK_1); + adc_val[adc_index][1]=HAL_ADCEx_InjectedGetValue(AdcHandle, ADC_INJECTED_RANK_2); + adc_val[adc_index][2]=HAL_ADCEx_InjectedGetValue(AdcHandle, ADC_INJECTED_RANK_3); + } +} + +#endif \ No newline at end of file diff --git a/src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_utils.cpp b/src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_utils.cpp new file mode 100644 index 00000000..7c254ce9 --- /dev/null +++ b/src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_utils.cpp @@ -0,0 +1,221 @@ +#include "stm32l4_utils.h" + +#if defined(STM32L4xx) + +/* Exported Functions */ +/** + * @brief Return ADC HAL channel linked to a PinName + * @param pin: PinName + * @retval Valid HAL channel + */ +uint32_t _getADCChannel(PinName pin) +{ + uint32_t function = pinmap_function(pin, PinMap_ADC); + uint32_t channel = 0; + switch (STM_PIN_CHANNEL(function)) { +#ifdef ADC_CHANNEL_0 + case 0: + channel = ADC_CHANNEL_0; + break; +#endif + case 1: + channel = ADC_CHANNEL_1; + break; + case 2: + channel = ADC_CHANNEL_2; + break; + case 3: + channel = ADC_CHANNEL_3; + break; + case 4: + channel = ADC_CHANNEL_4; + break; + case 5: + channel = ADC_CHANNEL_5; + break; + case 6: + channel = ADC_CHANNEL_6; + break; + case 7: + channel = ADC_CHANNEL_7; + break; + case 8: + channel = ADC_CHANNEL_8; + break; + case 9: + channel = ADC_CHANNEL_9; + break; + case 10: + channel = ADC_CHANNEL_10; + break; + case 11: + channel = ADC_CHANNEL_11; + break; + case 12: + channel = ADC_CHANNEL_12; + break; + case 13: + channel = ADC_CHANNEL_13; + break; + case 14: + channel = ADC_CHANNEL_14; + break; + case 15: + channel = ADC_CHANNEL_15; + break; +#ifdef ADC_CHANNEL_16 + case 16: + channel = ADC_CHANNEL_16; + break; +#endif + case 17: + channel = ADC_CHANNEL_17; + break; +#ifdef ADC_CHANNEL_18 + case 18: + channel = ADC_CHANNEL_18; + break; +#endif +#ifdef ADC_CHANNEL_19 + case 19: + channel = ADC_CHANNEL_19; + break; +#endif +#ifdef ADC_CHANNEL_20 + case 20: + channel = ADC_CHANNEL_20; + break; + case 21: + channel = ADC_CHANNEL_21; + break; + case 22: + channel = ADC_CHANNEL_22; + break; + case 23: + channel = ADC_CHANNEL_23; + break; +#ifdef ADC_CHANNEL_24 + case 24: + channel = ADC_CHANNEL_24; + break; + case 25: + channel = ADC_CHANNEL_25; + break; + case 26: + channel = ADC_CHANNEL_26; + break; +#ifdef ADC_CHANNEL_27 + case 27: + channel = ADC_CHANNEL_27; + break; + case 28: + channel = ADC_CHANNEL_28; + break; + case 29: + channel = ADC_CHANNEL_29; + break; + case 30: + channel = ADC_CHANNEL_30; + break; + case 31: + channel = ADC_CHANNEL_31; + break; +#endif +#endif +#endif + default: + _Error_Handler("ADC: Unknown adc channel", (int)(STM_PIN_CHANNEL(function))); + break; + } + return channel; +} + + +// timer to injected TRGO +// https://github.com/stm32duino/Arduino_Core_STM32/blob/e156c32db24d69cb4818208ccc28894e2f427cfa/system/Drivers/STM32L4xx_HAL_Driver/Inc/stm32l4xx_hal_adc_ex.h#L210 +uint32_t _timerToInjectedTRGO(HardwareTimer* timer){ + if(timer->getHandle()->Instance == TIM1) + return ADC_EXTERNALTRIGINJEC_T1_TRGO; +#ifdef TIM2 // if defined timer 2 + else if(timer->getHandle()->Instance == TIM2) + return ADC_EXTERNALTRIGINJEC_T2_TRGO; +#endif +#ifdef TIM3 // if defined timer 3 + else if(timer->getHandle()->Instance == TIM3) + return ADC_EXTERNALTRIGINJEC_T3_TRGO; +#endif +#ifdef TIM4 // if defined timer 4 + else if(timer->getHandle()->Instance == TIM4) + return ADC_EXTERNALTRIGINJEC_T4_TRGO; +#endif +#ifdef TIM6 // if defined timer 6 + else if(timer->getHandle()->Instance == TIM6) + return ADC_EXTERNALTRIGINJEC_T6_TRGO; +#endif +#ifdef TIM8 // if defined timer 8 + else if(timer->getHandle()->Instance == TIM8) + return ADC_EXTERNALTRIGINJEC_T8_TRGO; +#endif +#ifdef TIM15 // if defined timer 15 + else if(timer->getHandle()->Instance == TIM15) + return ADC_EXTERNALTRIGINJEC_T15_TRGO; +#endif + else + return _TRGO_NOT_AVAILABLE; +} + +// timer to regular TRGO +// https://github.com/stm32duino/Arduino_Core_STM32/blob/6588dee03382e73ed42c4a5e473900ab3b79d6e4/system/Drivers/STM32G4xx_HAL_Driver/Inc/stm32g4xx_hal_adc.h#L519 +uint32_t _timerToRegularTRGO(HardwareTimer* timer){ + if(timer->getHandle()->Instance == TIM1) + return ADC_EXTERNALTRIG_T1_TRGO; +#ifdef TIM2 // if defined timer 2 + else if(timer->getHandle()->Instance == TIM2) + return ADC_EXTERNALTRIG_T2_TRGO; +#endif +#ifdef TIM3 // if defined timer 3 + else if(timer->getHandle()->Instance == TIM3) + return ADC_EXTERNALTRIG_T3_TRGO; +#endif +#ifdef TIM4 // if defined timer 4 + else if(timer->getHandle()->Instance == TIM4) + return ADC_EXTERNALTRIG_T4_TRGO; +#endif +#ifdef TIM6 // if defined timer 6 + else if(timer->getHandle()->Instance == TIM6) + return ADC_EXTERNALTRIG_T6_TRGO; +#endif +#ifdef TIM8 // if defined timer 8 + else if(timer->getHandle()->Instance == TIM8) + return ADC_EXTERNALTRIG_T7_TRGO; +#endif +#ifdef TIM15 // if defined timer 15 + else if(timer->getHandle()->Instance == TIM15) + return ADC_EXTERNALTRIG_T15_TRGO; +#endif + else + return _TRGO_NOT_AVAILABLE; +} + + +int _adcToIndex(ADC_TypeDef *AdcHandle){ + if(AdcHandle == ADC1) return 0; +#ifdef ADC2 // if ADC2 exists + else if(AdcHandle == ADC2) return 1; +#endif +#ifdef ADC3 // if ADC3 exists + else if(AdcHandle == ADC3) return 2; +#endif +#ifdef ADC4 // if ADC4 exists + else if(AdcHandle == ADC4) return 3; +#endif +#ifdef ADC5 // if ADC5 exists + else if(AdcHandle == ADC5) return 4; +#endif + return 0; +} +int _adcToIndex(ADC_HandleTypeDef *AdcHandle){ + return _adcToIndex(AdcHandle->Instance); +} + +#endif \ No newline at end of file diff --git a/src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_utils.h b/src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_utils.h new file mode 100644 index 00000000..ceef9be7 --- /dev/null +++ b/src/current_sense/hardware_specific/stm32/stm32l4/stm32l4_utils.h @@ -0,0 +1,34 @@ + +#ifndef STM32L4_UTILS_HAL +#define STM32L4_UTILS_HAL + +#include "Arduino.h" + +#if defined(STM32L4xx) + +#define _TRGO_NOT_AVAILABLE 12345 + + +/* Exported Functions */ +/** + * @brief Return ADC HAL channel linked to a PinName + * @param pin: PinName + * @retval Valid HAL channel + */ +uint32_t _getADCChannel(PinName pin); + +// timer to injected TRGO +// https://github.com/stm32duino/Arduino_Core_STM32/blob/6588dee03382e73ed42c4a5e473900ab3b79d6e4/system/Drivers/STM32G4xx_HAL_Driver/Inc/stm32g4xx_hal_adc_ex.h#L217 +uint32_t _timerToInjectedTRGO(HardwareTimer* timer); + +// timer to regular TRGO +// https://github.com/stm32duino/Arduino_Core_STM32/blob/6588dee03382e73ed42c4a5e473900ab3b79d6e4/system/Drivers/STM32G4xx_HAL_Driver/Inc/stm32g4xx_hal_adc.h#L519 +uint32_t _timerToRegularTRGO(HardwareTimer* timer); + +// function returning index of the ADC instance +int _adcToIndex(ADC_HandleTypeDef *AdcHandle); +int _adcToIndex(ADC_TypeDef *AdcHandle); + +#endif + +#endif \ No newline at end of file diff --git a/src/sensors/MagneticSensorPWM.cpp b/src/sensors/MagneticSensorPWM.cpp index a01d6174..9d3fdee6 100644 --- a/src/sensors/MagneticSensorPWM.cpp +++ b/src/sensors/MagneticSensorPWM.cpp @@ -35,6 +35,7 @@ void MagneticSensorPWM::init(){ float MagneticSensorPWM::getSensorAngle(){ // raw data from sensor raw_count = getRawCount(); + if (raw_count > max_raw_count) raw_count = max_raw_count; return( (float) (raw_count - min_raw_count) / (float)cpr) * _2PI; }