From 42926d530ee05676ff9e0e5b01f17a103ae858b2 Mon Sep 17 00:00:00 2001 From: Marcos Chaparro Date: Thu, 30 Apr 2020 18:34:29 -0300 Subject: [PATCH 1/4] Add MTPA support Signed-off-by: Marcos Chaparro --- confgenerator.c | 3 +++ confgenerator.h | 4 ++-- datatypes.h | 1 + mcconf/mcconf_default.h | 3 +++ mcpwm_foc.c | 14 ++++++++++++++ 5 files changed, 23 insertions(+), 2 deletions(-) diff --git a/confgenerator.c b/confgenerator.c index 7be5d02d4..b736d2e60 100644 --- a/confgenerator.c +++ b/confgenerator.c @@ -72,6 +72,7 @@ int32_t confgenerator_serialize_mcconf(uint8_t *buffer, const mc_configuration * buffer_append_float32_auto(buffer, conf->foc_pll_kp, &ind); buffer_append_float32_auto(buffer, conf->foc_pll_ki, &ind); buffer_append_float32_auto(buffer, conf->foc_motor_l, &ind); + buffer_append_float32_auto(buffer, conf->foc_motor_ld_lq_diff, &ind); buffer_append_float32_auto(buffer, conf->foc_motor_r, &ind); buffer_append_float32_auto(buffer, conf->foc_motor_flux_linkage, &ind); buffer_append_float32_auto(buffer, conf->foc_observer_gain, &ind); @@ -365,6 +366,7 @@ bool confgenerator_deserialize_mcconf(const uint8_t *buffer, mc_configuration *c conf->foc_pll_kp = buffer_get_float32_auto(buffer, &ind); conf->foc_pll_ki = buffer_get_float32_auto(buffer, &ind); conf->foc_motor_l = buffer_get_float32_auto(buffer, &ind); + conf->foc_motor_ld_lq_diff = buffer_get_float32_auto(buffer, &ind); conf->foc_motor_r = buffer_get_float32_auto(buffer, &ind); conf->foc_motor_flux_linkage = buffer_get_float32_auto(buffer, &ind); conf->foc_observer_gain = buffer_get_float32_auto(buffer, &ind); @@ -654,6 +656,7 @@ void confgenerator_set_defaults_mcconf(mc_configuration *conf) { conf->foc_pll_kp = MCCONF_FOC_PLL_KP; conf->foc_pll_ki = MCCONF_FOC_PLL_KI; conf->foc_motor_l = MCCONF_FOC_MOTOR_L; + conf->foc_motor_ld_lq_diff = MCCONF_FOC_MOTOR_LD_LQ_DIFF; conf->foc_motor_r = MCCONF_FOC_MOTOR_R; conf->foc_motor_flux_linkage = MCCONF_FOC_MOTOR_FLUX_LINKAGE; conf->foc_observer_gain = MCCONF_FOC_OBSERVER_GAIN; diff --git a/confgenerator.h b/confgenerator.h index 4b143e3b6..1b205e874 100644 --- a/confgenerator.h +++ b/confgenerator.h @@ -8,8 +8,8 @@ #include // Constants -#define MCCONF_SIGNATURE 1775793947 -#define APPCONF_SIGNATURE 2662993821 +#define MCCONF_SIGNATURE 2013508732 +#define APPCONF_SIGNATURE 2460147246 // Functions int32_t confgenerator_serialize_mcconf(uint8_t *buffer, const mc_configuration *conf); diff --git a/datatypes.h b/datatypes.h index 49b04fe76..6b57f435c 100644 --- a/datatypes.h +++ b/datatypes.h @@ -277,6 +277,7 @@ typedef struct { float foc_encoder_cos_gain; float foc_encoder_sincos_filter_constant; float foc_motor_l; + float foc_motor_ld_lq_diff; float foc_motor_r; float foc_motor_flux_linkage; float foc_observer_gain; diff --git a/mcconf/mcconf_default.h b/mcconf/mcconf_default.h index 9c7acff66..f2c0b6977 100644 --- a/mcconf/mcconf_default.h +++ b/mcconf/mcconf_default.h @@ -263,6 +263,9 @@ #ifndef MCCONF_FOC_MOTOR_FLUX_LINKAGE #define MCCONF_FOC_MOTOR_FLUX_LINKAGE 0.00245 #endif +#ifndef MCCONF_FOC_MOTOR_LD_LQ_DIFF +#define MCCONF_FOC_MOTOR_LD_LQ_DIFF 0.0 +#endif #ifndef MCCONF_FOC_OBSERVER_GAIN #define MCCONF_FOC_OBSERVER_GAIN 9e7 // Can be something like 600 / L #endif diff --git a/mcpwm_foc.c b/mcpwm_foc.c index 9b2275b4c..a03f4d195 100644 --- a/mcpwm_foc.c +++ b/mcpwm_foc.c @@ -195,6 +195,7 @@ static void terminal_plot_hfi(int argc, const char **argv); static void timer_update(volatile motor_all_state_t *motor, float dt); static void input_current_offset_measurement( void ); static void hfi_update(volatile motor_all_state_t *motor); +static void apply_mtpa(float *id, float *iq, volatile motor_all_state_t *motor); // Threads static THD_WORKING_AREA(timer_thread_wa, 1024); @@ -2541,6 +2542,8 @@ void mcpwm_foc_adc_int_handler(void *p, uint32_t flags) { motor_now->m_motor_state.phase = motor_now->m_phase_now_override; } + apply_mtpa(&id_set_tmp, &iq_set_tmp, motor_now); + // Apply current limits // TODO: Consider D axis current for the input current as well. const float mod_q = motor_now->m_motor_state.mod_q; @@ -3981,3 +3984,14 @@ static void terminal_plot_hfi(int argc, const char **argv) { commands_printf("This command requires one argument.\n"); } } + +static void apply_mtpa(float *id, float *iq, volatile motor_all_state_t *motor) { + float ld_lq_diff = motor->m_conf->foc_motor_ld_lq_diff; + + if(ld_lq_diff != 0.0){ + float lambda = motor->m_conf->foc_motor_flux_linkage; + + *id = (lambda - sqrtf(SQ(lambda) + 8.0 * SQ(ld_lq_diff) * SQ(*iq))) / (4.0 * ld_lq_diff); + *iq = SIGN(*iq) * sqrtf(SQ(*iq) - SQ(*id)); + } +} From 2a3110cc53830241082c910128473b7b92995f20 Mon Sep 17 00:00:00 2001 From: Marcos Chaparro Date: Wed, 6 May 2020 12:36:21 -0300 Subject: [PATCH 2/4] Fix id and vd sign during inverted direction Signed-off-by: Marcos Chaparro --- mc_interface.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/mc_interface.c b/mc_interface.c index f6f4ae76c..a59b31f64 100644 --- a/mc_interface.c +++ b/mc_interface.c @@ -1211,7 +1211,7 @@ float mc_interface_read_reset_avg_id(void) { float res = motor_now()->m_motor_id_sum / motor_now()->m_motor_id_iterations; motor_now()->m_motor_id_sum = 0.0; motor_now()->m_motor_id_iterations = 0.0; - return DIR_MULT * res; // TODO: DIR_MULT? + return res; } /** @@ -1237,7 +1237,7 @@ float mc_interface_read_reset_avg_vd(void) { float res = motor_now()->m_motor_vd_sum / motor_now()->m_motor_vd_iterations; motor_now()->m_motor_vd_sum = 0.0; motor_now()->m_motor_vd_iterations = 0.0; - return DIR_MULT * res; + return res; } /** From 2c7ad3f797e61feef6eab7bc2990ffa32f9fd4f1 Mon Sep 17 00:00:00 2001 From: Maximiliano Cordoba Date: Sat, 9 May 2020 11:29:53 -0300 Subject: [PATCH 3/4] add Id into the calculation of motor current Signed-off-by: Maximiliano Cordoba --- mcpwm_foc.c | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/mcpwm_foc.c b/mcpwm_foc.c index a03f4d195..c317926cd 100644 --- a/mcpwm_foc.c +++ b/mcpwm_foc.c @@ -1094,10 +1094,12 @@ float mcpwm_foc_get_tot_current_motor(bool is_second_motor) { 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; + return SIGN(motor->m_motor_state.vq) * + sqrtf( SQ(motor->m_motor_state.iq_filter) + SQ(motor->m_motor_state.id_filter) ); #else (void)is_second_motor; - return SIGN(m_motor_1.m_motor_state.vq) * m_motor_1.m_motor_state.iq_filter; + return SIGN(m_motor_1.m_motor_state.vq) * + sqrtf( SQ(m_motor_1.m_motor_state.iq_filter) + SQ(m_motor_1.m_motor_state.id_filter) ); #endif } From 1876c52b02586dd828eb307340d9bae17c9a3461 Mon Sep 17 00:00:00 2001 From: Marcos Chaparro Date: Sat, 16 May 2020 18:39:58 -0300 Subject: [PATCH 4/4] Disable MTPA during encoder detection Avoids id current set by encoder detection to be overwritten by the MTPA algorithm. Signed-off-by: Marcos Chaparro --- mcpwm_foc.c | 3 +++ 1 file changed, 3 insertions(+) diff --git a/mcpwm_foc.c b/mcpwm_foc.c index c317926cd..cda70fee1 100644 --- a/mcpwm_foc.c +++ b/mcpwm_foc.c @@ -1465,10 +1465,12 @@ void mcpwm_foc_encoder_detect(float current, bool print, float *offset, float *r float offset_old = motor->m_conf->foc_encoder_offset; float inverted_old = motor->m_conf->foc_encoder_inverted; float ratio_old = motor->m_conf->foc_encoder_ratio; + float ldiff_old = motor->m_conf->foc_motor_ld_lq_diff; motor->m_conf->foc_encoder_offset = 0.0; motor->m_conf->foc_encoder_inverted = false; motor->m_conf->foc_encoder_ratio = 1.0; + motor->m_conf->foc_motor_ld_lq_diff = 0.0; // Find index int cnt = 0; @@ -1654,6 +1656,7 @@ void mcpwm_foc_encoder_detect(float current, bool print, float *offset, float *r motor->m_conf->foc_encoder_inverted = inverted_old; motor->m_conf->foc_encoder_offset = offset_old; motor->m_conf->foc_encoder_ratio = ratio_old; + motor->m_conf->foc_motor_ld_lq_diff = ldiff_old; // Enable timeout timeout_configure(tout, tout_c);