mirror of https://github.com/rusefi/bldc.git
Sensor port init in only one place, better encoder debug info
This commit is contained in:
parent
5cecd8f8bb
commit
015a657cc9
|
@ -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"
|
||||
|
||||
|
|
143
mc_interface.c
143
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) {
|
||||
|
|
15
mcpwm_foc.c
15
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));
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
@ -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");
|
||||
|
|
Loading…
Reference in New Issue