diff --git a/commands.c b/commands.c
index 0d5f3a4b..26508c00 100644
--- a/commands.c
+++ b/commands.c
@@ -640,7 +640,7 @@ void commands_process_packet(unsigned char *data, unsigned int len,
 			raw = data[ind++];
 		}
 
-		mc_interface_sample_print_data(mode, sample_len, decimation, raw);
+		mc_interface_sample_print_data(mode, sample_len, decimation, raw, send_func);
 	} break;
 
 	case COMM_REBOOT:
diff --git a/mc_interface.c b/mc_interface.c
index 322c5e03..822c75f7 100644
--- a/mc_interface.c
+++ b/mc_interface.c
@@ -130,6 +130,7 @@ static volatile motor_if_state_t *motor_now(void);
 
 // Function pointers
 static void(*pwn_done_func)(void) = 0;
+static void(* volatile send_func_sample)(unsigned char *data, unsigned int len) = 0;
 
 // Threads
 static THD_WORKING_AREA(timer_thread_wa, 512);
@@ -1371,7 +1372,9 @@ float mc_interface_get_last_sample_adc_isr_duration(void) {
 	return m_last_adc_duration_sample;
 }
 
-void mc_interface_sample_print_data(debug_sampling_mode mode, uint16_t len, uint8_t decimation, bool raw) {
+void mc_interface_sample_print_data(debug_sampling_mode mode, uint16_t len, uint8_t decimation, bool raw, 
+		void(*reply_func)(unsigned char *data, unsigned int len)) {
+
 	if (len > ADC_SAMPLE_MAX_LEN) {
 		len = ADC_SAMPLE_MAX_LEN;
 	}
@@ -1385,6 +1388,7 @@ void mc_interface_sample_print_data(debug_sampling_mode mode, uint16_t len, uint
 		m_sample_int = decimation;
 		m_sample_mode = mode;
 		m_sample_raw = raw;
+		send_func_sample = reply_func;
 #ifdef HW_HAS_DUAL_MOTORS
 		m_sample_is_second_motor = motor_now() == &m_motor_2;
 #endif
@@ -2672,7 +2676,7 @@ static THD_FUNCTION(sample_send_thread, arg) {
 			buffer[index++] = m_status_samples[ind_samp];
 			buffer[index++] = m_phase_samples[ind_samp];
 
-			commands_send_packet(buffer, index);
+			send_func_sample(buffer, index);
 		}
 	}
 }
diff --git a/mc_interface.h b/mc_interface.h
index 00c5da45..c6bb5c39 100644
--- a/mc_interface.h
+++ b/mc_interface.h
@@ -84,7 +84,8 @@ float mc_interface_get_pid_pos_set(void);
 float mc_interface_get_pid_pos_now(void);
 void mc_interface_update_pid_pos_offset(float angle_now, bool store);
 float mc_interface_get_last_sample_adc_isr_duration(void);
-void mc_interface_sample_print_data(debug_sampling_mode mode, uint16_t len, uint8_t decimation, bool raw);
+void mc_interface_sample_print_data(debug_sampling_mode mode, uint16_t len, uint8_t decimation, bool raw, 
+		void(*reply_func)(unsigned char *data, unsigned int len));
 float mc_interface_temp_fet_filtered(void);
 float mc_interface_temp_motor_filtered(void);
 float mc_interface_get_battery_level(float *wh_left);