diff --git a/conf_general.h b/conf_general.h
index a9308101..f75a258e 100644
--- a/conf_general.h
+++ b/conf_general.h
@@ -24,7 +24,7 @@
 #define FW_VERSION_MAJOR			5
 #define FW_VERSION_MINOR			03
 // Set to 0 for building a release and iterate during beta test builds
-#define FW_TEST_VERSION_NUMBER		34
+#define FW_TEST_VERSION_NUMBER		35
 
 #include "datatypes.h"
 
diff --git a/mc_interface.c b/mc_interface.c
index fce16778..52f2f002 100644
--- a/mc_interface.c
+++ b/mc_interface.c
@@ -1559,6 +1559,26 @@ void mc_interface_ignore_input_both(int time_ms) {
 #endif
 }
 
+void mc_interface_set_current_off_delay(float delay_sec) {
+	if (mc_interface_try_input()) {
+		return;
+	}
+
+	switch (motor_now()->m_conf.motor_type) {
+	case MOTOR_TYPE_BLDC:
+	case MOTOR_TYPE_DC:
+
+		break;
+
+	case MOTOR_TYPE_FOC:
+		mcpwm_foc_set_current_off_delay(delay_sec);
+		break;
+
+	default:
+		break;
+	}
+}
+
 // MC implementation functions
 
 /**
diff --git a/mc_interface.h b/mc_interface.h
index 71e79187..31ba9bf3 100644
--- a/mc_interface.h
+++ b/mc_interface.h
@@ -96,6 +96,8 @@ void mc_interface_set_odometer(uint64_t new_odometer_meters);
 void mc_interface_ignore_input(int time_ms);
 void mc_interface_ignore_input_both(int time_ms);
 
+void mc_interface_set_current_off_delay(float delay_sec);
+
 // MC implementation functions
 void mc_interface_fault_stop(mc_fault_code fault, bool is_second_motor, bool is_isr);
 int mc_interface_try_input(void);
diff --git a/mcpwm_foc.c b/mcpwm_foc.c
index e696c78e..84731cea 100644
--- a/mcpwm_foc.c
+++ b/mcpwm_foc.c
@@ -3090,10 +3090,10 @@ static void run_fw(volatile motor_all_state_t *motor, float dt) {
 static void timer_update(volatile motor_all_state_t *motor, float dt) {
 	run_fw(motor, dt);
 
-	utils_step_towards((float*)&motor->m_current_off_delay, 0.0, dt);
-
 	// Check if it is time to stop the modulation. Notice that modulation is kept on as long as there is
 	// field weakening current.
+	utils_sys_lock_cnt();
+	utils_step_towards((float*)&motor->m_current_off_delay, 0.0, dt);
 	if (!motor->m_phase_override && motor->m_state == MC_STATE_RUNNING &&
 			(motor->m_control_mode == CONTROL_MODE_CURRENT ||
 					motor->m_control_mode == CONTROL_MODE_CURRENT_BRAKE ||
@@ -3108,6 +3108,7 @@ static void timer_update(volatile motor_all_state_t *motor, float dt) {
 			stop_pwm_hw(motor);
 		}
 	}
+	utils_sys_unlock_cnt();
 
 	// Use this to study the openloop timers under experiment plot
 #if 0