From 47fedb08e7f9c21f4548ef1966c29a534e562a5b Mon Sep 17 00:00:00 2001 From: wzli Date: Mon, 28 Nov 2016 00:31:09 +0900 Subject: [PATCH 1/4] added routine to sync timers --- MotorControl/low_level.c | 75 +++++++++++++++++++++++++++++++--------- 1 file changed, 58 insertions(+), 17 deletions(-) diff --git a/MotorControl/low_level.c b/MotorControl/low_level.c index 3810c471..18799eb3 100644 --- a/MotorControl/low_level.c +++ b/MotorControl/low_level.c @@ -101,6 +101,61 @@ static void DRV8301_setup() { } } +static void start_pwm(TIM_HandleTypeDef htim){ + //Init PWM + int half_load = htim.Instance->ARR/2; + htim.Instance->CCR1 = half_load; + htim.Instance->CCR2 = half_load; + htim.Instance->CCR3 = half_load; + + //This hardware obfustication layer really is getting on my nerves + HAL_TIM_PWM_Start(&htim, TIM_CHANNEL_1); + HAL_TIMEx_PWMN_Start(&htim, TIM_CHANNEL_1); + HAL_TIM_PWM_Start(&htim, TIM_CHANNEL_2); + HAL_TIMEx_PWMN_Start(&htim, TIM_CHANNEL_2); + HAL_TIM_PWM_Start(&htim, TIM_CHANNEL_3); + HAL_TIMEx_PWMN_Start(&htim, TIM_CHANNEL_3); + + htim.Instance->CCR4 = 1; + HAL_TIM_PWM_Start_IT(&htim, TIM_CHANNEL_4); + + //Turn off output + //__HAL_TIM_MOE_DISABLE(&htim); +} + +static void sync_timers(TIM_HandleTypeDef htim_a, TIM_HandleTypeDef htim_b, + uint16_t internal_trigger_source, uint16_t count_offset) { + + uint16_t CR2_store = htim_a.Instance->CR2; + uint16_t SMCR_store = htim_b.Instance->SMCR; + + /* Disable both timer counters*/ + htim_a.Instance->CR1 &= ~TIM_CR1_CEN; + htim_b.Instance->CR1 &= ~TIM_CR1_CEN; + + /* Set first timer to send TRGO on counter enable*/ + htim_a.Instance->CR2 &= ~TIM_CR2_MMS; + htim_a.Instance->CR2 |= TIM_TRGO_ENABLE; + + /* Set Trigger Source of second timer to the TRGO of the first timer*/ + htim_b.Instance->SMCR &= ~TIM_SMCR_TS; + htim_b.Instance->SMCR |= TIM_CLOCKSOURCE_ITR0; + + /* Set 2nd timer to start on trigger*/ + htim_b.Instance->SMCR &= ~TIM_SMCR_SMS; + htim_b.Instance->SMCR |= (TIM_SMCR_SMS_2 | TIM_SMCR_SMS_1); + + htim_a.Instance->CNT = 0; + htim_b.Instance->CNT = count_offset; + + /* Start Timer 1*/ + htim_a.Instance->CR1 |= (TIM_CR1_CEN); + + /* Restore timer configs */ + htim_a.Instance->CR2 = CR2_store; + htim_b.Instance->SMCR = SMCR_store; +} + static void start_adc_pwm(){ //Enable ADC and interrupts __HAL_ADC_ENABLE(&hadc2); @@ -114,25 +169,11 @@ static void start_adc_pwm(){ __HAL_DBGMCU_FREEZE_TIM1(); __HAL_DBGMCU_FREEZE_TIM8(); - //Init PWM - int half_load = htim1.Instance->ARR/2; - htim1.Instance->CCR1 = half_load; - htim1.Instance->CCR2 = half_load; - htim1.Instance->CCR3 = half_load; + start_pwm(htim1); + start_pwm(htim8); - //This hardware obfustication layer really is getting on my nerves - HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_1); - HAL_TIMEx_PWMN_Start(&htim1, TIM_CHANNEL_1); - HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_2); - HAL_TIMEx_PWMN_Start(&htim1, TIM_CHANNEL_2); - HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_3); - HAL_TIMEx_PWMN_Start(&htim1, TIM_CHANNEL_3); + sync_timers(htim1, htim8, TIM_CLOCKSOURCE_ITR0, 0); - htim1.Instance->CCR4 = 1; - HAL_TIM_PWM_Start_IT(&htim1, TIM_CHANNEL_4); - - //Turn off output - //__HAL_TIM_MOE_DISABLE(&htim1); } static float phase_current_from_adcval(uint32_t ADCValue, int motornum) { From 7195f94367586c9f4f9a2ec8f5a2e1f70eb50454 Mon Sep 17 00:00:00 2001 From: wzli Date: Mon, 28 Nov 2016 01:15:49 +0900 Subject: [PATCH 2/4] turn off output during timer sync, added 90 deg phase offset to timers --- MotorControl/low_level.c | 14 ++++++++++---- 1 file changed, 10 insertions(+), 4 deletions(-) diff --git a/MotorControl/low_level.c b/MotorControl/low_level.c index 18799eb3..5052867c 100644 --- a/MotorControl/low_level.c +++ b/MotorControl/low_level.c @@ -126,6 +126,10 @@ static void start_pwm(TIM_HandleTypeDef htim){ static void sync_timers(TIM_HandleTypeDef htim_a, TIM_HandleTypeDef htim_b, uint16_t internal_trigger_source, uint16_t count_offset) { + //Turn off output + __HAL_TIM_MOE_DISABLE(&htim_a); + __HAL_TIM_MOE_DISABLE(&htim_b); + uint16_t CR2_store = htim_a.Instance->CR2; uint16_t SMCR_store = htim_b.Instance->SMCR; @@ -143,7 +147,7 @@ static void sync_timers(TIM_HandleTypeDef htim_a, TIM_HandleTypeDef htim_b, /* Set 2nd timer to start on trigger*/ htim_b.Instance->SMCR &= ~TIM_SMCR_SMS; - htim_b.Instance->SMCR |= (TIM_SMCR_SMS_2 | TIM_SMCR_SMS_1); + htim_b.Instance->SMCR |= TIM_SLAVEMODE_TRIGGER; htim_a.Instance->CNT = 0; htim_b.Instance->CNT = count_offset; @@ -154,6 +158,10 @@ static void sync_timers(TIM_HandleTypeDef htim_a, TIM_HandleTypeDef htim_b, /* Restore timer configs */ htim_a.Instance->CR2 = CR2_store; htim_b.Instance->SMCR = SMCR_store; + + //Turn on output + __HAL_TIM_MOE_ENABLE(&htim_a); + __HAL_TIM_MOE_ENABLE(&htim_b); } static void start_adc_pwm(){ @@ -171,9 +179,7 @@ static void start_adc_pwm(){ start_pwm(htim1); start_pwm(htim8); - - sync_timers(htim1, htim8, TIM_CLOCKSOURCE_ITR0, 0); - + sync_timers(htim1, htim8, TIM_CLOCKSOURCE_ITR0, htim1.Instance->ARR/2); } static float phase_current_from_adcval(uint32_t ADCValue, int motornum) { From 597761c57319cbb8b2895acef95d6429e8e00949 Mon Sep 17 00:00:00 2001 From: Oskar Weigl Date: Mon, 28 Nov 2016 01:20:54 +0900 Subject: [PATCH 3/4] even fancier --- Makefile | 2 +- MotorControl/low_level.c | 12 +++++++----- 2 files changed, 8 insertions(+), 6 deletions(-) diff --git a/Makefile b/Makefile index 1d357169..97f8a16a 100644 --- a/Makefile +++ b/Makefile @@ -13,7 +13,7 @@ TARGET = ODriveFirmware # debug build? DEBUG = 1 # optimization -OPT = -O0 +OPT = -Os ####################################### # pathes diff --git a/MotorControl/low_level.c b/MotorControl/low_level.c index c1012d4c..e506d8d7 100644 --- a/MotorControl/low_level.c +++ b/MotorControl/low_level.c @@ -56,7 +56,7 @@ static void init_encoders(); static void start_adc_pwm(); static float phase_current_from_adcval(uint32_t ADCValue, int motornum); static void pwm_trig_adc_cb(ADC_HandleTypeDef* hadc); -static void mark_timing(); +static bool check_timing(); static void set_timings(Motor_t* motor, float tA, float tB, float tC); static void wait_for_current_meas(osMailQId queue, float* phB_current, float* phC_current); static float measure_phase_resistance(Motor_t* motor, float test_current); @@ -64,7 +64,7 @@ static float measure_phase_resistance(Motor_t* motor, float test_current); //Special function name for ADC callback. //Automatically registered if defined. void HAL_ADCEx_InjectedConvCpltCallback(ADC_HandleTypeDef* hadc) { - //mark_timing(); + // check_timing(); pwm_trig_adc_cb(hadc); } @@ -231,6 +231,7 @@ static void pwm_trig_adc_cb(ADC_HandleTypeDef* hadc) { mail_ptr->current_phB = phB_current - phB_DC_calib; mail_ptr->current_phC = phC_current - phC_DC_calib; osMailPut(M0_Iph_queue, mail_ptr); + check_timing(); } else if (trig_src == ADC_EXTERNALTRIGINJECCONV_T1_TRGO) { //We are measuring DC_CAL here @@ -253,7 +254,7 @@ static void pwm_trig_adc_cb(ADC_HandleTypeDef* hadc) { } } -void mark_timing() { +static bool check_timing() { #define log_size 32 static uint16_t timings[log_size]; static int idx = 0; @@ -270,6 +271,7 @@ void mark_timing() { idx = 0; timings[idx] = timing; + return down; } static void wait_for_current_meas(osMailQId queue, float* phB_current, float* phC_current) { @@ -344,7 +346,7 @@ static void square_wave_test() { float M0_phB_current, M0_phC_current; wait_for_current_meas(M0_Iph_queue, &M0_phB_current, &M0_phC_current); - mark_timing(); + check_timing(); float Ialpha = -M0_phB_current - M0_phC_current; float delta = Ialpha - mean[i][rep]; @@ -358,7 +360,7 @@ static void square_wave_test() { SVM(mod, 0.0f, &tA, &tB, &tC); set_timings(&motors[0], tA, tB, tC); - mark_timing(); + safe_assert(!check_timing()); } } ++cycle_num; From 5df7b74a2c7d3d1a5e03eeff004b4082227ce1aa Mon Sep 17 00:00:00 2001 From: wzli Date: Thu, 1 Dec 2016 23:31:26 +0900 Subject: [PATCH 4/4] restore timers to original output after sync --- MotorControl/low_level.c | 17 +++++++++++------ 1 file changed, 11 insertions(+), 6 deletions(-) diff --git a/MotorControl/low_level.c b/MotorControl/low_level.c index c6e1568c..ad41b9bb 100644 --- a/MotorControl/low_level.c +++ b/MotorControl/low_level.c @@ -130,13 +130,17 @@ static void start_pwm(TIM_HandleTypeDef htim){ static void sync_timers(TIM_HandleTypeDef htim_a, TIM_HandleTypeDef htim_b, uint16_t internal_trigger_source, uint16_t count_offset) { - //Turn off output - __HAL_TIM_MOE_DISABLE(&htim_a); - __HAL_TIM_MOE_DISABLE(&htim_b); + //Store intial timer configs + uint16_t MOE_store_a = htim_a.Instance->BDTR & (TIM_BDTR_MOE); + uint16_t MOE_store_b = htim_b.Instance->BDTR & (TIM_BDTR_MOE); uint16_t CR2_store = htim_a.Instance->CR2; uint16_t SMCR_store = htim_b.Instance->SMCR; + //Turn off output + __HAL_TIM_MOE_DISABLE(&htim_a); + __HAL_TIM_MOE_DISABLE(&htim_b); + /* Disable both timer counters*/ htim_a.Instance->CR1 &= ~TIM_CR1_CEN; htim_b.Instance->CR1 &= ~TIM_CR1_CEN; @@ -153,6 +157,7 @@ static void sync_timers(TIM_HandleTypeDef htim_a, TIM_HandleTypeDef htim_b, htim_b.Instance->SMCR &= ~TIM_SMCR_SMS; htim_b.Instance->SMCR |= TIM_SLAVEMODE_TRIGGER; + // set counter offset htim_a.Instance->CNT = 0; htim_b.Instance->CNT = count_offset; @@ -163,9 +168,9 @@ static void sync_timers(TIM_HandleTypeDef htim_a, TIM_HandleTypeDef htim_b, htim_a.Instance->CR2 = CR2_store; htim_b.Instance->SMCR = SMCR_store; - //Turn on output - __HAL_TIM_MOE_ENABLE(&htim_a); - __HAL_TIM_MOE_ENABLE(&htim_b); + //restore output + htim_a.Instance->BDTR |= MOE_store_a; + htim_b.Instance->BDTR |= MOE_store_b; } static void init_encoders() {