From d8a99fd0f1d065eb75ecdb8b742d1883e46c5b84 Mon Sep 17 00:00:00 2001 From: Benjamin Vedder Date: Fri, 15 Oct 2021 22:25:07 +0200 Subject: [PATCH] Inductance and resistance measurement and scaling fixes, various other fixes --- CHANGELOG | 5 ++++ conf_general.c | 6 ++--- conf_general.h | 2 +- confgenerator.h | 2 +- hwconf/hw_60.c | 2 +- hwconf/hw_60.h | 29 ++++++++++++++++++++ hwconf/hw_es19.h | 24 ++++++++++++++++- hwconf/hw_rd2.h | 18 ++++++------- mcconf/mcconf_default.h | 2 +- mcpwm_foc.c | 59 ++++++++++++++++++++++------------------- terminal.c | 2 +- 11 files changed, 105 insertions(+), 46 deletions(-) diff --git a/CHANGELOG b/CHANGELOG index 17ffb793..76f55650 100644 --- a/CHANGELOG +++ b/CHANGELOG @@ -34,6 +34,11 @@ * Added several AUX port modes. * Added configurable safe start modes. * Added fusion IMU filter. +* Added constant torque PAS mode. +* Correct scaling for resistance and inductance. +* Fixed inductance measurement bug with f_sw > 30k. +* Corrected inductance measurement algorithm. +* Fixed max power loss calculation. === FW 5.02 === * IMU calibration improvement. diff --git a/conf_general.c b/conf_general.c index 11462eb8..90bda596 100644 --- a/conf_general.c +++ b/conf_general.c @@ -1068,7 +1068,7 @@ bool conf_general_measure_flux_linkage_openloop(float current, float duty, float rad_s = rpm_now * ((2.0 * M_PI) / 60.0); float v_mag = sqrtf(SQ(vq_avg) + SQ(vd_avg)); float i_mag = sqrtf(SQ(iq_avg) + SQ(id_avg)); - *linkage = (v_mag - (2.0 / 3.0) * res * i_mag) / rad_s - (2.0 / 3.0) * i_mag * ind; + *linkage = (v_mag - res * i_mag) / rad_s - i_mag * ind; mcconf->foc_motor_r = res; mcconf->foc_motor_l = ind; @@ -1308,7 +1308,7 @@ static bool measure_r_l_imax(float current_min, float current_max, return false; } - if ((i * i * res_tmp) >= (max_power_loss / 3.0)) { + if ((i * i * res_tmp * 2.0) >= (max_power_loss / 5.0)) { break; } } @@ -1319,7 +1319,7 @@ static bool measure_r_l_imax(float current_min, float current_max, mc_interface_set_configuration(mcconf); *l = mcpwm_foc_measure_inductance_current(i_last, 100, 0, 0) * 1e-6; - *i_max = sqrtf(max_power_loss / *r); + *i_max = sqrtf(max_power_loss / *r / 2.0); utils_truncate_number(i_max, HW_LIM_CURRENT); mcconf->foc_motor_r = res_old; diff --git a/conf_general.h b/conf_general.h index 45f0d327..601109e2 100755 --- a/conf_general.h +++ b/conf_general.h @@ -24,7 +24,7 @@ #define FW_VERSION_MAJOR 5 #define FW_VERSION_MINOR 03 // Set to 0 for building a release and iterate during beta test builds -#define FW_TEST_VERSION_NUMBER 51 +#define FW_TEST_VERSION_NUMBER 54 #include "datatypes.h" diff --git a/confgenerator.h b/confgenerator.h index 34a0274f..5b84c75d 100644 --- a/confgenerator.h +++ b/confgenerator.h @@ -8,7 +8,7 @@ #include // Constants -#define MCCONF_SIGNATURE 3706516163 +#define MCCONF_SIGNATURE 2393270318 #define APPCONF_SIGNATURE 763356168 // Functions diff --git a/hwconf/hw_60.c b/hwconf/hw_60.c index eb3fb008..d2243176 100644 --- a/hwconf/hw_60.c +++ b/hwconf/hw_60.c @@ -111,7 +111,7 @@ void hw_init_gpio(void) { palSetPadMode(HW_HALL_ENC_GPIO3, HW_HALL_ENC_PIN3, PAL_MODE_INPUT_PULLUP); // Phase filters -#ifdef HW60_IS_MK5 +#ifdef PHASE_FILTER_GPIO palSetPadMode(PHASE_FILTER_GPIO, PHASE_FILTER_PIN, PAL_MODE_OUTPUT_PUSHPULL | PAL_STM32_OSPEED_HIGHEST); diff --git a/hwconf/hw_60.h b/hwconf/hw_60.h index 4a7fd59c..f606074e 100644 --- a/hwconf/hw_60.h +++ b/hwconf/hw_60.h @@ -137,6 +137,35 @@ #define ADC_IND_SHUTDOWN 10 #endif +// -------- Current sensor test +#if 0 + +#undef ADC_IND_CURR1 +#undef ADC_IND_CURR2 +#undef ADC_IND_CURR3 +#undef CURRENT_FILTER_ON +#undef CURRENT_FILTER_OFF + +#define CURRENT_FILTER_OFF() palClearPad(HW_UART_RX_PORT, HW_UART_RX_PIN) +#define CURRENT_FILTER_ON() palClearPad(HW_UART_RX_PORT, HW_UART_RX_PIN) + +#define ADC_IND_CURR1 6 +#define ADC_IND_CURR2 7 +#define ADC_IND_CURR3 10 + +#define HW_EARLY_INIT() palSetPadMode(HW_UART_TX_PORT, HW_UART_TX_PIN, PAL_MODE_OUTPUT_PUSHPULL); \ + palSetPadMode(HW_UART_RX_PORT, HW_UART_RX_PIN, PAL_MODE_OUTPUT_PUSHPULL); \ + palSetPad(HW_UART_TX_PORT, HW_UART_TX_PIN) + +#define CURRENT_SHUNT_RES 1 +#define CURRENT_AMP_GAIN (2.22e-3 * (4.7 / (4.7 + 2.2))) + +#define APPCONF_APP_TO_USE APP_NONE + +#endif + +// ---------------------------- + // ADC macros and settings // Component parameters (can be overridden) diff --git a/hwconf/hw_es19.h b/hwconf/hw_es19.h index 6ed83a21..36473520 100644 --- a/hwconf/hw_es19.h +++ b/hwconf/hw_es19.h @@ -27,7 +27,7 @@ // HW properties #define HW_HAS_3_SHUNTS #define HW_HAS_PHASE_SHUNTS -#define HW_HAS_PHASE_FILTERS +//#define HW_HAS_PHASE_FILTERS #define HW_HAS_SI8900 // Macros @@ -94,6 +94,28 @@ #define ADC_IND_TEMP_MOTOR 9 #define ADC_IND_VREFINT 12 +// -------- Current sensor test +#if 1 + +#undef ADC_IND_CURR1 +#undef ADC_IND_CURR2 +#undef ADC_IND_CURR3 +#undef CURRENT_FILTER_ON +#undef CURRENT_FILTER_OFF + +#define ADC_IND_CURR1 6 +#define ADC_IND_CURR2 7 +#define ADC_IND_CURR3 10 + +#define CURRENT_SHUNT_RES 1 +#define CURRENT_AMP_GAIN (2.22e-3 * (4.7 / (4.7 + 2.2))) + +#define APPCONF_APP_TO_USE APP_NONE + +#endif + +// ---------------------------- + // ADC macros and settings // Component parameters (can be overridden) diff --git a/hwconf/hw_rd2.h b/hwconf/hw_rd2.h index 5a6e292b..aea03917 100644 --- a/hwconf/hw_rd2.h +++ b/hwconf/hw_rd2.h @@ -255,34 +255,34 @@ #define MCCONF_L_MAX_ABS_CURRENT 480.0 #endif #ifndef MCCONF_L_RPM_MAX -#define MCCONF_L_RPM_MAX 32000.0 +#define MCCONF_L_RPM_MAX 30000.0 #endif #ifndef MCCONF_L_RPM_MIN -#define MCCONF_L_RPM_MIN -32000.0 +#define MCCONF_L_RPM_MIN -30000.0 #endif #ifndef MCCONF_L_RPM_START -#define MCCONF_L_RPM_START 0.8 +#define MCCONF_L_RPM_START 0.9 #endif #ifndef MCCONF_FOC_CURRENT_KP -#define MCCONF_FOC_CURRENT_KP 0.0046 +#define MCCONF_FOC_CURRENT_KP 0.01 #endif #ifndef MCCONF_FOC_CURRENT_KI -#define MCCONF_FOC_CURRENT_KI 8.0 +#define MCCONF_FOC_CURRENT_KI 6.0 #endif #ifndef MCCONF_FOC_F_SW #define MCCONF_FOC_F_SW 30000.0 #endif #ifndef MCCONF_FOC_MOTOR_L -#define MCCONF_FOC_MOTOR_L 2.32e-6 +#define MCCONF_FOC_MOTOR_L 5.0e-6 #endif #ifndef MCCONF_FOC_MOTOR_R -#define MCCONF_FOC_MOTOR_R 0.004 +#define MCCONF_FOC_MOTOR_R 0.0024 #endif #ifndef MCCONF_FOC_MOTOR_FLUX_LINKAGE -#define MCCONF_FOC_MOTOR_FLUX_LINKAGE 0.00805 +#define MCCONF_FOC_MOTOR_FLUX_LINKAGE 0.00796 #endif #ifndef MCCONF_FOC_OBSERVER_GAIN -#define MCCONF_FOC_OBSERVER_GAIN 15.0e6 +#define MCCONF_FOC_OBSERVER_GAIN 6.0e6 #endif // Setup Info diff --git a/mcconf/mcconf_default.h b/mcconf/mcconf_default.h index 38b052be..eda3a74b 100644 --- a/mcconf/mcconf_default.h +++ b/mcconf/mcconf_default.h @@ -72,7 +72,7 @@ #define MCCONF_L_RPM_START 0.8 // Fraction of full speed where RPM current limiting starts #endif #ifndef MCCONF_L_SLOW_ABS_OVERCURRENT -#define MCCONF_L_SLOW_ABS_OVERCURRENT true // Use the filtered (and hence slower) current for the overcurrent fault detection +#define MCCONF_L_SLOW_ABS_OVERCURRENT false // Use the filtered (and hence slower) current for the overcurrent fault detection #endif #ifndef MCCONF_L_MIN_DUTY #define MCCONF_L_MIN_DUTY 0.005 // Minimum duty cycle diff --git a/mcpwm_foc.c b/mcpwm_foc.c index aa51a081..3858fe5f 100644 --- a/mcpwm_foc.c +++ b/mcpwm_foc.c @@ -1784,7 +1784,7 @@ float mcpwm_foc_measure_resistance(float current, int samples, bool stop_after) timeout_configure(tout, tout_c, tout_ksw); mc_interface_unlock(); - return (voltage_avg / current_avg) * (2.0 / 3.0); + return voltage_avg / current_avg; } /** @@ -1821,15 +1821,19 @@ float mcpwm_foc_measure_inductance(float duty, int samples, float *curr, float * stop_pwm_hw(motor); motor->m_conf->foc_sensor_mode = FOC_SENSOR_MODE_HFI; - motor->m_conf->foc_hfi_voltage_start = duty * mc_interface_get_input_voltage_filtered() * (2.0 / 3.0); - motor->m_conf->foc_hfi_voltage_run = duty * mc_interface_get_input_voltage_filtered() * (2.0 / 3.0); - motor->m_conf->foc_hfi_voltage_max = duty * mc_interface_get_input_voltage_filtered() * (2.0 / 3.0); + motor->m_conf->foc_hfi_voltage_start = duty * mc_interface_get_input_voltage_filtered(); + motor->m_conf->foc_hfi_voltage_run = duty * mc_interface_get_input_voltage_filtered(); + motor->m_conf->foc_hfi_voltage_max = duty * mc_interface_get_input_voltage_filtered(); motor->m_conf->foc_sl_erpm_hfi = 20000.0; motor->m_conf->foc_sample_v0_v7 = false; motor->m_conf->foc_hfi_samples = HFI_SAMPLES_32; motor->m_conf->foc_sample_high_current = false; - update_hfi_samples(motor->m_conf->foc_hfi_samples, motor); + if (motor->m_conf->foc_f_sw > 30.0e3) { + motor->m_conf->foc_f_sw = 30.0e3; + } + + mcpwm_foc_set_configuration(motor->m_conf); chThdSleepMilliseconds(1); @@ -1873,7 +1877,7 @@ float mcpwm_foc_measure_inductance(float duty, int samples, float *curr, float * motor->m_conf->foc_hfi_samples = samples_old; motor->m_conf->foc_sample_high_current = sample_high_current_old; - update_hfi_samples(motor->m_conf->foc_hfi_samples, motor); + mcpwm_foc_set_configuration(motor->m_conf); mc_interface_unlock(); @@ -1893,7 +1897,7 @@ float mcpwm_foc_measure_inductance(float duty, int samples, float *curr, float * motor->m_hfi.fft_bin0_func((float*)motor->m_hfi.buffer_current, &real_bin0_i, &imag_bin0_i); l_sum += real_bin0; - ld_lq_diff_sum += 2.0 * sqrtf(SQ(real_bin2) + SQ(imag_bin2)); + ld_lq_diff_sum += 4.0 * sqrtf(SQ(real_bin2) + SQ(imag_bin2)); i_sum += real_bin0_i; iterations++; @@ -1911,7 +1915,7 @@ float mcpwm_foc_measure_inductance(float duty, int samples, float *curr, float * motor->m_conf->foc_hfi_samples = samples_old; motor->m_conf->foc_sample_high_current = sample_high_current_old; - update_hfi_samples(motor->m_conf->foc_hfi_samples, motor); + mcpwm_foc_set_configuration(motor->m_conf); mc_interface_unlock(); @@ -1920,10 +1924,10 @@ float mcpwm_foc_measure_inductance(float duty, int samples, float *curr, float * } if (ld_lq_diff) { - *ld_lq_diff = (ld_lq_diff_sum / iterations) * 1e6 * (2.0 / 3.0); + *ld_lq_diff = (ld_lq_diff_sum / iterations) * 1e6; } - return (l_sum / iterations) * 1e6 * (2.0 / 3.0); + return (l_sum / iterations) * 1e6; } /** @@ -3541,8 +3545,8 @@ void observer_update(float v_alpha, float v_beta, float i_alpha, float i_beta, volatile mc_configuration *conf_now = motor->m_conf; - const float L = (3.0 / 2.0) * conf_now->foc_motor_l; - float R = (3.0 / 2.0) * conf_now->foc_motor_r; + const float L = conf_now->foc_motor_l; + float R = conf_now->foc_motor_r; // Saturation compensation const float sign = (motor->m_motor_state.iq * motor->m_motor_state.vq) >= 0.0 ? 1.0 : -1.0; @@ -3722,8 +3726,8 @@ static void control_current(volatile motor_all_state_t *motor, float dt) { switch (conf_now->foc_cc_decoupling) { case FOC_CC_DECOUPLING_CROSS: - dec_vd = state_m->iq_filter * motor->m_speed_est_fast * lq * (3.0 / 2.0); - dec_vq = state_m->id_filter * motor->m_speed_est_fast * ld * (3.0 / 2.0); + dec_vd = state_m->iq_filter * motor->m_speed_est_fast * lq; + dec_vq = state_m->id_filter * motor->m_speed_est_fast * ld; break; case FOC_CC_DECOUPLING_BEMF: @@ -3731,8 +3735,8 @@ static void control_current(volatile motor_all_state_t *motor, float dt) { break; case FOC_CC_DECOUPLING_CROSS_BEMF: - dec_vd = state_m->iq_filter * motor->m_speed_est_fast * lq * (3.0 / 2.0); - dec_vq = state_m->id_filter * motor->m_speed_est_fast * ld * (3.0 / 2.0); + dec_vd = state_m->iq_filter * motor->m_speed_est_fast * lq; + dec_vq = state_m->id_filter * motor->m_speed_est_fast * ld; dec_bemf = motor->m_speed_est_fast * conf_now->foc_motor_flux_linkage; break; @@ -3794,18 +3798,17 @@ static void control_current(volatile motor_all_state_t *motor, float dt) { conf_now->foc_hfi_voltage_run, conf_now->foc_hfi_voltage_max); } - utils_truncate_number_abs(&hfi_voltage, state_m->v_bus * (2.0 / 3.0) * 0.9); + utils_truncate_number_abs(&hfi_voltage, state_m->v_bus * 0.9); if (motor->m_hfi.is_samp_n) { float sample_now = (utils_tab_sin_32_1[motor->m_hfi.ind * motor->m_hfi.table_fact] * state_m->i_alpha - utils_tab_cos_32_1[motor->m_hfi.ind * motor->m_hfi.table_fact] * state_m->i_beta); - float current_sample = sample_now - motor->m_hfi.prev_sample; + float di = (sample_now - motor->m_hfi.prev_sample); - motor->m_hfi.buffer_current[motor->m_hfi.ind] = current_sample; + motor->m_hfi.buffer_current[motor->m_hfi.ind] = di; - if (current_sample > 0.01) { - motor->m_hfi.buffer[motor->m_hfi.ind] = ((hfi_voltage / 2.0 - conf_now->foc_motor_r * - current_sample) / (conf_now->foc_f_sw * current_sample)); + if (di > 0.01) { + motor->m_hfi.buffer[motor->m_hfi.ind] = ((hfi_voltage / 2.0 - conf_now->foc_motor_r * di) / (conf_now->foc_f_sw * di)); } motor->m_hfi.ind++; @@ -3814,14 +3817,14 @@ static void control_current(volatile motor_all_state_t *motor, float dt) { motor->m_hfi.ready = true; } - mod_alpha_tmp += hfi_voltage * utils_tab_sin_32_1[motor->m_hfi.ind * motor->m_hfi.table_fact] / ((2.0 / 3.0) * state_m->v_bus); - mod_beta_tmp -= hfi_voltage * utils_tab_cos_32_1[motor->m_hfi.ind * motor->m_hfi.table_fact] / ((2.0 / 3.0) * state_m->v_bus); + mod_alpha_tmp += hfi_voltage * utils_tab_sin_32_1[motor->m_hfi.ind * motor->m_hfi.table_fact] / state_m->v_bus; + mod_beta_tmp -= hfi_voltage * utils_tab_cos_32_1[motor->m_hfi.ind * motor->m_hfi.table_fact] / state_m->v_bus; } else { motor->m_hfi.prev_sample = utils_tab_sin_32_1[motor->m_hfi.ind * motor->m_hfi.table_fact] * state_m->i_alpha - utils_tab_cos_32_1[motor->m_hfi.ind * motor->m_hfi.table_fact] * state_m->i_beta; - mod_alpha_tmp -= hfi_voltage * utils_tab_sin_32_1[motor->m_hfi.ind * motor->m_hfi.table_fact] / ((2.0 / 3.0) * state_m->v_bus); - mod_beta_tmp += hfi_voltage * utils_tab_cos_32_1[motor->m_hfi.ind * motor->m_hfi.table_fact] / ((2.0 / 3.0) * state_m->v_bus); + mod_alpha_tmp -= hfi_voltage * utils_tab_sin_32_1[motor->m_hfi.ind * motor->m_hfi.table_fact] / state_m->v_bus; + mod_beta_tmp += hfi_voltage * utils_tab_cos_32_1[motor->m_hfi.ind * motor->m_hfi.table_fact] / state_m->v_bus; } utils_saturate_vector_2d(&mod_alpha_tmp, &mod_beta_tmp, SQRT3_BY_2 * 0.95); @@ -4597,8 +4600,8 @@ static void terminal_tmp(int argc, const char **argv) { // float linkage = conf_now->foc_motor_flux_linkage; float linkage = sqrtf(SQ(m_motor_1.m_observer_x1) + SQ(m_motor_1.m_observer_x2)); - rpm_est += (motor_state->vq - (3.0 / 2.0) * R * motor_state->iq) / linkage; - res_est += -(motor_state->speed_rad_s * linkage - motor_state->vq) / (motor_state->iq * (3.0 / 2.0)); + rpm_est += (motor_state->vq - R * motor_state->iq) / linkage; + res_est += -(motor_state->speed_rad_s * linkage - motor_state->vq) / motor_state->iq; samples += 1.0; chThdSleep(1); diff --git a/terminal.c b/terminal.c index 3fc227a3..537dd903 100644 --- a/terminal.c +++ b/terminal.c @@ -1201,7 +1201,7 @@ void terminal_process_string(char *str) { commands_printf("measure_linkage_openloop [current] [duty] [erpm_per_sec] [motor_res] [motor_ind]"); commands_printf(" Run the motor in openloop FOC and measure the flux linkage"); - commands_printf(" example measure_linkage 5 0.5 1000 0.076 0.000015"); + commands_printf(" example measure_linkage_openloop 5 0.5 1000 0.076 0.000015"); commands_printf(" tip: measure the resistance with measure_res first"); commands_printf("foc_state");