diff --git a/ArduSub/GCS_Mavlink.cpp b/ArduSub/GCS_Mavlink.cpp
index 964cceab63..a180374d20 100644
--- a/ArduSub/GCS_Mavlink.cpp
+++ b/ArduSub/GCS_Mavlink.cpp
@@ -792,32 +792,6 @@ void GCS_MAVLINK_Sub::handle_rc_channels_override(const mavlink_message_t &msg)
  *  MAVLink to process packets while waiting for the initialisation to
  *  complete
  */
-void Sub::mavlink_delay_cb()
-{
-    static uint32_t last_1hz, last_50hz, last_5s;
-
-    logger.EnableWrites(false);
-
-    uint32_t tnow = AP_HAL::millis();
-    if (tnow - last_1hz > 1000) {
-        last_1hz = tnow;
-        gcs().send_message(MSG_HEARTBEAT);
-        gcs().send_message(MSG_SYS_STATUS);
-    }
-    if (tnow - last_50hz > 20) {
-        last_50hz = tnow;
-        gcs().update_receive();
-        gcs().update_send();
-        notify.update();
-    }
-    if (tnow - last_5s > 5000) {
-        last_5s = tnow;
-        gcs().send_text(MAV_SEVERITY_INFO, "Initialising APM");
-    }
-
-    logger.EnableWrites(true);
-}
-
 MAV_RESULT GCS_MAVLINK_Sub::handle_flight_termination(const mavlink_command_long_t &packet) {
     if (packet.param1 > 0.5f) {
         sub.arming.disarm();
diff --git a/ArduSub/Sub.h b/ArduSub/Sub.h
index 103e4ece46..aee01d584f 100644
--- a/ArduSub/Sub.h
+++ b/ArduSub/Sub.h
@@ -665,7 +665,6 @@ private:
 
 
 public:
-    void mavlink_delay_cb();
     void mainloop_failsafe_check();
 };
 
diff --git a/ArduSub/system.cpp b/ArduSub/system.cpp
index 32f6670d11..a78e3374e5 100644
--- a/ArduSub/system.cpp
+++ b/ArduSub/system.cpp
@@ -7,11 +7,6 @@
 *
 *****************************************************************************/
 
-static void mavlink_delay_cb_static()
-{
-    sub.mavlink_delay_cb();
-}
-
 static void failsafe_check_static()
 {
     sub.mainloop_failsafe_check();
@@ -78,10 +73,7 @@ void Sub::init_ardupilot()
 
     barometer.init();
 
-    // Register the mavlink service callback. This will run
-    // anytime there are more than 5ms remaining in a call to
-    // hal.scheduler->delay.
-    hal.scheduler->register_delay_callback(mavlink_delay_cb_static, 5);
+    register_scheduler_delay_callback();
 
     // setup telem slots with serial ports
     gcs().setup_uarts();