Mirrors_Framework/ODrive
1
0
Fork 0
mirror of https://github.com/odriverobotics/ODrive.git synced 2026-09-22 08:04:07 +08:00
Code Issues Actions Packages Projects Releases Wiki Activity

trigger on tim trigger output instead of CC channel: Fixes single motor operation

Browse Source
This commit is contained in:
Oskar Weigl
2017-07-21 20:38:59 -07:00
parent b0b4bc300f
commit b0910c71b9
3 changed files with 23 additions and 62 deletions
Download Patch File Download Diff File Expand all files Collapse all files
MotorControl/low_level.c
+18 -57
View File
@@ -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);
MotorControl/low_level.h
+2 -2
View File
@@ -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);
Src/stm32f4xx_it.c
+3 -3
View File
@@ -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));
}
}