From 015a657cc960f1c44da87f11a134fdd4ff21076c Mon Sep 17 00:00:00 2001 From: Benjamin Vedder Date: Tue, 25 Jan 2022 13:57:25 +0100 Subject: [PATCH] Sensor port init in only one place, better encoder debug info --- conf_general.h | 2 +- mc_interface.c | 143 ++++++++++++++++++------------------------------- mcpwm_foc.c | 15 ++++-- terminal.c | 4 ++ 4 files changed, 68 insertions(+), 96 deletions(-) diff --git a/conf_general.h b/conf_general.h index 2b2e852c..07212c61 100755 --- a/conf_general.h +++ b/conf_general.h @@ -24,7 +24,7 @@ #define FW_VERSION_MAJOR 6 #define FW_VERSION_MINOR 00 // Set to 0 for building a release and iterate during beta test builds -#define FW_TEST_VERSION_NUMBER 3 +#define FW_TEST_VERSION_NUMBER 4 #include "datatypes.h" diff --git a/mc_interface.c b/mc_interface.c index 81094e8a..946e5f3d 100644 --- a/mc_interface.c +++ b/mc_interface.c @@ -143,6 +143,57 @@ static thread_t *fault_stop_tp; static THD_WORKING_AREA(stat_thread_wa, 512); static THD_FUNCTION(stat_thread, arg); +static void init_sensor_port(volatile mc_configuration *conf) { + switch (conf->m_sensor_port_mode) { + case SENSOR_PORT_MODE_ABI: + SENSOR_PORT_5V(); + encoder_init_abi(conf->m_encoder_counts); + break; + + case SENSOR_PORT_MODE_AS5047_SPI: + SENSOR_PORT_3V3(); + encoder_init_as5047p_spi(); + break; + + case SENSOR_PORT_MODE_MT6816_SPI: + SENSOR_PORT_5V(); + encoder_init_mt6816_spi(); + break; + + case SENSOR_PORT_MODE_AD2S1205: + SENSOR_PORT_5V(); + encoder_init_ad2s1205_spi(); + break; + + case SENSOR_PORT_MODE_SINCOS: + SENSOR_PORT_5V(); + encoder_init_sincos(conf->foc_encoder_sin_gain, conf->foc_encoder_sin_offset, + conf->foc_encoder_cos_gain, conf->foc_encoder_cos_offset, + conf->foc_encoder_sincos_filter_constant); + break; + + case SENSOR_PORT_MODE_TS5700N8501: + case SENSOR_PORT_MODE_TS5700N8501_MULTITURN: { + SENSOR_PORT_5V(); + app_configuration *appconf = mempools_alloc_appconf(); + conf_general_read_app_configuration(appconf); + if (appconf->app_to_use == APP_ADC || + appconf->app_to_use == APP_UART || + appconf->app_to_use == APP_PPM_UART || + appconf->app_to_use == APP_ADC_UART) { + appconf->app_to_use = APP_NONE; + conf_general_store_app_configuration(appconf); + } + mempools_free_appconf(appconf); + encoder_init_ts5700n8501(); + } break; + + default: + SENSOR_PORT_5V(); + break; + } +} + void mc_interface_init(void) { memset((void*)&m_motor_1, 0, sizeof(motor_if_state_t)); #ifdef HW_HAS_DUAL_MOTORS @@ -207,51 +258,7 @@ void mc_interface_init(void) { #endif mc_interface_select_motor_thread(motor_old); - // Initialize encoder - switch (motor_now()->m_conf.m_sensor_port_mode) { - case SENSOR_PORT_MODE_ABI: - SENSOR_PORT_3V3(); - encoder_init_abi(motor_now()->m_conf.m_encoder_counts); - break; - - case SENSOR_PORT_MODE_AS5047_SPI: - SENSOR_PORT_3V3(); - encoder_init_as5047p_spi(); - break; - - case SENSOR_PORT_MODE_MT6816_SPI: - encoder_init_mt6816_spi(); - break; - - case SENSOR_PORT_MODE_AD2S1205: - encoder_init_ad2s1205_spi(); - break; - - case SENSOR_PORT_MODE_SINCOS: - encoder_init_sincos(motor_now()->m_conf.foc_encoder_sin_gain, motor_now()->m_conf.foc_encoder_sin_offset, - motor_now()->m_conf.foc_encoder_cos_gain, motor_now()->m_conf.foc_encoder_cos_offset, - motor_now()->m_conf.foc_encoder_sincos_filter_constant); - break; - - case SENSOR_PORT_MODE_TS5700N8501: - case SENSOR_PORT_MODE_TS5700N8501_MULTITURN: { - app_configuration *appconf = mempools_alloc_appconf(); - conf_general_read_app_configuration(appconf); - if (appconf->app_to_use == APP_ADC || - appconf->app_to_use == APP_UART || - appconf->app_to_use == APP_PPM_UART || - appconf->app_to_use == APP_ADC_UART) { - appconf->app_to_use = APP_NONE; - conf_general_store_app_configuration(appconf); - } - mempools_free_appconf(appconf); - encoder_init_ts5700n8501(); - } break; - - default: - SENSOR_PORT_5V(); - break; - } + init_sensor_port(&motor_now()->m_conf); // Initialize selected implementation switch (motor_now()->m_conf.motor_type) { @@ -341,51 +348,7 @@ void mc_interface_set_configuration(mc_configuration *configuration) { if (motor->m_conf.m_sensor_port_mode != configuration->m_sensor_port_mode) { encoder_deinit(); - switch (configuration->m_sensor_port_mode) { - case SENSOR_PORT_MODE_ABI: - SENSOR_PORT_3V3(); - encoder_init_abi(configuration->m_encoder_counts); - break; - - case SENSOR_PORT_MODE_AS5047_SPI: - SENSOR_PORT_3V3(); - encoder_init_as5047p_spi(); - break; - - case SENSOR_PORT_MODE_MT6816_SPI: - encoder_init_mt6816_spi(); - break; - - case SENSOR_PORT_MODE_AD2S1205: - encoder_init_ad2s1205_spi(); - break; - - case SENSOR_PORT_MODE_SINCOS: - encoder_init_sincos(configuration->foc_encoder_sin_gain, configuration->foc_encoder_sin_offset, - configuration->foc_encoder_cos_gain, configuration->foc_encoder_cos_offset, - configuration->foc_encoder_sincos_filter_constant); - break; - - case SENSOR_PORT_MODE_TS5700N8501: - case SENSOR_PORT_MODE_TS5700N8501_MULTITURN: { - app_configuration *appconf = mempools_alloc_appconf(); - *appconf = *app_get_configuration(); - if (appconf->app_to_use == APP_ADC || - appconf->app_to_use == APP_UART || - appconf->app_to_use == APP_PPM_UART || - appconf->app_to_use == APP_ADC_UART) { - appconf->app_to_use = APP_NONE; - conf_general_store_app_configuration(appconf); - app_set_configuration(appconf); - } - mempools_free_appconf(appconf); - encoder_init_ts5700n8501(); - } break; - - default: - SENSOR_PORT_5V(); - break; - } + init_sensor_port(configuration); } if (configuration->m_sensor_port_mode == SENSOR_PORT_MODE_ABI) { diff --git a/mcpwm_foc.c b/mcpwm_foc.c index a867d9a6..fe429093 100644 --- a/mcpwm_foc.c +++ b/mcpwm_foc.c @@ -1564,7 +1564,7 @@ void mcpwm_foc_encoder_detect(float current, bool print, float *offset, float *r // Inverted and ratio chThdSleepMilliseconds(1000); - const int it_rat = 20; + const int it_rat = 30; float s_sum = 0.0; float c_sum = 0.0; float first = motor->m_phase_now_encoder; @@ -1579,6 +1579,7 @@ void mcpwm_foc_encoder_detect(float current, bool print, float *offset, float *r } utils_norm_angle_rad((float*)&motor->m_phase_now_override); chThdSleepMilliseconds(300); + timeout_reset(); float diff = utils_angle_difference_rad(motor->m_phase_now_encoder, phase_old); float s, c; @@ -1587,7 +1588,7 @@ void mcpwm_foc_encoder_detect(float current, bool print, float *offset, float *r c_sum += c; if (print) { - commands_printf("%.2f", (double)RAD2DEG_f(diff)); + commands_printf("Diff: %.2f", (double)RAD2DEG_f(diff)); } if (i > 3 && fabsf(utils_angle_difference_rad(motor->m_phase_now_encoder, first)) < fabsf(diff / 2.0)) { @@ -1607,6 +1608,7 @@ void mcpwm_foc_encoder_detect(float current, bool print, float *offset, float *r } utils_norm_angle_rad((float*)&motor->m_phase_now_override); chThdSleepMilliseconds(300); + timeout_reset(); float diff = utils_angle_difference_rad(phase_old, motor->m_phase_now_encoder); float s, c; @@ -1615,7 +1617,7 @@ void mcpwm_foc_encoder_detect(float current, bool print, float *offset, float *r c_sum += c; if (print) { - commands_printf("%.2f", (double)RAD2DEG_f(diff)); + commands_printf("Diff: %.2f", (double)RAD2DEG_f(diff)); } if (i > 3 && fabsf(utils_angle_difference_rad(motor->m_phase_now_encoder, first)) < fabsf(diff / 2.0)) { @@ -1632,6 +1634,7 @@ void mcpwm_foc_encoder_detect(float current, bool print, float *offset, float *r if (print) { commands_printf("Inversion and ratio detected"); + commands_printf("Ratio: %.2f", (double)*ratio); } // Rotate @@ -1659,6 +1662,7 @@ void mcpwm_foc_encoder_detect(float current, bool print, float *offset, float *r } chThdSleepMilliseconds(100); + timeout_reset(); float angle_diff = utils_angle_difference_rad(motor->m_phase_now_encoder, motor->m_phase_now_override); float s, c; @@ -1667,7 +1671,7 @@ void mcpwm_foc_encoder_detect(float current, bool print, float *offset, float *r c_sum += c; if (print) { - commands_printf("%.2f", (double)RAD2DEG_f(angle_diff)); + commands_printf("Ovr: %.2f/%.2f Diff: %.2f", (double)override, (double)(it_ofs * step), (double)RAD2DEG_f(angle_diff)); } } @@ -1681,6 +1685,7 @@ void mcpwm_foc_encoder_detect(float current, bool print, float *offset, float *r } chThdSleepMilliseconds(100); + timeout_reset(); float angle_diff = utils_angle_difference_rad(motor->m_phase_now_encoder, motor->m_phase_now_override); float s, c; @@ -1689,7 +1694,7 @@ void mcpwm_foc_encoder_detect(float current, bool print, float *offset, float *r c_sum += c; if (print) { - commands_printf("%.2f", (double)RAD2DEG_f(angle_diff)); + commands_printf("Ovr: %.2f/%.2f Diff: %.2f", (double)override, (double)(it_ofs * step), (double)RAD2DEG_f(angle_diff)); } } diff --git a/terminal.c b/terminal.c index 0fc4aa05..7823fd61 100644 --- a/terminal.c +++ b/terminal.c @@ -935,6 +935,10 @@ void terminal_process_string(char *str) { encoder_resolver_loss_of_signal_error_cnt(), (double)encoder_resolver_loss_of_signal_error_rate() * (double)100.0); } + + if (mcconf->m_sensor_port_mode == SENSOR_PORT_MODE_ABI) { + commands_printf("Index found: %d\n", encoder_index_found()); + } } else if (strcmp(argv[0], "encoder_clear_errors") == 0) { encoder_ts57n8501_reset_errors(); commands_printf("Done!\n");