From b0910c71b99b0a38c3d1b38b13d726ae1f7ec7fe Mon Sep 17 00:00:00 2001 From: Oskar Weigl Date: Fri, 21 Jul 2017 20:27:40 -0700 Subject: [PATCH] trigger on tim trigger output instead of CC channel: Fixes single motor operation --- MotorControl/low_level.c | 75 ++++++++++------------------------------ MotorControl/low_level.h | 4 +-- Src/stm32f4xx_it.c | 6 ++-- 3 files changed, 23 insertions(+), 62 deletions(-) diff --git a/MotorControl/low_level.c b/MotorControl/low_level.c index 0095d587..3f61d893 100755 --- a/MotorControl/low_level.c +++ b/MotorControl/low_level.c @@ -591,8 +591,8 @@ static void start_adc_pwm(){ __HAL_DBGMCU_FREEZE_TIM8(); // Turn off the regular conversion trigger for the inital phase - hadc2.Instance->CR2 &= ~ADC_CR2_EXTEN; - hadc3.Instance->CR2 &= ~ADC_CR2_EXTEN; + // hadc2.Instance->CR2 &= ~ADC_CR2_EXTEN; + // hadc3.Instance->CR2 &= ~ADC_CR2_EXTEN; start_pwm(&htim1); start_pwm(&htim8); @@ -708,7 +708,7 @@ void step_cb(uint16_t GPIO_Pin) { } } -void vbus_sense_adc_cb(ADC_HandleTypeDef* hadc) { +void vbus_sense_adc_cb(ADC_HandleTypeDef* hadc, bool injected) { static const float voltage_scale = 3.3f * 11.0f / (float)(1<<12); // Only one conversion in sequence, so only rank1 uint32_t ADCValue = HAL_ADCEx_InjectedGetValue(hadc, ADC_INJECTED_RANK_1); @@ -717,7 +717,7 @@ void vbus_sense_adc_cb(ADC_HandleTypeDef* hadc) { // This is the callback from the ADC that we expect after the PWM has triggered an ADC conversion. // TODO: Document how the phasing is done, link to timing diagram -void pwm_trig_adc_cb(ADC_HandleTypeDef* hadc) { +void pwm_trig_adc_cb(ADC_HandleTypeDef* hadc, bool injected) { #define calib_tau 0.2f //@TOTO make more easily configurable static const float calib_filter_k = CURRENT_MEAS_PERIOD / calib_tau; @@ -727,27 +727,17 @@ void pwm_trig_adc_cb(ADC_HandleTypeDef* hadc) { return; }; + // Motor 0 is on Timer 1, which triggers ADC 2 and 3 on an injected conversion + // Motor 1 is on Timer 8, which triggers ADC 2 and 3 on a regular conversion + // If the corresponding timer is counting up, we just sampled in SVM vector 0, i.e. real current + // If we are counting down, we just sampled in SVM vector 7, with zero current + Motor_t* motor = injected ? &motors[0] : &motors[1]; + bool counting_down = motor->motor_timer->Instance->CR1 & TIM_CR1_DIR; + bool current_meas_not_DC_CAL; - Motor_t* motor; - - // Check if this trigger was the CC4 channel, used for actual current measurement at SVM vector 0 - // or the update trigger, which is used for DC_CAL measurement at SVM vector 7 - // M1 DC_CAL is a special case since due to hardware limitations, it uses the "regular" conversions - // rather than the injected ones. - uint32_t inj_src = hadc->Instance->CR2 & ADC_CR2_JEXTSEL; - uint32_t reg_edge = hadc->Instance->CR2 & ADC_CR2_EXTEN; - if (reg_edge != ADC_EXTERNALTRIGCONVEDGE_NONE) { + if (motor == &motors[1] && counting_down) { // We are measuring M1 DC_CAL here current_meas_not_DC_CAL = false; - motor = &motors[1]; - // Next measurement on this motor will be M1 current measurement - HAL_GPIO_WritePin(M1_DC_CAL_GPIO_Port, M1_DC_CAL_Pin, GPIO_PIN_RESET); - // Next measurement on this ADC will be M0 current - hadc->Instance->CR2 &= ~(ADC_CR2_JEXTEN | ADC_CR2_EXTEN | ADC_CR2_JEXTSEL); - hadc->Instance->CR2 |= (ADC_EXTERNALTRIGINJECCONVEDGE_RISING | ADC_EXTERNALTRIGINJECCONV_T1_CC4); - // Set ADC channels for next measurement - hadc->Instance->JSQR &= ~ADC_JSQR(ADC_JSQR_JSQ1, 1, 1); - hadc->Instance->JSQR |= ADC_JSQR((hadc == &hadc2) ? ADC_CHANNEL_10 : ADC_CHANNEL_11, 1, 1); // Load next timings for M0 (only once is sufficient) if (hadc == &hadc2) { motors[0].motor_timer->Instance->CCR1 = motors[0].next_timings[0]; @@ -757,18 +747,9 @@ void pwm_trig_adc_cb(ADC_HandleTypeDef* hadc) { // Check the timing of the sequencing check_timing(motor); - } else if (inj_src == ADC_EXTERNALTRIGINJECCONV_T1_CC4) { + } else if (motor == &motors[0] && !counting_down) { // We are measuring M0 current here current_meas_not_DC_CAL = true; - motor = &motors[0]; - // Next measurement on this motor will be M0 DC_CAL measurement - HAL_GPIO_WritePin(M0_DC_CAL_GPIO_Port, M0_DC_CAL_Pin, GPIO_PIN_SET); - // Next measurement on this ADC will be M1 current - hadc->Instance->CR2 &= ~(ADC_CR2_JEXTEN | ADC_CR2_EXTEN | ADC_CR2_JEXTSEL); - hadc->Instance->CR2 |= (ADC_EXTERNALTRIGINJECCONVEDGE_RISING | ADC_EXTERNALTRIGINJECCONV_T8_CC4); - // Set ADC channels for next measurement - hadc->Instance->JSQR &= ~ADC_JSQR(ADC_JSQR_JSQ1, 1, 1); - hadc->Instance->JSQR |= ADC_JSQR((hadc == &hadc2) ? ADC_CHANNEL_13 : ADC_CHANNEL_12, 1, 1); // Load next timings for M1 (only once is sufficient) if (hadc == &hadc2) { motors[1].motor_timer->Instance->CCR1 = motors[1].next_timings[0]; @@ -778,33 +759,15 @@ void pwm_trig_adc_cb(ADC_HandleTypeDef* hadc) { // Check the timing of the sequencing check_timing(motor); - } else if (inj_src == ADC_EXTERNALTRIGINJECCONV_T8_CC4) { + } else if (motor == &motors[1] && !counting_down) { // We are measuring M1 current here current_meas_not_DC_CAL = true; - motor = &motors[1]; - // Next measurement on this motor will be M1 DC_CAL measurement - HAL_GPIO_WritePin(M1_DC_CAL_GPIO_Port, M1_DC_CAL_Pin, GPIO_PIN_SET); - // Next measurement on this ADC will be M0 DC_CAL - hadc->Instance->CR2 &= ~(ADC_CR2_JEXTEN | ADC_CR2_EXTEN | ADC_CR2_JEXTSEL); - hadc->Instance->CR2 |= (ADC_EXTERNALTRIGINJECCONVEDGE_RISING | ADC_EXTERNALTRIGINJECCONV_T1_TRGO); - // Set ADC channels for next measurement - hadc->Instance->JSQR &= ~ADC_JSQR(ADC_JSQR_JSQ1, 1, 1); - hadc->Instance->JSQR |= ADC_JSQR((hadc == &hadc2) ? ADC_CHANNEL_10 : ADC_CHANNEL_11, 1, 1); // Check the timing of the sequencing check_timing(motor); - } else if (inj_src == ADC_EXTERNALTRIGINJECCONV_T1_TRGO) { + } else if (motor == &motors[0] && counting_down) { // We are measuring M0 DC_CAL here current_meas_not_DC_CAL = false; - motor = &motors[0]; - // Next measurement on this motor will be M0 current measurement - HAL_GPIO_WritePin(M0_DC_CAL_GPIO_Port, M0_DC_CAL_Pin, GPIO_PIN_RESET); - // Next measurement on this ADC will be M1 DC_CAL - hadc->Instance->CR2 &= ~(ADC_CR2_JEXTEN | ADC_CR2_EXTEN | ADC_CR2_JEXTSEL); - hadc->Instance->CR2 |= ADC_EXTERNALTRIGCONVEDGE_RISING; - // Set ADC channels for next measurement - hadc->Instance->JSQR &= ~ADC_JSQR(ADC_JSQR_JSQ1, 1, 1); - hadc->Instance->JSQR |= ADC_JSQR((hadc == &hadc2) ? ADC_CHANNEL_13 : ADC_CHANNEL_12, 1, 1); // Check the timing of the sequencing check_timing(motor); @@ -814,10 +777,10 @@ void pwm_trig_adc_cb(ADC_HandleTypeDef* hadc) { } uint32_t ADCValue; - if (reg_edge != ADC_EXTERNALTRIGCONVEDGE_NONE) { - ADCValue = HAL_ADC_GetValue(hadc); - } else { + if (injected) { ADCValue = HAL_ADCEx_InjectedGetValue(hadc, ADC_INJECTED_RANK_1); + } else { + ADCValue = HAL_ADC_GetValue(hadc); } float current = phase_current_from_adcval(motor, ADCValue); @@ -1315,7 +1278,6 @@ void motor_thread(void const * argument) { for (;;) { if (motor->do_calibration) { - osDelay(10); __HAL_TIM_MOE_ENABLE(motor->motor_timer);// enable pwm outputs motor_calibration(motor); if(!motor->calibration_ok){ @@ -1325,7 +1287,6 @@ void motor_thread(void const * argument) { } if (motor->calibration_ok && motor->enable_control) { - osDelay(10); motor->enable_step_dir = true; __HAL_TIM_MOE_ENABLE(motor->motor_timer); control_motor_loop(motor); diff --git a/MotorControl/low_level.h b/MotorControl/low_level.h index c724bb7d..edfd2403 100644 --- a/MotorControl/low_level.h +++ b/MotorControl/low_level.h @@ -130,8 +130,8 @@ void set_current_setpoint(Motor_t* motor, float current_setpoint); void safe_assert(int arg); void init_motor_control(); void step_cb(uint16_t GPIO_Pin); -void pwm_trig_adc_cb(ADC_HandleTypeDef* hadc); -void vbus_sense_adc_cb(ADC_HandleTypeDef* hadc); +void pwm_trig_adc_cb(ADC_HandleTypeDef* hadc, bool injected); +void vbus_sense_adc_cb(ADC_HandleTypeDef* hadc, bool injected); //@TODO move motor thread to high level file void motor_thread(void const * argument); diff --git a/Src/stm32f4xx_it.c b/Src/stm32f4xx_it.c index 0cc75fda..302c116b 100644 --- a/Src/stm32f4xx_it.c +++ b/Src/stm32f4xx_it.c @@ -40,7 +40,7 @@ #include "freertos_vars.h" #include "low_level.h" -typedef void (*ADC_handler_t)(ADC_HandleTypeDef* hadc); +typedef void (*ADC_handler_t)(ADC_HandleTypeDef* hadc, bool injected); void ADC_IRQ_Dispatch(ADC_HandleTypeDef* hadc, ADC_handler_t callback); /* USER CODE END 0 */ @@ -250,14 +250,14 @@ void ADC_IRQ_Dispatch(ADC_HandleTypeDef* hadc, ADC_handler_t callback) { uint32_t JEOC = __HAL_ADC_GET_FLAG(hadc, ADC_FLAG_JEOC); uint32_t JEOC_IT_EN = __HAL_ADC_GET_IT_SOURCE(hadc, ADC_IT_JEOC); if (JEOC && JEOC_IT_EN) { - callback(hadc); + callback(hadc, true); __HAL_ADC_CLEAR_FLAG(hadc, (ADC_FLAG_JSTRT | ADC_FLAG_JEOC)); } // Regular measurements uint32_t EOC = __HAL_ADC_GET_FLAG(hadc, ADC_FLAG_EOC); uint32_t EOC_IT_EN = __HAL_ADC_GET_IT_SOURCE(hadc, ADC_IT_EOC); if (EOC && EOC_IT_EN) { - callback(hadc); + callback(hadc, false); __HAL_ADC_CLEAR_FLAG(hadc, (ADC_FLAG_STRT | ADC_FLAG_EOC)); } }