Copter: use AP_Scheduler's loop() function

ArduCopter/ArduCopter.cpp

@@ -188,11 +188,6 @@ void Copter::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 Copter::perf_update(void)
@@ -201,46 +196,21 @@ void Copter::perf_update(void)
         Log_Write_Performance();
     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 Copter::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;
 }
 
 

ArduCopter/Copter.cpp

@@ -47,7 +47,6 @@ Copter::Copter(void)
     yaw_look_ahead_bearing(0.0f),
     inertial_nav(ahrs),
     pmTest1(0),
-    fast_loopTimer(0),
     auto_trim_counter(0),
     in_mavlink_delay(false),
     param_loader(var_info),

ArduCopter/Copter.h

@@ -79,7 +79,6 @@
 #include <AC_Fence/AC_Fence.h>           // Arducopter Fence library
 #include <AC_Avoidance/AC_Avoid.h>           // Arducopter stop at fence library
 #include <AP_Scheduler/AP_Scheduler.h>       // main loop scheduler
-#include <AP_Scheduler/PerfInfo.h>       // loop perf monitoring
 #include <AP_RCMapper/AP_RCMapper.h>        // RC input mapping library
 #include <AP_Notify/AP_Notify.h>          // Notify library
 #include <AP_BattMonitor/AP_BattMonitor.h>     // Battery monitor library
@@ -177,7 +176,7 @@ private:
     ParametersG2 g2;
 
     // main loop scheduler
-    AP_Scheduler scheduler;
+    AP_Scheduler scheduler{FUNCTOR_BIND_MEMBER(&Copter::fast_loop, void)};
 
     // AP_Notify instance
     AP_Notify notify;
@@ -423,9 +422,6 @@ private:
     // filtered pilot's throttle input used to cancel landing if throttle held high
     LowPassFilterFloat rc_throttle_control_in_filter;
 
-    // loop performance monitoring:
-    AP::PerfInfo perf_info;
-
     // 3D Location vectors
     // Current location of the vehicle (altitude is relative to home)
     Location_Class current_loc;
@@ -477,8 +473,6 @@ private:
 
     // System Timers
     // --------------
-    // Time in microseconds of main control loop
-    uint32_t fast_loopTimer;
     // arm_time_ms - Records when vehicle was armed. Will be Zero if we are disarmed.
     uint32_t arm_time_ms;
 

ArduCopter/Log.cpp

@@ -199,13 +199,13 @@ void Copter::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));

ArduCopter/motors.cpp

@@ -220,7 +220,7 @@ bool Copter::init_arm_motors(bool arming_from_gcs)
     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;