Sensor port init in only one place, better encoder debug info

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"
 

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
+	init_sensor_port(&motor_now()->m_conf);
-	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;
-	}
 
 	// 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) {
+		init_sensor_port(configuration);
-		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;
-		}
 	}
 
 	if (configuration->m_sensor_port_mode == SENSOR_PORT_MODE_ABI) {

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));
 		}
 	}
 

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");