diff --git a/Firmware/Board/v3/Inc/gpio.h b/Firmware/Board/v3/Inc/gpio.h index 6ec71035..7c7a4e63 100644 --- a/Firmware/Board/v3/Inc/gpio.h +++ b/Firmware/Board/v3/Inc/gpio.h @@ -77,6 +77,14 @@ bool GPIO_subscribe(GPIO_TypeDef* GPIO_port, uint16_t GPIO_pin, void GPIO_unsubscribe(GPIO_TypeDef* GPIO_port, uint16_t GPIO_pin); void GPIO_set_to_analog(GPIO_TypeDef* GPIO_port, uint16_t GPIO_pin); +uint16_t get_gpio_pin_by_pin(uint16_t GPIO_pin); +GPIO_TypeDef* get_gpio_port_by_pin(uint16_t GPIO_pin); + +#if HW_VERSION_MAJOR == 3 && HW_VERSION_MINOR <= 4 +#define GPIO_COUNT 5 +#else +#define GPIO_COUNT 8 +#endif /* USER CODE END Prototypes */ diff --git a/Firmware/Board/v3/Inc/stm32f4xx_it.h b/Firmware/Board/v3/Inc/stm32f4xx_it.h index 1970c2a8..d5c53ffa 100644 --- a/Firmware/Board/v3/Inc/stm32f4xx_it.h +++ b/Firmware/Board/v3/Inc/stm32f4xx_it.h @@ -58,6 +58,7 @@ void DMA1_Stream2_IRQHandler(void); void DMA1_Stream4_IRQHandler(void); void ADC_IRQHandler(void); void TIM8_TRG_COM_TIM14_IRQHandler(void); +void TIM5_IRQHandler(void); void UART4_IRQHandler(void); void OTG_FS_IRQHandler(void); diff --git a/Firmware/Board/v3/Inc/tim.h b/Firmware/Board/v3/Inc/tim.h index 4ef67076..7006b95a 100644 --- a/Firmware/Board/v3/Inc/tim.h +++ b/Firmware/Board/v3/Inc/tim.h @@ -65,6 +65,7 @@ extern TIM_HandleTypeDef htim1; extern TIM_HandleTypeDef htim2; extern TIM_HandleTypeDef htim3; extern TIM_HandleTypeDef htim4; +extern TIM_HandleTypeDef htim5; extern TIM_HandleTypeDef htim8; /* USER CODE BEGIN Private defines */ @@ -77,6 +78,7 @@ void MX_TIM1_Init(void); void MX_TIM2_Init(void); void MX_TIM3_Init(void); void MX_TIM4_Init(void); +void MX_TIM5_Init(void); void MX_TIM8_Init(void); void HAL_TIM_MspPostInit(TIM_HandleTypeDef *htim); diff --git a/Firmware/Board/v3/Odrive.ioc b/Firmware/Board/v3/Odrive.ioc index 84db04ed..91bd4542 100644 --- a/Firmware/Board/v3/Odrive.ioc +++ b/Firmware/Board/v3/Odrive.ioc @@ -129,10 +129,11 @@ Mcu.IP10=TIM1 Mcu.IP11=TIM2 Mcu.IP12=TIM3 Mcu.IP13=TIM4 -Mcu.IP14=TIM8 -Mcu.IP15=UART4 -Mcu.IP16=USB_DEVICE -Mcu.IP17=USB_OTG_FS +Mcu.IP14=TIM5 +Mcu.IP15=TIM8 +Mcu.IP16=UART4 +Mcu.IP17=USB_DEVICE +Mcu.IP18=USB_OTG_FS Mcu.IP2=ADC3 Mcu.IP3=CAN1 Mcu.IP4=DMA @@ -141,7 +142,7 @@ Mcu.IP6=NVIC Mcu.IP7=RCC Mcu.IP8=SPI3 Mcu.IP9=SYS -Mcu.IPNb=18 +Mcu.IPNb=19 Mcu.Name=STM32F405RGTx Mcu.Package=LQFP64 Mcu.Pin0=PC13-ANTI_TAMP @@ -221,6 +222,7 @@ NVIC.PriorityGroup=NVIC_PRIORITYGROUP_4 NVIC.SVCall_IRQn=true\:0\:0\:false\:false\:false\:false\:true NVIC.SysTick_IRQn=true\:15\:0\:false\:false\:true\:true\:true NVIC.TIM1_UP_TIM10_IRQn=true\:0\:0\:false\:false\:false\:false\:true +NVIC.TIM5_IRQn=true\:5\:0\:false\:false\:true\:true\:true NVIC.TIM8_TRG_COM_TIM14_IRQn=true\:0\:0\:false\:false\:true\:false\:false NVIC.TIM8_UP_TIM13_IRQn=true\:0\:0\:false\:false\:false\:false\:true NVIC.TimeBase=TIM8_TRG_COM_TIM14_IRQn @@ -258,12 +260,11 @@ PA15.Signal=GPIO_Input PA2.GPIOParameters=GPIO_Label PA2.GPIO_Label=GPIO_3 PA2.Locked=true -PA2.Signal=GPIO_Input -PA3.GPIOParameters=GPIO_PuPd,GPIO_Label +PA2.Signal=S_TIM5_CH3 +PA3.GPIOParameters=GPIO_Label PA3.GPIO_Label=GPIO_4 -PA3.GPIO_PuPd=GPIO_NOPULL PA3.Locked=true -PA3.Signal=GPIO_Input +PA3.Signal=S_TIM5_CH4 PA4.GPIOParameters=GPIO_Label PA4.GPIO_Label=M1_TEMP PA4.Locked=true @@ -449,7 +450,7 @@ ProjectManager.StackSize=0x800 ProjectManager.TargetToolchain=Makefile ProjectManager.ToolChainLocation= ProjectManager.UnderRoot=false -ProjectManager.functionlistsort=1-MX_GPIO_Init-GPIO-false-HAL-true,2-MX_DMA_Init-DMA-false-HAL-true,3-MX_ADC1_Init-ADC1-false-HAL-true,4-MX_ADC2_Init-ADC2-false-HAL-true,5-MX_TIM1_Init-TIM1-false-HAL-true,6-MX_TIM8_Init-TIM8-false-HAL-true,7-MX_TIM3_Init-TIM3-false-HAL-true,8-MX_TIM4_Init-TIM4-false-HAL-true,9-MX_SPI3_Init-SPI3-false-HAL-true,10-MX_ADC3_Init-ADC3-false-HAL-true,11-SystemClock_Config-RCC-false-HAL-true,12-MX_TIM2_Init-TIM2-false-HAL-true,13-MX_USB_DEVICE_Init-USB_DEVICE-false-HAL-true,14-MX_UART4_Init-UART4-false-HAL-true,15-MX_CAN1_Init-CAN1-false-HAL-true +ProjectManager.functionlistsort=1-MX_GPIO_Init-GPIO-false-HAL-true,2-MX_DMA_Init-DMA-false-HAL-true,3-MX_ADC1_Init-ADC1-false-HAL-true,4-MX_ADC2_Init-ADC2-false-HAL-true,5-MX_CAN1_Init-CAN1-false-HAL-true,6-MX_TIM1_Init-TIM1-false-HAL-true,7-MX_TIM8_Init-TIM8-false-HAL-true,8-MX_TIM3_Init-TIM3-false-HAL-true,9-MX_TIM4_Init-TIM4-false-HAL-true,10-MX_SPI3_Init-SPI3-false-HAL-true,11-MX_ADC3_Init-ADC3-false-HAL-true,12-SystemClock_Config-RCC-false-HAL-true,13-MX_TIM2_Init-TIM2-false-HAL-true,14-MX_USB_DEVICE_Init-USB_DEVICE-false-HAL-true,15-MX_UART4_Init-UART4-false-HAL-true,16-MX_TIM5_Init-TIM5-false-HAL-true RCC.48MHZClocksFreq_Value=48000000 RCC.AHBFreq_Value=168000000 RCC.APB1CLKDivider=RCC_HCLK_DIV4 @@ -530,6 +531,10 @@ SH.S_TIM4_CH1.0=TIM4_CH1,Encoder_Interface SH.S_TIM4_CH1.ConfNb=1 SH.S_TIM4_CH2.0=TIM4_CH2,Encoder_Interface SH.S_TIM4_CH2.ConfNb=1 +SH.S_TIM5_CH3.0=TIM5_CH3,Input_Capture3_from_TI3 +SH.S_TIM5_CH3.ConfNb=1 +SH.S_TIM5_CH4.0=TIM5_CH4,Input_Capture4_from_TI4 +SH.S_TIM5_CH4.ConfNb=1 SH.S_TIM8_CH1.0=TIM8_CH1,PWM Generation1 CH1 CH1N SH.S_TIM8_CH1.ConfNb=1 SH.S_TIM8_CH2.0=TIM8_CH2,PWM Generation2 CH2 CH2N @@ -597,6 +602,14 @@ TIM4.IC2Filter=4 TIM4.IC2Polarity=TIM_ICPOLARITY_RISING TIM4.IPParameters=EncoderMode,IC1Polarity,IC2Polarity,IC1Filter,IC2Filter,Period TIM4.Period=0xffff +TIM5.Channel-Input_Capture3_from_TI3=TIM_CHANNEL_3 +TIM5.Channel-Input_Capture4_from_TI4=TIM_CHANNEL_4 +TIM5.ICFilter_CH3=15 +TIM5.ICFilter_CH4=15 +TIM5.ICPolarity_CH3=TIM_INPUTCHANNELPOLARITY_BOTHEDGE +TIM5.ICPolarity_CH4=TIM_INPUTCHANNELPOLARITY_BOTHEDGE +TIM5.IPParameters=Channel-Input_Capture3_from_TI3,Channel-Input_Capture4_from_TI4,ICFilter_CH3,ICFilter_CH4,ICPolarity_CH3,ICPolarity_CH4,Period +TIM5.Period=0xFFFFFFFF TIM8.Channel-Output\ Compare4\ No\ Output=TIM_CHANNEL_4 TIM8.Channel-PWM\ Generation1\ CH1\ CH1N=TIM_CHANNEL_1 TIM8.Channel-PWM\ Generation2\ CH2\ CH2N=TIM_CHANNEL_2 diff --git a/Firmware/Board/v3/Src/gpio.c b/Firmware/Board/v3/Src/gpio.c index 130708bf..4cac8c69 100644 --- a/Firmware/Board/v3/Src/gpio.c +++ b/Firmware/Board/v3/Src/gpio.c @@ -106,12 +106,6 @@ void MX_GPIO_Init(void) GPIO_InitStruct.Pull = GPIO_NOPULL; HAL_GPIO_Init(GPIOC, &GPIO_InitStruct); - /*Configure GPIO pins : PAPin PAPin PAPin */ - GPIO_InitStruct.Pin = GPIO_3_Pin|GPIO_4_Pin|GPIO_7_Pin; - GPIO_InitStruct.Mode = GPIO_MODE_INPUT; - GPIO_InitStruct.Pull = GPIO_NOPULL; - HAL_GPIO_Init(GPIOA, &GPIO_InitStruct); - /*Configure GPIO pins : PBPin PBPin */ GPIO_InitStruct.Pin = GPIO_6_Pin|GPIO_8_Pin; GPIO_InitStruct.Mode = GPIO_MODE_INPUT; @@ -125,6 +119,12 @@ void MX_GPIO_Init(void) GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW; HAL_GPIO_Init(EN_GATE_GPIO_Port, &GPIO_InitStruct); + /*Configure GPIO pin : PtPin */ + GPIO_InitStruct.Pin = GPIO_7_Pin; + GPIO_InitStruct.Mode = GPIO_MODE_INPUT; + GPIO_InitStruct.Pull = GPIO_NOPULL; + HAL_GPIO_Init(GPIO_7_GPIO_Port, &GPIO_InitStruct); + /*Configure GPIO pin : PtPin */ GPIO_InitStruct.Pin = nFAULT_Pin; GPIO_InitStruct.Mode = GPIO_MODE_INPUT; @@ -277,6 +277,28 @@ void HAL_GPIO_EXTI_Callback(uint16_t GPIO_pin) { } } +GPIO_TypeDef* get_gpio_port_by_pin(uint16_t GPIO_pin){ + switch(GPIO_pin){ + case 1: return GPIO_1_GPIO_Port; break; + case 2: return GPIO_2_GPIO_Port; break; + case 3: return GPIO_3_GPIO_Port; break; + case 4: return GPIO_4_GPIO_Port; break; + case 5: return GPIO_5_GPIO_Port; break; + default: return GPIO_1_GPIO_Port; + } +} + +uint16_t get_gpio_pin_by_pin(uint16_t GPIO_pin){ + switch(GPIO_pin){ + case 1: return GPIO_1_Pin; break; + case 2: return GPIO_2_Pin; break; + case 3: return GPIO_3_Pin; break; + case 4: return GPIO_4_Pin; break; + case 5: return GPIO_5_Pin; break; + default: return GPIO_1_Pin; + } +} + /* USER CODE END 2 */ /** diff --git a/Firmware/Board/v3/Src/main.c b/Firmware/Board/v3/Src/main.c index efaeef10..b71d6416 100644 --- a/Firmware/Board/v3/Src/main.c +++ b/Firmware/Board/v3/Src/main.c @@ -179,6 +179,7 @@ int main(void) MX_DMA_Init(); MX_ADC1_Init(); MX_ADC2_Init(); + MX_CAN1_Init(); MX_TIM1_Init(); MX_TIM8_Init(); MX_TIM3_Init(); @@ -187,6 +188,7 @@ int main(void) MX_ADC3_Init(); MX_TIM2_Init(); MX_UART4_Init(); + MX_TIM5_Init(); /* USER CODE BEGIN 2 */ //Required to use OC4 for ADC triggering. @@ -234,8 +236,9 @@ void SystemClock_Config(void) /**Initializes the CPU, AHB and APB busses clocks */ - RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSE; + RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_LSI|RCC_OSCILLATORTYPE_HSE; RCC_OscInitStruct.HSEState = RCC_HSE_ON; + RCC_OscInitStruct.LSIState = RCC_LSI_ON; RCC_OscInitStruct.PLL.PLLState = RCC_PLL_ON; RCC_OscInitStruct.PLL.PLLSource = RCC_PLLSOURCE_HSE; RCC_OscInitStruct.PLL.PLLM = 4; diff --git a/Firmware/Board/v3/Src/stm32f4xx_it.c b/Firmware/Board/v3/Src/stm32f4xx_it.c index 1ef20c8c..3b614f60 100644 --- a/Firmware/Board/v3/Src/stm32f4xx_it.c +++ b/Firmware/Board/v3/Src/stm32f4xx_it.c @@ -43,10 +43,14 @@ typedef void (*ADC_handler_t)(ADC_HandleTypeDef* hadc, bool injected); void ADC_IRQ_Dispatch(ADC_HandleTypeDef* hadc, ADC_handler_t callback); +typedef void (*TIM_capture_callback_t)(int channel, uint32_t timestamp); +void decode_tim_capture(TIM_HandleTypeDef *htim, TIM_capture_callback_t callback); + // TODO: move somewhere else void pwm_trig_adc_cb(ADC_HandleTypeDef* hadc, bool injected); void vbus_sense_adc_cb(ADC_HandleTypeDef* hadc, bool injected); void tim_update_cb(TIM_HandleTypeDef* htim); +void pwm_in_cb(int channel, uint32_t timestamp); extern TIM_HandleTypeDef htim1; extern I2C_HandleTypeDef hi2c1; @@ -58,6 +62,7 @@ extern PCD_HandleTypeDef hpcd_USB_OTG_FS; extern ADC_HandleTypeDef hadc1; extern ADC_HandleTypeDef hadc2; extern ADC_HandleTypeDef hadc3; +extern TIM_HandleTypeDef htim5; extern TIM_HandleTypeDef htim8; extern DMA_HandleTypeDef hdma_uart4_rx; extern DMA_HandleTypeDef hdma_uart4_tx; @@ -257,6 +262,23 @@ void TIM8_TRG_COM_TIM14_IRQHandler(void) /* USER CODE END TIM8_TRG_COM_TIM14_IRQn 1 */ } +/** +* @brief This function handles TIM5 global interrupt. +*/ +void TIM5_IRQHandler(void) +{ + /* USER CODE BEGIN TIM5_IRQn 0 */ + + // We know we only use capture mode here, so bypass HAL + decode_tim_capture(&htim5, &pwm_in_cb); + + /* USER CODE END TIM5_IRQn 0 */ + HAL_TIM_IRQHandler(&htim5); + /* USER CODE BEGIN TIM5_IRQn 1 */ + + /* USER CODE END TIM5_IRQn 1 */ +} + /** * @brief This function handles UART4 global interrupt. */ @@ -312,6 +334,25 @@ void ADC_IRQ_Dispatch(ADC_HandleTypeDef* hadc, ADC_handler_t callback) { } } +void decode_tim_capture(TIM_HandleTypeDef *htim, TIM_capture_callback_t callback) { + if(__HAL_TIM_GET_FLAG(htim, TIM_FLAG_CC1)) { + __HAL_TIM_CLEAR_IT(htim, TIM_IT_CC1); + callback(1, htim->Instance->CCR1); + } + if(__HAL_TIM_GET_FLAG(htim, TIM_FLAG_CC2)) { + __HAL_TIM_CLEAR_IT(htim, TIM_IT_CC2); + callback(2, htim->Instance->CCR2); + } + if(__HAL_TIM_GET_FLAG(htim, TIM_FLAG_CC3)) { + __HAL_TIM_CLEAR_IT(htim, TIM_IT_CC3); + callback(3, htim->Instance->CCR3); + } + if(__HAL_TIM_GET_FLAG(htim, TIM_FLAG_CC4)) { + __HAL_TIM_CLEAR_IT(htim, TIM_IT_CC4); + callback(4, htim->Instance->CCR4); + } +} + /** * @brief This function handles TIM1 update interrupt and TIM10 global interrupt. */ diff --git a/Firmware/Board/v3/Src/tim.c b/Firmware/Board/v3/Src/tim.c index f1cb0c07..7b159ea5 100644 --- a/Firmware/Board/v3/Src/tim.c +++ b/Firmware/Board/v3/Src/tim.c @@ -78,6 +78,7 @@ TIM_HandleTypeDef htim1; TIM_HandleTypeDef htim2; TIM_HandleTypeDef htim3; TIM_HandleTypeDef htim4; +TIM_HandleTypeDef htim5; TIM_HandleTypeDef htim8; /* TIM1 init function */ @@ -272,6 +273,44 @@ void MX_TIM4_Init(void) _Error_Handler(__FILE__, __LINE__); } +} +/* TIM5 init function */ +void MX_TIM5_Init(void) +{ + TIM_MasterConfigTypeDef sMasterConfig; + TIM_IC_InitTypeDef sConfigIC; + + htim5.Instance = TIM5; + htim5.Init.Prescaler = 0; + htim5.Init.CounterMode = TIM_COUNTERMODE_UP; + htim5.Init.Period = 0xFFFFFFFF; + htim5.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1; + if (HAL_TIM_IC_Init(&htim5) != HAL_OK) + { + _Error_Handler(__FILE__, __LINE__); + } + + sMasterConfig.MasterOutputTrigger = TIM_TRGO_RESET; + sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE; + if (HAL_TIMEx_MasterConfigSynchronization(&htim5, &sMasterConfig) != HAL_OK) + { + _Error_Handler(__FILE__, __LINE__); + } + + sConfigIC.ICPolarity = TIM_INPUTCHANNELPOLARITY_BOTHEDGE; + sConfigIC.ICSelection = TIM_ICSELECTION_DIRECTTI; + sConfigIC.ICPrescaler = TIM_ICPSC_DIV1; + sConfigIC.ICFilter = 15; + if (HAL_TIM_IC_ConfigChannel(&htim5, &sConfigIC, TIM_CHANNEL_3) != HAL_OK) + { + _Error_Handler(__FILE__, __LINE__); + } + + if (HAL_TIM_IC_ConfigChannel(&htim5, &sConfigIC, TIM_CHANNEL_4) != HAL_OK) + { + _Error_Handler(__FILE__, __LINE__); + } + } /* TIM8 init function */ void MX_TIM8_Init(void) @@ -440,6 +479,38 @@ void HAL_TIM_Encoder_MspInit(TIM_HandleTypeDef* tim_encoderHandle) /* USER CODE END TIM4_MspInit 1 */ } } + +void HAL_TIM_IC_MspInit(TIM_HandleTypeDef* tim_icHandle) +{ + + GPIO_InitTypeDef GPIO_InitStruct; + if(tim_icHandle->Instance==TIM5) + { + /* USER CODE BEGIN TIM5_MspInit 0 */ + + /* USER CODE END TIM5_MspInit 0 */ + /* TIM5 clock enable */ + __HAL_RCC_TIM5_CLK_ENABLE(); + + /**TIM5 GPIO Configuration + PA2 ------> TIM5_CH3 + PA3 ------> TIM5_CH4 + */ + GPIO_InitStruct.Pin = GPIO_3_Pin|GPIO_4_Pin; + GPIO_InitStruct.Mode = GPIO_MODE_AF_PP; + GPIO_InitStruct.Pull = GPIO_NOPULL; + GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW; + GPIO_InitStruct.Alternate = GPIO_AF2_TIM5; + HAL_GPIO_Init(GPIOA, &GPIO_InitStruct); + + /* TIM5 interrupt Init */ + HAL_NVIC_SetPriority(TIM5_IRQn, 5, 0); + HAL_NVIC_EnableIRQ(TIM5_IRQn); + /* USER CODE BEGIN TIM5_MspInit 1 */ + + /* USER CODE END TIM5_MspInit 1 */ + } +} void HAL_TIM_MspPostInit(TIM_HandleTypeDef* timHandle) { @@ -627,6 +698,31 @@ void HAL_TIM_Encoder_MspDeInit(TIM_HandleTypeDef* tim_encoderHandle) /* USER CODE END TIM4_MspDeInit 1 */ } +} + +void HAL_TIM_IC_MspDeInit(TIM_HandleTypeDef* tim_icHandle) +{ + + if(tim_icHandle->Instance==TIM5) + { + /* USER CODE BEGIN TIM5_MspDeInit 0 */ + + /* USER CODE END TIM5_MspDeInit 0 */ + /* Peripheral clock disable */ + __HAL_RCC_TIM5_CLK_DISABLE(); + + /**TIM5 GPIO Configuration + PA2 ------> TIM5_CH3 + PA3 ------> TIM5_CH4 + */ + HAL_GPIO_DeInit(GPIOA, GPIO_3_Pin|GPIO_4_Pin); + + /* TIM5 interrupt Deinit */ + HAL_NVIC_DisableIRQ(TIM5_IRQn); + /* USER CODE BEGIN TIM5_MspDeInit 1 */ + + /* USER CODE END TIM5_MspDeInit 1 */ + } } /* USER CODE BEGIN 1 */ diff --git a/Firmware/MotorControl/low_level.cpp b/Firmware/MotorControl/low_level.cpp index 9b701609..d809c236 100644 --- a/Firmware/MotorControl/low_level.cpp +++ b/Firmware/MotorControl/low_level.cpp @@ -603,3 +603,104 @@ void update_brake_current() { safety_critical_disarm_brake_resistor(); } } + + +/* RC PWM input --------------------------------------------------------------*/ + +// @brief Returns the ODrive GPIO number for a given +// TIM2 or TIM5 input capture channel number. +int tim_2_5_channel_num_to_gpio_num(int channel) { +#if HW_VERSION_MAJOR == 3 && HW_VERSION_MINOR >= 3 + if (channel >= 1 && channel <= 4) { + // the channel numbers just happen to coincide with + // the GPIO numbers + return channel; + } else { + return -1; + } +#else +#error "Not implemented" +#endif +} +// @brief Returns the TIM2 or TIM5 channel number +// for a given GPIO number. +uint32_t gpio_num_to_tim_2_5_channel(int gpio_num) { +#if HW_VERSION_MAJOR == 3 && HW_VERSION_MINOR >= 3 + switch (gpio_num) { + case 1: return TIM_CHANNEL_1; + case 2: return TIM_CHANNEL_2; + case 3: return TIM_CHANNEL_3; + case 4: return TIM_CHANNEL_4; + default: return 0; + } +#else +#error "Not implemented" +#endif +} + +void pwm_in_init() { + GPIO_InitTypeDef GPIO_InitStruct; + GPIO_InitStruct.Mode = GPIO_MODE_AF_PP; + GPIO_InitStruct.Pull = GPIO_NOPULL; + GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW; + GPIO_InitStruct.Alternate = GPIO_AF2_TIM5; + + for (int i = 1; i <= 4; ++i) { + if (board_config.pwm_mappings[i].endpoint) { + GPIO_InitStruct.Pin = get_gpio_pin_by_pin(i); + HAL_GPIO_Init(get_gpio_port_by_pin(i), &GPIO_InitStruct); + HAL_TIM_IC_Start_IT(&htim5, gpio_num_to_tim_2_5_channel(i)); + } + } +} + +#define TIM_2_5_CLOCK_HZ TIM_APB1_CLOCK_HZ +#define PWM_MIN_HIGH_TIME ((TIM_2_5_CLOCK_HZ / 1000000UL) * 1000UL) // 1ms high is considered full reverse +#define PWM_MAX_HIGH_TIME ((TIM_2_5_CLOCK_HZ / 1000000UL) * 2000UL) // 2ms high is considered full forward +#define PWM_MIN_LEGAL_HIGH_TIME ((TIM_2_5_CLOCK_HZ / 1000000UL) * 500UL) // ignore high periods shorter than 0.5ms +#define PWM_MAX_LEGAL_HIGH_TIME ((TIM_2_5_CLOCK_HZ / 1000000UL) * 2500UL) // ignore high periods longer than 2.5ms +#define PWM_INVERT_INPUT false + +void handle_pulse(int gpio_num, uint32_t high_time) { + if (high_time < PWM_MIN_LEGAL_HIGH_TIME || high_time > PWM_MAX_LEGAL_HIGH_TIME) + return; + + if (high_time < PWM_MIN_HIGH_TIME) + high_time = PWM_MIN_HIGH_TIME; + if (high_time > PWM_MAX_HIGH_TIME) + high_time = PWM_MAX_HIGH_TIME; + float fraction = (float)(high_time - PWM_MIN_HIGH_TIME) / (float)(PWM_MAX_HIGH_TIME - PWM_MIN_HIGH_TIME); + float value = board_config.pwm_mappings[gpio_num].min + + (fraction * (board_config.pwm_mappings[gpio_num].max - board_config.pwm_mappings[gpio_num].min)); + + uint32_t endpoint_id = board_config.pwm_mappings[gpio_num].endpoint; + if (endpoint_id >= n_endpoints_) + return; + + Endpoint* endpoint = endpoints_[endpoint_id]; + if (!endpoint) + return; + + endpoint->set_from_float(value); +} + +void pwm_in_cb(int channel, uint32_t timestamp) { + static uint32_t last_timestamp[GPIO_COUNT] = { 0 }; + static bool last_pin_state[GPIO_COUNT] = { false }; + static bool last_sample_valid[GPIO_COUNT] = { false }; + + int gpio_num = tim_2_5_channel_num_to_gpio_num(channel); + if (gpio_num < 0 || gpio_num >= GPIO_COUNT) + return; + bool current_pin_state = HAL_GPIO_ReadPin(get_gpio_port_by_pin(gpio_num), get_gpio_pin_by_pin(gpio_num)) != GPIO_PIN_RESET; + + if (last_sample_valid[gpio_num] + && (last_pin_state[gpio_num] != PWM_INVERT_INPUT) + && (current_pin_state == PWM_INVERT_INPUT)) { + handle_pulse(gpio_num, timestamp - last_timestamp[gpio_num]); + } + + last_timestamp[gpio_num] = timestamp; + last_pin_state[gpio_num] = current_pin_state; + last_sample_valid[gpio_num] = true; +} \ No newline at end of file diff --git a/Firmware/MotorControl/low_level.h b/Firmware/MotorControl/low_level.h index 095881cc..41f7f4b5 100644 --- a/Firmware/MotorControl/low_level.h +++ b/Firmware/MotorControl/low_level.h @@ -35,6 +35,7 @@ extern "C" { void pwm_trig_adc_cb(ADC_HandleTypeDef* hadc, bool injected); void vbus_sense_adc_cb(ADC_HandleTypeDef* hadc, bool injected); void tim_update_cb(TIM_HandleTypeDef* htim); +void pwm_in_cb(int channel, uint32_t timestamp); } // Initalisation @@ -43,8 +44,8 @@ void start_pwm(TIM_HandleTypeDef* htim); void sync_timers(TIM_HandleTypeDef* htim_a, TIM_HandleTypeDef* htim_b, uint16_t TIM_CLOCKSOURCE_ITRx, uint16_t count_offset); void start_general_purpose_adc(); - float get_adc_voltage(GPIO_TypeDef* GPIO_port, uint16_t GPIO_pin); +void pwm_in_init(); void update_brake_current(); diff --git a/Firmware/MotorControl/main.cpp b/Firmware/MotorControl/main.cpp index 3c2c815e..3958d9a1 100644 --- a/Firmware/MotorControl/main.cpp +++ b/Firmware/MotorControl/main.cpp @@ -153,6 +153,9 @@ int odrive_main(void) { // Start ADC for temperature measurements and user measurements start_general_purpose_adc(); + // Start pwm-in compare modules + pwm_in_init(); + // TODO: make dynamically reconfigurable #if HW_VERSION_MAJOR == 3 && HW_VERSION_MINOR >= 3 if (board_config.enable_uart) { diff --git a/Firmware/MotorControl/odrive_main.h b/Firmware/MotorControl/odrive_main.h index bae964fb..e67ab274 100644 --- a/Firmware/MotorControl/odrive_main.h +++ b/Firmware/MotorControl/odrive_main.h @@ -55,6 +55,12 @@ extern SystemStats_t system_stats_; #ifdef __cplusplus } +struct PWMMapping_t { + uint32_t endpoint = 0; + float min = 0; + float max = 0; +}; + // @brief general user configurable board configuration struct BoardConfig_t { bool enable_uart = false; @@ -65,6 +71,7 @@ struct BoardConfig_t { //make_protocol_definitions()), make_protocol_object("axis1", axes[1]->make_protocol_definitions()), diff --git a/Firmware/communication/protocol.hpp b/Firmware/communication/protocol.hpp index f51fc2de..cad17d35 100644 --- a/Firmware/communication/protocol.hpp +++ b/Firmware/communication/protocol.hpp @@ -11,6 +11,7 @@ see protocol.md for the protocol specification #include #include #include +#include #include "crc.hpp" // Note that this option cannot be used to debug UART because it prints on UART @@ -407,8 +408,9 @@ class Endpoint { public: //const char* const name_; virtual void handle(const uint8_t* input, size_t input_length, StreamSink* output) = 0; - virtual bool get_string(char * output, size_t length) { return false; }; + virtual bool get_string(char * output, size_t length) { return false; } virtual bool set_string(char * buffer, size_t length) { return false; } + virtual bool set_from_float(float value) { return false; } }; class EndpointProvider { @@ -549,6 +551,40 @@ ProtocolObject make_protocol_object(const char * name, TMembers&&.. return ProtocolObject(name, std::forward(member_list)...); } +//template +//bool set_from_float_ex(float value, T* property) { +// return false; +//} + +namespace conversion { +//template +template +bool set_from_float_ex(float value, float* property, int) { + return *property = value, true; +} +template +bool set_from_float_ex(float value, bool* property, int) { + return *property = (value >= 0.0f), true; +} +template::value && !std::is_const::value>> +bool set_from_float_ex(float value, T* property, int) { + return *property = static_cast(std::round(value)), true; +} +template +bool set_from_float_ex(float value, T* property, ...) { + return false; +} +template +bool set_from_float(float value, T* property) { + return set_from_float_ex(value, property, 0); +} +} + +//template +//bool set_from_float_ex<>(float value, T* property) { +// return false; +//} + // TODO: move to cpp_utils #define ENABLE_IF_SAME(a, b, type) \ @@ -675,6 +711,10 @@ public: return set_string_ex(buffer, length, 0); } + bool set_from_float(float value) final { + return conversion::set_from_float(value, property_); + } + void register_endpoints(Endpoint** list, size_t id, size_t length) { if (id < length) list[id] = this;