From 7313d9e7a7f134e51856b4fd0c63305261074af9 Mon Sep 17 00:00:00 2001
From: Peter Barker <pbarker@barker.dropbear.id.au>
Date: Mon, 13 Nov 2017 19:04:31 +1100
Subject: [PATCH] Sub: use AP_Scheduler's loop() function

---
 ArduSub/ArduSub.cpp | 48 +++++++++------------------------------------
 ArduSub/Log.cpp     |  8 ++++----
 ArduSub/Sub.cpp     |  1 -
 ArduSub/Sub.h       | 10 +---------
 ArduSub/motors.cpp  |  2 +-
 5 files changed, 15 insertions(+), 54 deletions(-)

diff --git a/ArduSub/ArduSub.cpp b/ArduSub/ArduSub.cpp
index 770e6dc3fb..3e9ff88cb1 100644
--- a/ArduSub/ArduSub.cpp
+++ b/ArduSub/ArduSub.cpp
@@ -90,11 +90,6 @@ void Sub::setup()
 
     // initialise the main loop scheduler
     scheduler.init(&scheduler_tasks[0], ARRAY_SIZE(scheduler_tasks));
-
-    // setup initial performance counters
-    perf_info.set_loop_rate(scheduler.get_loop_rate_hz());
-    perf_info.reset();
-    fast_loopTimer = AP_HAL::micros();
 }
 
 void Sub::perf_update(void)
@@ -104,46 +99,21 @@ void Sub::perf_update(void)
     }
     if (scheduler.debug()) {
         gcs().send_text(MAV_SEVERITY_WARNING, "PERF: %u/%u max=%lu min=%lu avg=%lu sd=%lu",
-                          (unsigned)perf_info.get_num_long_running(),
-                          (unsigned)perf_info.get_num_loops(),
-                          (unsigned long)perf_info.get_max_time(),
-                          (unsigned long)perf_info.get_min_time(),
-                          (unsigned long)perf_info.get_avg_time(),
-                          (unsigned long)perf_info.get_stddev_time());
+                          (unsigned)scheduler.perf_info.get_num_long_running(),
+                          (unsigned)scheduler.perf_info.get_num_loops(),
+                          (unsigned long)scheduler.perf_info.get_max_time(),
+                          (unsigned long)scheduler.perf_info.get_min_time(),
+                          (unsigned long)scheduler.perf_info.get_avg_time(),
+                          (unsigned long)scheduler.perf_info.get_stddev_time());
     }
-    perf_info.reset(scheduler.get_loop_rate_hz());
+    scheduler.perf_info.reset();
     pmTest1 = 0;
 }
 
 void Sub::loop()
 {
-    // wait for an INS sample
-    ins.wait_for_sample();
-
-    uint32_t timer = micros();
-
-    // check loop time
-    perf_info.check_loop_time(timer - fast_loopTimer);
-
-    // used by PI Loops
-    G_Dt                    = (float)(timer - fast_loopTimer) / 1000000.0f;
-    fast_loopTimer          = timer;
-
-    // Execute the fast loop
-    // ---------------------
-    fast_loop();
-
-    // tell the scheduler one tick has passed
-    scheduler.tick();
-
-    // run all the tasks that are due to run. Note that we only
-    // have to call this once per loop, as the tasks are scheduled
-    // 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
-    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);
+    scheduler.loop();
+    G_Dt = scheduler.last_loop_time;
 }
 
 
diff --git a/ArduSub/Log.cpp b/ArduSub/Log.cpp
index 98ede71ea9..48d3d599c3 100644
--- a/ArduSub/Log.cpp
+++ b/ArduSub/Log.cpp
@@ -150,13 +150,13 @@ void Sub::Log_Write_Performance()
     struct log_Performance pkt = {
         LOG_PACKET_HEADER_INIT(LOG_PERFORMANCE_MSG),
         time_us          : AP_HAL::micros64(),
-        num_long_running : perf_info.get_num_long_running(),
-        num_loops        : perf_info.get_num_loops(),
-        max_time         : perf_info.get_max_time(),
+        num_long_running : scheduler.perf_info.get_num_long_running(),
+        num_loops        : scheduler.perf_info.get_num_loops(),
+        max_time         : scheduler.perf_info.get_max_time(),
         pm_test          : pmTest1,
         i2c_lockup_count : 0,
         ins_error_count  : ins.error_count(),
-        log_dropped      : DataFlash.num_dropped() - perf_info.get_num_dropped(),
+        log_dropped      : DataFlash.num_dropped() - scheduler.perf_info.get_num_dropped(),
         hal.util->available_memory()
     };
     DataFlash.WriteCriticalBlock(&pkt, sizeof(pkt));
diff --git a/ArduSub/Sub.cpp b/ArduSub/Sub.cpp
index 72b6d5195e..63505cbe28 100644
--- a/ArduSub/Sub.cpp
+++ b/ArduSub/Sub.cpp
@@ -52,7 +52,6 @@ Sub::Sub(void)
           wp_nav(inertial_nav, ahrs_view, pos_control, attitude_control),
           circle_nav(inertial_nav, ahrs_view, pos_control),
           pmTest1(0),
-          fast_loopTimer(0),
           in_mavlink_delay(false),
           param_loader(var_info),
           last_pilot_yaw_input_ms(0)
diff --git a/ArduSub/Sub.h b/ArduSub/Sub.h
index d216518aeb..6e1c0fded4 100644
--- a/ArduSub/Sub.h
+++ b/ArduSub/Sub.h
@@ -147,7 +147,7 @@ private:
     ParametersG2 g2;
 
     // main loop scheduler
-    AP_Scheduler scheduler;
+    AP_Scheduler scheduler{FUNCTOR_BIND_MEMBER(&Sub::fast_loop, void)};
 
     // AP_Notify instance
     AP_Notify notify;
@@ -347,9 +347,6 @@ private:
     // Flag indicating if we are currently using input hold
     bool input_hold_engaged;
 
-    // loop performance monitoring:
-    AP::PerfInfo perf_info;
-
     // 3D Location vectors
     // Current location of the Sub (altitude is relative to home)
     Location_Class current_loc;
@@ -401,11 +398,6 @@ private:
     // Performance monitoring
     int16_t pmTest1;
 
-    // System Timers
-    // --------------
-    // Time in microseconds of main control loop
-    uint32_t fast_loopTimer;
-
     // Reference to the relay object
     AP_Relay relay;
 
diff --git a/ArduSub/motors.cpp b/ArduSub/motors.cpp
index 284258160a..8a9ac10b64 100644
--- a/ArduSub/motors.cpp
+++ b/ArduSub/motors.cpp
@@ -75,7 +75,7 @@ bool Sub::init_arm_motors(bool arming_from_gcs)
     mainloop_failsafe_enable();
 
     // perf monitor ignores delay due to arming
-    perf_info.ignore_this_loop();
+    scheduler.perf_info.ignore_this_loop();
 
     // flag exiting this function
     in_arm_motors = false;