Plane: use AP::PerfInfo library

ArduPlane/ArduPlane.cpp

@@ -115,19 +115,19 @@ void Plane::setup()
 
 void Plane::loop()
 {
-    uint32_t loop_us = 1000000UL / scheduler.get_loop_rate_hz();
-
     // wait for an INS sample
     ins.wait_for_sample();
 
     uint32_t timer = micros();
 
     // check loop time
-    perf_info.check_loop_time(timer - fast_loopTimer);
-    G_Dt = (float)(timer - fast_loopTimer) * 1.0e-6;
-    fast_loopTimer = timer;
+    perf_info.check_loop_time(timer - perf.fast_loopTimer_us);
+
+    G_Dt                     = (timer - perf.fast_loopTimer_us) * 1.0e-6f;
+    perf.fast_loopTimer_us   = timer;
 
-    mainLoop_count++;
+    // for mainloop failure monitoring
+    perf.mainLoop_count++;
 
     // tell the scheduler one tick has passed
     scheduler.tick();
@@ -137,7 +137,9 @@ void Plane::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
-    scheduler.run(loop_us);
+    const uint32_t loop_us = scheduler.get_loop_period_us();
+    const uint32_t time_available = (timer + loop_us) - micros();
+    scheduler.run(time_available);
 }
 
 void Plane::update_soft_armed()
@@ -359,19 +361,23 @@ void Plane::one_second_loop()
 
 void Plane::log_perf_info()
 {
+    if (scheduler.debug() != 0) {
+        gcs().send_text(MAV_SEVERITY_INFO,
+                        "PERF: %u/%u max=%lu min=%lu avg=%lu sd=%lu Log=%u",
+                        (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)(DataFlash.num_dropped() - perf_info.get_num_dropped()));
+    }
+
     if (should_log(MASK_LOG_PM)) {
         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());
-    }
-    perf_info.reset(scheduler.get_loop_rate_hz());
+
+    perf_info.reset();
 }
 
 void Plane::compass_save()

ArduPlane/Log.cpp

@@ -73,10 +73,10 @@ void Plane::Log_Write_Performance()
         LOG_PACKET_HEADER_INIT(LOG_PERFORMANCE_MSG),
         time_us         : AP_HAL::micros64(),
         num_long        : perf_info.get_num_long_running(),
-        main_loop_count : (uint16_t)mainLoop_count,
+        main_loop_count : perf_info.get_num_loops(),
         g_dt_max        : perf_info.get_max_time(),
         g_dt_min        : perf_info.get_min_time(),
-        log_dropped     : dropped - last_log_dropped,
+        log_dropped     : DataFlash.num_dropped() - perf_info.get_num_dropped(),
         hal.util->available_memory()
     };
     last_log_dropped = dropped;

ArduPlane/Plane.h

@@ -64,6 +64,7 @@
 #include <AP_Declination/AP_Declination.h> // ArduPilot Mega Declination Helper Library
 #include <DataFlash/DataFlash.h>
 #include <AP_Scheduler/AP_Scheduler.h>       // main loop scheduler
+#include <AP_Scheduler/PerfInfo.h>                  // loop perf monitoring
 
 #include <AP_Navigation/AP_Navigation.h>
 #include <AP_L1_Control/AP_L1_Control.h>
@@ -95,7 +96,6 @@
 #include <AP_Button/AP_Button.h>
 #include <AP_ICEngine/AP_ICEngine.h>
 #include <AP_Landing/AP_Landing.h>
-#include <AP_Scheduler/PerfInfo.h>       // loop perf monitoring
 
 #include "GCS_Mavlink.h"
 #include "GCS_Plane.h"
@@ -737,9 +737,14 @@ private:
 
     // loop performance monitoring:
     AP::PerfInfo perf_info;
-    uint32_t mainLoop_count;
-    uint32_t fast_loopTimer;
-    uint32_t last_log_dropped;
+    struct {
+        // System Timers
+        // Time in microseconds of start of main control loop
+        uint32_t fast_loopTimer_us;
+        
+        // Counter of main loop executions.  Used for performance monitoring and failsafe processing
+        uint16_t mainLoop_count;
+    } perf;
 
     struct {
         uint32_t last_trim_check;