diff --git a/ArduSub/ArduSub.cpp b/ArduSub/ArduSub.cpp
index 6a6303a39a..830172a4f4 100644
--- a/ArduSub/ArduSub.cpp
+++ b/ArduSub/ArduSub.cpp
@@ -146,8 +146,9 @@ void Sub::loop()
     // in multiples of the main loop tick. So if they don't run on
     // the first call to the scheduler they won't run on a later
     // call until scheduler.tick() is called again
-    uint32_t time_available = (timer + MAIN_LOOP_MICROS) - micros();
-    scheduler.run(time_available > MAIN_LOOP_MICROS ? 0u : time_available);
+    const uint32_t loop_us = scheduler.get_loop_period_us();
+    const uint32_t time_available = (timer + loop_us) - micros();
+    scheduler.run(time_available > loop_us ? 0u : time_available);
 }
 
 
diff --git a/ArduSub/config.h b/ArduSub/config.h
index 0b01b85e7b..e477e36a21 100644
--- a/ArduSub/config.h
+++ b/ArduSub/config.h
@@ -41,7 +41,6 @@
 // run at 400Hz on all systems
 # define MAIN_LOOP_RATE    400
 # define MAIN_LOOP_SECONDS 0.0025f
-# define MAIN_LOOP_MICROS  2500
 
 #ifndef SURFACE_DEPTH_DEFAULT
 # define SURFACE_DEPTH_DEFAULT -10.0f // pressure sensor reading 10cm depth means craft is considered surfaced