diff --git a/firmware/controllers/bench_test.cpp b/firmware/controllers/bench_test.cpp
index 59fe951dc0..18b294823a 100644
--- a/firmware/controllers/bench_test.cpp
+++ b/firmware/controllers/bench_test.cpp
@@ -65,34 +65,21 @@ bool isRunningBenchTest(void) {
 
 static void runBench(brain_pin_e brainPin, OutputPin *output, float delayMs, float onTimeMs, float offTimeMs,
 		int count) {
-    int delaySt = delayMs < 1 ? 1 : TIME_MS2I(delayMs);
-	int onTimeSt = onTimeMs < 1 ? 1 : TIME_MS2I(onTimeMs);
-	int offTimeSt = offTimeMs < 1 ? 1 : TIME_MS2I(offTimeMs);
-	if (delaySt < 0) {
-		scheduleMsg(logger, "Invalid delay %.2f", delayMs);
-		return;
-	}
-	if (onTimeSt <= 0) {
-		scheduleMsg(logger, "Invalid onTime %.2f", onTimeMs);
-		return;
-	}
-	if (offTimeSt <= 0) {
-		scheduleMsg(logger, "Invalid offTime %.2f", offTimeMs);
-		return;
-	}
-	scheduleMsg(logger, "Running bench: ON_TIME=%.2f ms OFF_TIME=%.2fms Counter=%d", onTimeMs, offTimeMs, count);
+	int delayUs = MS2US(maxF(1, delayMs));
+	int onTimeUs = MS2US(maxF(1, onTimeMs));
+	int offTimeUs = MS2US(maxF(1, offTimeMs));
+
+	scheduleMsg(logger, "Running bench: ON_TIME=%.2f us OFF_TIME=%.2f us Counter=%d", onTimeUs, offTimeUs, count);
 	scheduleMsg(logger, "output on %s", hwPortname(brainPin));
 
-	if (delaySt != 0) {
-		chThdSleep(delaySt);
-	}
+	chThdSleepMicroseconds(delayUs);
 
 	isRunningBench = true;
 	for (int i = 0; i < count; i++) {
 		output->setValue(true);
-		chThdSleep(onTimeSt);
+		chThdSleepMicroseconds(onTimeUs);
 		output->setValue(false);
-		chThdSleep(offTimeSt);
+		chThdSleepMicroseconds(offTimeUs);
 	}
 	scheduleMsg(logger, "Done!");
 	isRunningBench = false;