From 68163a88b6f6d175f5b8f5a98fdb9bbe50da961b Mon Sep 17 00:00:00 2001 From: Benjamin Vedder Date: Fri, 20 Mar 2020 21:48:21 +0100 Subject: [PATCH] Some optimizations to make mc_interface timer ISR run faster --- hwconf/hw_stormcore_100d.h | 1 - hwconf/hw_stormcore_60d.h | 1 - mc_interface.c | 65 ++++++++++++++++++++++++-------------- mcpwm_foc.c | 59 ++++++++++++++++++++++++++++++++++ mcpwm_foc.h | 9 ++++++ 5 files changed, 109 insertions(+), 26 deletions(-) diff --git a/hwconf/hw_stormcore_100d.h b/hwconf/hw_stormcore_100d.h index 887c6c05..7cbe2099 100644 --- a/hwconf/hw_stormcore_100d.h +++ b/hwconf/hw_stormcore_100d.h @@ -34,7 +34,6 @@ //#define HW_DEAD_TIME_NSEC 360.0 // Dead time -#define HW_HAS_DUAL_MOTOR //Switch Pins #define HW_HAS_STORMCORE_SWITCH #define HW_HAS_RGB_SWITCH diff --git a/hwconf/hw_stormcore_60d.h b/hwconf/hw_stormcore_60d.h index d14f45da..382bf6e8 100644 --- a/hwconf/hw_stormcore_60d.h +++ b/hwconf/hw_stormcore_60d.h @@ -34,7 +34,6 @@ //#define HW_DEAD_TIME_NSEC 360.0 // Dead time -#define HW_HAS_DUAL_MOTOR //Switch Pins #define HW_HAS_STORMCORE_SWITCH #define HW_HAS_RGB_SWITCH diff --git a/mc_interface.c b/mc_interface.c index 5624e87c..1d0234a8 100644 --- a/mc_interface.c +++ b/mc_interface.c @@ -80,6 +80,7 @@ typedef struct { float m_gate_driver_voltage; float m_motor_current_unbalance; float m_motor_current_unbalance_error_rate; + float m_f_samp_now; } motor_if_state_t; // Private variables @@ -1583,24 +1584,22 @@ void mc_interface_mc_timer_isr(bool is_second_motor) { wrong_voltage_iterations = 0; } - // Monitor currents balance. The sum of the 3 currents should be zero -#ifdef HW_HAS_3_SHUNTS - if (!motor->m_conf.foc_sample_high_current) { // This won't work when high current sampling is used - motor->m_motor_current_unbalance = mc_interface_get_abs_motor_current_unbalance(); - - if (motor->m_motor_current_unbalance > MCCONF_MAX_CURRENT_UNBALANCE) { - UTILS_LP_FAST(motor->m_motor_current_unbalance_error_rate, 1.0, (1 / 20000.0)); - } else { - UTILS_LP_FAST(motor->m_motor_current_unbalance_error_rate, 0.0, (1 / 20000.0)); - } - - if (motor->m_motor_current_unbalance_error_rate > MCCONF_MAX_CURRENT_UNBALANCE_RATE) { - mc_interface_fault_stop(FAULT_CODE_UNBALANCED_CURRENTS, is_second_motor); - } + // Fetch these values here in a config-specific code to avoid some overheat of the general + // function. That will make this interrupt run a bit faster. + mc_state state; + float current; + float current_in; + if (motor->m_conf.motor_type == MOTOR_TYPE_FOC) { + state = mcpwm_foc_get_state_motor(is_second_motor); + current = mcpwm_foc_get_tot_current_filtered_motor(is_second_motor); + current_in = mcpwm_foc_get_tot_current_in_filtered_motor(is_second_motor); + } else { + state = mcpwm_get_state(); + current = mcpwm_get_tot_current_filtered(); + current_in = mcpwm_get_tot_current_in_filtered(); } -#endif - if (mc_interface_get_state() == MC_STATE_RUNNING) { + if (state == MC_STATE_RUNNING) { motor->m_cycles_running++; } else { motor->m_cycles_running = 0; @@ -1610,8 +1609,6 @@ void mc_interface_mc_timer_isr(bool is_second_motor) { pwn_done_func(); } - const float current = mc_interface_get_tot_current_filtered(); - const float current_in = mc_interface_get_tot_current_in_filtered(); motor->m_motor_current_sum += current; motor->m_input_current_sum += current_in; motor->m_motor_current_iterations++; @@ -1631,8 +1628,8 @@ void mc_interface_mc_timer_isr(bool is_second_motor) { float abs_current_filtered = current; if (motor->m_conf.motor_type == MOTOR_TYPE_FOC) { // TODO: Make this more general - abs_current = mcpwm_foc_get_abs_motor_current(); - abs_current_filtered = mcpwm_foc_get_abs_motor_current_filtered(); + abs_current = mcpwm_foc_get_abs_motor_current_motor(is_second_motor); + abs_current_filtered = mcpwm_foc_get_abs_motor_current_filtered_motor(is_second_motor); } // Current fault code @@ -1671,8 +1668,9 @@ void mc_interface_mc_timer_isr(bool is_second_motor) { } #endif + float f_samp = motor->m_f_samp_now; + // Watt and ah counters - const float f_samp = mc_interface_get_sampling_frequency_now(); if (fabsf(current) > 1.0) { // Some extra filtering static float curr_diff_sum = 0.0; @@ -1714,7 +1712,7 @@ void mc_interface_mc_timer_isr(bool is_second_motor) { break; case DEBUG_SAMPLING_START: - if (mc_interface_get_state() == MC_STATE_RUNNING || m_sample_now > 0) { + if (state == MC_STATE_RUNNING || m_sample_now > 0) { sample = true; } @@ -1752,7 +1750,7 @@ void mc_interface_mc_timer_isr(bool is_second_motor) { m_sample_mode = DEBUG_SAMPLING_OFF; } - if (mc_interface_get_state() == MC_STATE_RUNNING && m_sample_trigger < 0) { + if (state == MC_STATE_RUNNING && m_sample_trigger < 0) { m_sample_trigger = m_sample_now; } } break; @@ -1818,7 +1816,7 @@ void mc_interface_mc_timer_isr(bool is_second_motor) { m_phase_samples[m_sample_now] = 0; } - if (mc_interface_get_state() == MC_STATE_DETECTING) { + if (state == MC_STATE_DETECTING) { m_curr0_samples[m_sample_now] = (int16_t)mcpwm_detect_currents[mcpwm_get_comm_step() - 1]; m_curr1_samples[m_sample_now] = (int16_t)mcpwm_detect_currents_diff[mcpwm_get_comm_step() - 1]; @@ -2100,6 +2098,8 @@ static volatile motor_if_state_t *motor_now(void) { static void run_timer_tasks(volatile motor_if_state_t *motor) { bool is_motor_1 = motor == &m_motor_1; + motor->m_f_samp_now = mc_interface_get_sampling_frequency_now(); + // Decrease fault iterations if (motor->m_ignore_iterations > 0) { motor->m_ignore_iterations--; @@ -2193,6 +2193,23 @@ static void run_timer_tasks(volatile motor_if_state_t *motor) { } #endif } + + // Monitor currents balance. The sum of the 3 currents should be zero +#ifdef HW_HAS_3_SHUNTS + if (!motor->m_conf.foc_sample_high_current) { // This won't work when high current sampling is used + motor->m_motor_current_unbalance = mc_interface_get_abs_motor_current_unbalance(); + + if (motor->m_motor_current_unbalance > MCCONF_MAX_CURRENT_UNBALANCE) { + UTILS_LP_FAST(motor->m_motor_current_unbalance_error_rate, 1.0, (1 / 1000.0)); + } else { + UTILS_LP_FAST(motor->m_motor_current_unbalance_error_rate, 0.0, (1 / 1000.0)); + } + + if (motor->m_motor_current_unbalance_error_rate > MCCONF_MAX_CURRENT_UNBALANCE_RATE) { + mc_interface_fault_stop(FAULT_CODE_UNBALANCED_CURRENTS, !is_motor_1); + } + } +#endif } static THD_FUNCTION(timer_thread, arg) { diff --git a/mcpwm_foc.c b/mcpwm_foc.c index 08ab1aed..2e0c386e 100644 --- a/mcpwm_foc.c +++ b/mcpwm_foc.c @@ -1051,6 +1051,65 @@ bool mcpwm_foc_is_using_encoder(void) { return motor_now()->m_using_encoder; } +float mcpwm_foc_get_tot_current_motor(bool is_second_motor) { +#ifdef HW_HAS_DUAL_MOTORS + volatile motor_all_state_t *motor = is_second_motor ? &m_motor_2 : &m_motor_1; + return SIGN(motor->m_motor_state.vq) * motor->m_motor_state.iq; +#else + return SIGN(m_motor_1.m_motor_state.vq) * m_motor_1.m_motor_state.iq; +#endif +} + +float mcpwm_foc_get_tot_current_filtered_motor(bool is_second_motor) { +#ifdef HW_HAS_DUAL_MOTORS + volatile motor_all_state_t *motor = is_second_motor ? &m_motor_2 : &m_motor_1; + return SIGN(motor->m_motor_state.vq) * motor->m_motor_state.iq_filter; +#else + return SIGN(m_motor_1.m_motor_state.vq) * m_motor_1.m_motor_state.iq_filter; +#endif +} + +float mcpwm_foc_get_tot_current_in_motor(bool is_second_motor) { +#ifdef HW_HAS_DUAL_MOTORS + return (is_second_motor ? &m_motor_2 : &m_motor_1)->m_motor_state.i_bus; +#else + return m_motor_1.m_motor_state.i_bus; +#endif +} + +float mcpwm_foc_get_tot_current_in_filtered_motor(bool is_second_motor) { + // TODO: Filter current? +#ifdef HW_HAS_DUAL_MOTORS + return (is_second_motor ? &m_motor_2 : &m_motor_1)->m_motor_state.i_bus; +#else + return m_motor_1.m_motor_state.i_bus; +#endif +} + +float mcpwm_foc_get_abs_motor_current_motor(bool is_second_motor) { +#ifdef HW_HAS_DUAL_MOTORS + return (is_second_motor ? &m_motor_2 : &m_motor_1)->m_motor_state.i_abs; +#else + return m_motor_1.m_motor_state.i_abs; +#endif +} + +float mcpwm_foc_get_abs_motor_current_filtered_motor(bool is_second_motor) { +#ifdef HW_HAS_DUAL_MOTORS + return (is_second_motor ? &m_motor_2 : &m_motor_1)->m_motor_state.i_abs_filter; +#else + return m_motor_1.m_motor_state.i_abs_filter; +#endif +} + +mc_state mcpwm_foc_get_state_motor(bool is_second_motor) { +#ifdef HW_HAS_DUAL_MOTORS + return (is_second_motor ? &m_motor_2 : &m_motor_1)->m_state; +#else + return m_motor_1.m_state; +#endif +} + /** * Calculate the current RPM of the motor. This is a signed value and the sign * depends on the direction the motor is rotating in. Note that this value has diff --git a/mcpwm_foc.h b/mcpwm_foc.h index f69ee50f..ca36cb63 100644 --- a/mcpwm_foc.h +++ b/mcpwm_foc.h @@ -91,6 +91,15 @@ void mcpwm_foc_set_current_offsets( float mcpwm_foc_get_ts(void); bool mcpwm_foc_is_using_encoder(void); +// Functions where the motor can be selected +float mcpwm_foc_get_tot_current_motor(bool is_second_motor); +float mcpwm_foc_get_tot_current_filtered_motor(bool is_second_motor); +float mcpwm_foc_get_tot_current_in_motor(bool is_second_motor); +float mcpwm_foc_get_tot_current_in_filtered_motor(bool is_second_motor); +float mcpwm_foc_get_abs_motor_current_motor(bool is_second_motor); +float mcpwm_foc_get_abs_motor_current_filtered_motor(bool is_second_motor); +mc_state mcpwm_foc_get_state_motor(bool is_second_motor); + // Interrupt handlers void mcpwm_foc_tim_sample_int_handler(void); void mcpwm_foc_adc_int_handler(void *p, uint32_t flags);