Improve fault handling during detection routines

Added more checks for faults during detection routines.
Always return an error if a fault occured.
Handing for returned errors.

conf_general.c

@@ -1012,7 +1012,13 @@ bool conf_general_measure_flux_linkage_openloop(float current, float duty,
 	while (fabsf(mc_interface_get_duty_cycle_now()) < duty) {
 		rpm_now += erpm_per_sec / 1000.0;
 		mcpwm_foc_set_openloop(current, mcconf->m_invert_direction ? -rpm_now : rpm_now);
-
+
+		if (mc_interface_get_fault() != FAULT_CODE_NONE) {
+			cnt = max_time;
+			*linkage = -4.0;
+			break;
+		}
+
 		chThdSleepMilliseconds(1);
 		cnt++;
 
@@ -1223,7 +1229,10 @@ int conf_general_autodetect_apply_sensors_foc(float current,
 
 		for (int i = 0;i < 1000;i++) {
 			mcpwm_foc_set_openloop_phase((float)i * current / 1000.0, 0.0);
-			chThdSleepMilliseconds(1);
+			if (mc_interface_get_fault() != FAULT_CODE_NONE) {
+				break;
+			}
+			chThdSleepMilliseconds(1);			
 		}
 
 		float phase_start = encoder_read_deg();
@@ -1232,6 +1241,10 @@ int conf_general_autodetect_apply_sensors_foc(float current,
 
 		for (int i = 0;i < 180.0;i++) {
 			mcpwm_foc_set_openloop_phase(current, i);
+			if (mc_interface_get_fault() != FAULT_CODE_NONE) {
+				break;
+			} 
+
 			chThdSleepMilliseconds(5);
 
 			if (i == 90) {
@@ -1323,7 +1336,7 @@ static bool measure_r_l_imax(float current_min, float current_max,
 		float res_tmp = mcpwm_foc_measure_resistance(i, 5, false);
 		i_last = i;
 
-		if (mc_interface_get_fault() != FAULT_CODE_NONE) {
+		if (res_tmp == 0.0) {
 			mempools_free_mcconf(mcconf);
 			return false;
 		}
@@ -1334,11 +1347,20 @@ static bool measure_r_l_imax(float current_min, float current_max,
 	}
 
 	*r = mcpwm_foc_measure_resistance(i_last, 100, true);
-
+	if (*r == 0.0) {
+		mempools_free_mcconf(mcconf);
+		return false;
+	}
+
 	mcconf->foc_motor_r = *r;
-	mc_interface_set_configuration(mcconf);
-
+	mc_interface_set_configuration(mcconf);	
+
+	bool result = true;
 	*l = mcpwm_foc_measure_inductance_current(i_last, 100, 0, ld_lq_diff) * 1e-6;
+	if (*l == 0.0) {
+		result = false;
+	}
+
 	*ld_lq_diff *= 1e-6;
 	*i_max = sqrtf(max_power_loss / *r / 1.5);
 	utils_truncate_number(i_max, HW_LIM_CURRENT);
@@ -1347,7 +1369,7 @@ static bool measure_r_l_imax(float current_min, float current_max,
 	mc_interface_set_configuration(mcconf);
 	mempools_free_mcconf(mcconf);
 
-	return true;
+	return result;
 }
 
 static bool wait_fault(int timeout_ms) {

mcpwm_foc.c

@@ -1633,7 +1633,20 @@ float mcpwm_foc_measure_resistance(float current, int samples, bool stop_after)
 
 	// Ramp up the current slowly
 	while (fabsf(motor->m_iq_set - current) > 0.001) {
-		utils_step_towards((float*)&motor->m_iq_set, current, fabsf(current) / 500.0);
+		utils_step_towards((float*)&motor->m_iq_set, current, fabsf(current) / 500.0);		
+		if (mc_interface_get_fault() != FAULT_CODE_NONE) {
+			motor->m_id_set = 0.0;
+			motor->m_iq_set = 0.0;
+			motor->m_phase_override = false;
+			motor->m_control_mode = CONTROL_MODE_NONE;
+			motor->m_state = MC_STATE_OFF;
+			stop_pwm_hw((motor_all_state_t*)motor);
+
+			timeout_configure(tout, tout_c, tout_ksw);
+			mc_interface_unlock();
+
+			return 0.0;
+		}
 		chThdSleepMilliseconds(1);
 	}
 
@@ -1863,7 +1876,9 @@ float mcpwm_foc_measure_inductance_current(float curr_goal, int samples, float *
 	float duty_last = 0.0;
 	for (float i = 0.02;i < 0.5;i *= 1.5) {
 		float i_tmp;
-		mcpwm_foc_measure_inductance(i, 10, &i_tmp, 0);
+		if (mcpwm_foc_measure_inductance(i, 10, &i_tmp, 0) == 0.0) {
+			return 0.0;
+		}
 
 		duty_last = i;
 		if (i_tmp >= curr_goal) {
@@ -1892,8 +1907,8 @@ float mcpwm_foc_measure_inductance_current(float curr_goal, int samples, float *
  */
 bool mcpwm_foc_measure_res_ind(float *res, float *ind, float *ld_lq_diff) {
 	volatile motor_all_state_t *motor = get_motor_now();
-
-	const float f_zv_old = motor->m_conf->foc_f_zv;
+	bool result = false;
+
 	const float kp_old = motor->m_conf->foc_current_kp;
 	const float ki_old = motor->m_conf->foc_current_ki;
 	const float res_old = motor->m_conf->foc_motor_r;
@@ -1903,7 +1918,13 @@ bool mcpwm_foc_measure_res_ind(float *res, float *ind, float *ld_lq_diff) {
 
 	float i_last = 0.0;
 	for (float i = 2.0;i < (motor->m_conf->l_current_max / 2.0);i *= 1.5) {
-		if (i > (1.0 / mcpwm_foc_measure_resistance(i, 20, false))) {
+		float r_tmp = mcpwm_foc_measure_resistance(i, 20, false);
+		if (r_tmp == 0.0) {
+			motor->m_conf->foc_current_kp = kp_old;
+			motor->m_conf->foc_current_ki = ki_old;
+			return false;
+		}
+		if (i > (1.0 / r_tmp)) {
 			i_last = i;
 			break;
 		}
@@ -1918,15 +1939,19 @@ bool mcpwm_foc_measure_res_ind(float *res, float *ind, float *ld_lq_diff) {
 #endif
 
 	*res = mcpwm_foc_measure_resistance(i_last, 200, true);
-	motor->m_conf->foc_motor_r = *res;
-	*ind = mcpwm_foc_measure_inductance_current(i_last, 200, 0, ld_lq_diff);
-
-	motor->m_conf->foc_f_zv = f_zv_old;
+	if (*res != 0.0) {
+		motor->m_conf->foc_motor_r = *res;
+		*ind = mcpwm_foc_measure_inductance_current(i_last, 200, 0, ld_lq_diff);
+		if (*ind != 0.0) {
+			result = true;
+		}	
+	}
+
 	motor->m_conf->foc_current_kp = kp_old;
 	motor->m_conf->foc_current_ki = ki_old;
 	motor->m_conf->foc_motor_r = res_old;
 
-	return true;
+	return result;
 }
 
 /**

terminal.c

@@ -464,9 +464,13 @@ void terminal_process_string(char *str) {
 		float res = 0.0;
 		float ind = 0.0;
 		float ld_lq_diff = 0.0;
-		mcpwm_foc_measure_res_ind(&res, &ind, &ld_lq_diff);
-		commands_printf("Resistance: %.6f ohm", (double)res);
-		commands_printf("Inductance: %.2f uH (Lq-Ld: %.2f uH)\n", (double)ind, (double)ld_lq_diff);
+		if (mcpwm_foc_measure_res_ind(&res, &ind, &ld_lq_diff)) {
+			commands_printf("Resistance: %.6f ohm", (double)res);
+			commands_printf("Inductance: %.2f uH (Lq-Ld: %.2f uH)\n", (double)ind, (double)ld_lq_diff);
+		} else {
+			commands_printf("Error measuring resistance or inductance\n");
+		}
+
 
 		mc_interface_set_configuration(mcconf_old);