diff --git a/conf_general.c b/conf_general.c
index 3d738d649c..028b908200 100644
--- a/conf_general.c
+++ b/conf_general.c
@@ -1472,10 +1472,12 @@ static int measure_r_l_imax(float current_min, float current_max,
 	const float res_old = mcconf->foc_motor_r;
 
 	float i_last = 0.0;
+	bool first_run = true;
 	for (float i = current_start;i < current_max;i *= 1.5) {
 		float res_tmp = 0.0;
-		fault = mcpwm_foc_measure_resistance(i, 5, false, &res_tmp);
+		fault = mcpwm_foc_measure_resistance(i, 5, false, &res_tmp, first_run);
 		i_last = i;
+		first_run = false;
 
 		if (fault != FAULT_CODE_NONE) {
 			mempools_free_mcconf(mcconf);
@@ -1487,7 +1489,7 @@ static int measure_r_l_imax(float current_min, float current_max,
 		}
 	}
 
-	fault = mcpwm_foc_measure_resistance(i_last, 100, true, r);
+	fault = mcpwm_foc_measure_resistance(i_last, 100, true, r, false);
 	if (fault != FAULT_CODE_NONE) {
 		mempools_free_mcconf(mcconf);
 		return fault;
diff --git a/datatypes.h b/datatypes.h
index 6ce73d4315..4ee8064887 100644
--- a/datatypes.h
+++ b/datatypes.h
@@ -170,6 +170,7 @@ typedef enum {
 	FAULT_CODE_PHASE_FILTER,
 	FAULT_CODE_ENCODER_FAULT,
 	FAULT_CODE_LV_OUTPUT_FAULT,
+	FAULT_CODE_PHASE_OUTPUT_ERROR,
 } mc_fault_code;
 
 typedef enum {
diff --git a/hwconf/teamtriforceuk/a50s_v23c/hw_a50s_v23c_core.h b/hwconf/teamtriforceuk/a50s_v23c/hw_a50s_v23c_core.h
index 225ec00152..9ef8f0063f 100644
--- a/hwconf/teamtriforceuk/a50s_v23c/hw_a50s_v23c_core.h
+++ b/hwconf/teamtriforceuk/a50s_v23c/hw_a50s_v23c_core.h
@@ -260,12 +260,6 @@
 #define MCCONF_SL_MIN_ERPM_CYCLE_INT_LIMIT	4000.0	// Minimum RPM to calculate the BEMF coupling from
 #endif
 
-// Don't call on boot, cal during motor config instead.
-// This significatly speeds up boot time, which is important for combat robots
-#ifndef MCCONF_FOC_OFFSETS_CAL_ON_BOOT
-#define MCCONF_FOC_OFFSETS_CAL_ON_BOOT	false
-#endif
-
 // Setting limits
 #define HW_LIM_CURRENT				-80.0, 80.0 
 #define HW_LIM_CURRENT_IN			-40.0, 40.0
diff --git a/lispBM/c_libs/vesc_c_if.h b/lispBM/c_libs/vesc_c_if.h
index 67224bba90..8a5f31e040 100755
--- a/lispBM/c_libs/vesc_c_if.h
+++ b/lispBM/c_libs/vesc_c_if.h
@@ -109,6 +109,9 @@ typedef enum {
 	FAULT_CODE_ENCODER_NO_MAGNET,
 	FAULT_CODE_ENCODER_MAGNET_TOO_STRONG,
 	FAULT_CODE_PHASE_FILTER,
+	FAULT_CODE_ENCODER_FAULT,
+	FAULT_CODE_LV_OUTPUT_FAULT,
+	FAULT_CODE_PHASE_OUTPUT_ERROR,
 } mc_fault_code;
 
 typedef union {
diff --git a/lispBM/lispif_vesc_extensions.c b/lispBM/lispif_vesc_extensions.c
index 5ab652ab3b..d9f46b301c 100644
--- a/lispBM/lispif_vesc_extensions.c
+++ b/lispBM/lispif_vesc_extensions.c
@@ -3712,7 +3712,7 @@ static void measure_res_task(void *arg) {
 	if (lbm_start_flatten(&v, 10)) {
 		float res = -1.0;
 		mc_interface_select_motor_thread(a->motor);
-		mcpwm_foc_measure_resistance(a->current, a->samples, true, &res);
+		mcpwm_foc_measure_resistance(a->current, a->samples, true, &res, true);
 		mc_interface_select_motor_thread(1);
 
 		if (restart_cnt != lispif_get_restart_cnt()) {
diff --git a/motor/mc_interface.c b/motor/mc_interface.c
index 635ecd609e..9f900d808f 100644
--- a/motor/mc_interface.c
+++ b/motor/mc_interface.c
@@ -513,6 +513,7 @@ const char* mc_interface_fault_to_string(mc_fault_code fault) {
     case FAULT_CODE_PHASE_FILTER: return "FAULT_CODE_PHASE_FILTER";
     case FAULT_CODE_ENCODER_FAULT: return "FAULT_CODE_ENCODER_FAULT";
 	case FAULT_CODE_LV_OUTPUT_FAULT: return "FAULT_CODE_LV_OUTPUT_FAULT";
+	case FAULT_CODE_PHASE_OUTPUT_ERROR: return "FAULT_CODE_PHASE_OUTPUT_ERROR";
 	}
 
 	return "Unknown fault";
diff --git a/motor/mcpwm_foc.c b/motor/mcpwm_foc.c
index e7bbd3ba55..7db47a44a9 100644
--- a/motor/mcpwm_foc.c
+++ b/motor/mcpwm_foc.c
@@ -1736,7 +1736,7 @@ int mcpwm_foc_encoder_detect(float current, bool print, float *offset, float *ra
  * @return
  * The fault code.
  */
-int mcpwm_foc_measure_resistance(float current, int samples, bool stop_after, float *resistance) {
+int mcpwm_foc_measure_resistance(float current, int samples, bool stop_after, float *resistance, bool settle_first) {
 	mc_interface_lock();
 
 	volatile motor_all_state_t *motor = get_motor_now();
@@ -1755,10 +1755,100 @@ int mcpwm_foc_measure_resistance(float current, int samples, bool stop_after, fl
 	KILL_SW_MODE tout_ksw = timeout_get_kill_sw_mode();
 	timeout_reset();
 	timeout_configure(60000, 0.0, KILL_SW_MODE_DISABLED);
-
-	// 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) / 200.0);
+	
+	static bool plot_started = false;	
+	static float plot_int = 0.0;
+	static int get_fw_version_cnt = 0;
+	
+	
+	if(settle_first) {
+		for(int i = 0; i < 500; i++) {
+			if (commands_get_fw_version_sent_cnt() != get_fw_version_cnt) {
+				get_fw_version_cnt = commands_get_fw_version_sent_cnt();
+				plot_started = false;
+			}
+			if (!plot_started) {
+				plot_started = true;
+				commands_init_plot("Time", "Current");			
+				commands_plot_add_graph("iq_filter");
+				commands_plot_add_graph("id_filter");
+				commands_plot_add_graph("iq_set");
+				commands_plot_add_graph("id_trip_limit");
+			}		
+			commands_plot_set_graph(0);
+			commands_send_plot_points(plot_int, motor->m_motor_state.iq_filter);
+			commands_plot_set_graph(1);
+			commands_send_plot_points(plot_int, motor->m_motor_state.id_filter);
+			commands_plot_set_graph(2);
+			commands_send_plot_points(plot_int, motor->m_iq_set);
+			commands_plot_set_graph(3);
+			commands_send_plot_points(plot_int, 0);
+			plot_int++;		
+			chThdSleepMilliseconds(1);
+			
+			// report back faults
+			fault = mc_interface_get_fault();
+			if (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 fault;
+			}	
+		}
+	}
+	
+	// Change setpoint
+	motor->m_iq_set = current;	
+	chThdSleepMilliseconds(1);		
+
+	for(int i = 0; i < 50; i++) {	
+		#ifdef HW_HAS_3_SHUNTS
+			if(i > 30) {
+		#else
+			if(i > 5) {
+		#endif
+			commands_plot_set_graph(0);
+			commands_send_plot_points(plot_int, fabs(motor->m_motor_state.iq_filter));
+			commands_plot_set_graph(1);
+			commands_send_plot_points(plot_int, fabs(motor->m_motor_state.id_filter));
+			commands_plot_set_graph(2);
+			commands_send_plot_points(plot_int, motor->m_iq_set);
+			commands_plot_set_graph(3);
+			
+		#ifdef HW_HAS_3_SHUNTS
+			commands_send_plot_points(plot_int, fabs(motor->m_motor_state.iq_filter) * 0.5);			
+			if (fabs(motor->m_motor_state.id_filter) > fabs(motor->m_motor_state.iq_filter) * 0.5) {
+				mc_interface_fault_stop(FAULT_CODE_PHASE_OUTPUT_ERROR, &m_motor_1 != motor, false);	
+			}
+		#else
+			commands_send_plot_points(plot_int, fabs(motor->m_motor_state.iq_filter) * 0.4);			
+			if (fabs(motor->m_motor_state.id_filter) > fabs(motor->m_motor_state.iq_filter) * 0.4) {
+				mc_interface_fault_stop(FAULT_CODE_PHASE_OUTPUT_ERROR, &m_motor_1 != motor, false);	
+			}
+		#endif
+			
+			
+		} else {
+			commands_plot_set_graph(0);
+			commands_send_plot_points(plot_int, fabs(motor->m_motor_state.iq_filter));
+			commands_plot_set_graph(1);
+			commands_send_plot_points(plot_int, fabs(motor->m_motor_state.id_filter));
+			commands_plot_set_graph(2);
+			commands_send_plot_points(plot_int, motor->m_iq_set);
+			commands_plot_set_graph(3);
+			commands_send_plot_points(plot_int, 0);
+		}
+		plot_int++;		
+		chThdSleepMilliseconds(1);
+		
+		// report back faults
 		fault = mc_interface_get_fault();
 		if (fault != FAULT_CODE_NONE) {
 			motor->m_id_set = 0.0;
@@ -1772,12 +1862,9 @@ int mcpwm_foc_measure_resistance(float current, int samples, bool stop_after, fl
 			mc_interface_unlock();
 
 			return fault;
-		}
-		chThdSleepMilliseconds(1);
+		}	
 	}
-
-	// Wait for the current to rise and the motor to lock.
-	chThdSleepMilliseconds(50);
+	
 
 	// Sample
 	motor->m_samples.avg_current_tot = 0.0;
@@ -1785,7 +1872,16 @@ int mcpwm_foc_measure_resistance(float current, int samples, bool stop_after, fl
 	motor->m_samples.sample_num = 0;
 
 	int cnt = 0;
-	while (motor->m_samples.sample_num < samples) {
+	while (motor->m_samples.sample_num < samples) {		
+		commands_plot_set_graph(0);
+		commands_send_plot_points(plot_int, fabs(motor->m_motor_state.iq_filter));
+		commands_plot_set_graph(1);
+		commands_send_plot_points(plot_int, fabs(motor->m_motor_state.id_filter));
+		commands_plot_set_graph(2);
+		commands_send_plot_points(plot_int, motor->m_iq_set);
+		commands_plot_set_graph(3);
+		commands_send_plot_points(plot_int, 0);
+		plot_int++;		
 		chThdSleepMilliseconds(1);
 		cnt++;
 		// Timeout
@@ -2239,10 +2335,12 @@ int mcpwm_foc_measure_res_ind(float *res, float *ind, float *ld_lq_diff) {
 	motor->m_conf->foc_current_kp = 0.001;
 	motor->m_conf->foc_current_ki = 1.0;
 
+	bool first_run = true;
 	float i_last = 0.0;
 	for (float i = 2.0;i < (motor->m_conf->l_current_max / 2.0);i *= 1.5) {
 		float r_tmp = 0.0;
-		fault = mcpwm_foc_measure_resistance(i, 20, false, &r_tmp);
+		fault = mcpwm_foc_measure_resistance(i, 20, false, &r_tmp, first_run);
+		first_run = false;
 		if (fault != FAULT_CODE_NONE || r_tmp == 0.0) {
 			goto exit_measure_res_ind;
 		}
@@ -2260,7 +2358,7 @@ int mcpwm_foc_measure_res_ind(float *res, float *ind, float *ld_lq_diff) {
 	i_last = (motor->m_conf->l_current_max / 2.0);
 #endif
 
-	fault = mcpwm_foc_measure_resistance(i_last, 200, true, res);
+	fault = mcpwm_foc_measure_resistance(i_last, 200, true, res, false);
 	if (fault == FAULT_CODE_NONE && *res != 0.0) {
 		motor->m_conf->foc_motor_r = *res;
 		mcpwm_foc_set_current(0.0);
diff --git a/motor/mcpwm_foc.h b/motor/mcpwm_foc.h
index 0cfd811321..75d47ef445 100644
--- a/motor/mcpwm_foc.h
+++ b/motor/mcpwm_foc.h
@@ -88,7 +88,7 @@ float mcpwm_foc_get_est_lambda(void);
 float mcpwm_foc_get_est_res(void);
 float mcpwm_foc_get_est_ind(void);
 int mcpwm_foc_encoder_detect(float current, bool print, float *offset, float *ratio, bool *inverted);
-int mcpwm_foc_measure_resistance(float current, int samples, bool stop_after, float *resistance);
+int mcpwm_foc_measure_resistance(float current, int samples, bool stop_after, float *resistance, bool settle_first);
 int mcpwm_foc_measure_inductance(float duty, int samples, float *curr, float *ld_lq_diff, float *inductance);
 int mcpwm_foc_measure_inductance_current(float curr_goal, int samples, float *curr, float *ld_lq_diff, float *inductance);
 
diff --git a/terminal.c b/terminal.c
index 672d0f8dca..c1cab1a11c 100644
--- a/terminal.c
+++ b/terminal.c
@@ -323,7 +323,7 @@ void terminal_process_string(char *str) {
 				mcconf->motor_type = MOTOR_TYPE_FOC;
 				mc_interface_set_configuration(mcconf);
 				float tmp_r = 0.0;
-				int fault = mcpwm_foc_measure_resistance(current, 2000, true, &tmp_r);
+				int fault = mcpwm_foc_measure_resistance(current, 2000, true, &tmp_r, true);
 				if(fault == FAULT_CODE_NONE) {
 					commands_printf("Resistance: %.6f ohm\n", (double)tmp_r);
 				} else {