Plane: use AP:PerfInfo class

ArduPlane/ArduPlane.cpp

@@ -108,6 +108,9 @@ void Plane::setup()
 
     // initialise the main loop scheduler
     scheduler.init(&scheduler_tasks[0], ARRAY_SIZE(scheduler_tasks));
+
+    perf_info.reset(scheduler.get_loop_rate_hz());
+    perf_info.ignore_this_loop();
 }
 
 void Plane::loop()
@@ -119,23 +122,12 @@ void Plane::loop()
 
     uint32_t timer = micros();
 
-    perf.delta_us_fast_loop  = timer - perf.fast_loopTimer_us;
-    G_Dt = perf.delta_us_fast_loop * 1.0e-6f;
-
-    if (perf.delta_us_fast_loop > loop_us + 500) {
-        perf.num_long++;
-    }
-
-    if (perf.delta_us_fast_loop > perf.G_Dt_max && perf.fast_loopTimer_us != 0) {
-        perf.G_Dt_max = perf.delta_us_fast_loop;
-    }
+    // check loop time
+    perf_info.check_loop_time(timer - fast_loopTimer);
+    G_Dt = (float)(timer - fast_loopTimer) * 1.0e-6;
+    fast_loopTimer = timer;
 
-    if (perf.delta_us_fast_loop < perf.G_Dt_min || perf.G_Dt_min == 0) {
-        perf.G_Dt_min = perf.delta_us_fast_loop;
-    }
-    perf.fast_loopTimer_us = timer;
-
-    perf.mainLoop_count++;
+    mainLoop_count++;
 
     // tell the scheduler one tick has passed
     scheduler.tick();
@@ -367,20 +359,19 @@ void Plane::one_second_loop()
 
 void Plane::log_perf_info()
 {
-    if (scheduler.debug() != 0) {
-        gcs().send_text(MAV_SEVERITY_INFO, "PERF: %u/%u Dt=%u/%u Log=%u",
-                          (unsigned)perf.num_long,
-                          (unsigned)perf.mainLoop_count,
-                          (unsigned)perf.G_Dt_max,
-                          (unsigned)perf.G_Dt_min,
-                          (unsigned)(DataFlash.num_dropped() - perf.last_log_dropped));
-    }
-
     if (should_log(MASK_LOG_PM)) {
         Log_Write_Performance();
     }
-
-    resetPerfData();
+    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());
 }
 
 void Plane::compass_save()

ArduPlane/Log.cpp

@@ -68,16 +68,18 @@ struct PACKED log_Performance {
 // Write a performance monitoring packet. Total length : 19 bytes
 void Plane::Log_Write_Performance()
 {
+    uint32_t dropped = DataFlash.num_dropped();
     struct log_Performance pkt = {
         LOG_PACKET_HEADER_INIT(LOG_PERFORMANCE_MSG),
         time_us         : AP_HAL::micros64(),
-        num_long        : perf.num_long,
-        main_loop_count : perf.mainLoop_count,
-        g_dt_max        : perf.G_Dt_max,
-        g_dt_min        : perf.G_Dt_min,
-        log_dropped     : DataFlash.num_dropped() - perf.last_log_dropped,
+        num_long        : perf_info.get_num_long_running(),
+        main_loop_count : (uint16_t)mainLoop_count,
+        g_dt_max        : perf_info.get_max_time(),
+        g_dt_min        : perf_info.get_min_time(),
+        log_dropped     : dropped - last_log_dropped,
         hal.util->available_memory()
     };
+    last_log_dropped = dropped;
     DataFlash.WriteCriticalBlock(&pkt, sizeof(pkt));
 }
 

ArduPlane/Plane.h

@@ -95,6 +95,7 @@
 #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"
@@ -734,31 +735,11 @@ private:
     // This is the time between calls to the DCM algorithm and is the Integration time for the gyros.
     float G_Dt = 0.02f;
 
-    struct {
-        // Performance monitoring
-        // Timer used to accrue data and trigger recording of the performanc monitoring log message
-        uint32_t start_ms;
-
-        // The maximum and minimum main loop execution time, in microseconds, recorded in the current performance monitoring interval
-        uint32_t G_Dt_max;
-        uint32_t G_Dt_min;
-
-        // System Timers
-        // Time in microseconds of start of main control loop
-        uint32_t fast_loopTimer_us;
-
-        // Number of milliseconds used in last main loop cycle
-        uint32_t delta_us_fast_loop;
-
-        // Counter of main loop executions.  Used for performance monitoring and failsafe processing
-        uint16_t mainLoop_count;
-
-        // number of long loops
-        uint16_t num_long;
-
-        // accumulated lost log messages
-        uint32_t last_log_dropped;
-    } perf;
+    // loop performance monitoring:
+    AP::PerfInfo perf_info;
+    uint32_t mainLoop_count;
+    uint32_t fast_loopTimer;
+    uint32_t last_log_dropped;
 
     struct {
         uint32_t last_trim_check;
@@ -972,7 +953,6 @@ private:
     void check_short_failsafe();
     void startup_INS_ground(void);
     void update_notify();
-    void resetPerfData(void);
     bool should_log(uint32_t mask);
     int8_t throttle_percentage(void);
     void change_arm_state(void);

ArduPlane/failsafe.cpp

@@ -21,9 +21,9 @@ void Plane::failsafe_check(void)
     static bool in_failsafe;
     uint32_t tnow = micros();
 
-    if (perf.mainLoop_count != last_mainLoop_count) {
+    if (perf_info.get_num_loops() != last_mainLoop_count) {
         // the main loop is running, all is OK
-        last_mainLoop_count = perf.mainLoop_count;
+        last_mainLoop_count = perf_info.get_num_loops();
         last_timestamp = tnow;
         in_failsafe = false;
         return;

ArduPlane/system.cpp

@@ -599,17 +599,6 @@ void Plane::update_notify()
     notify.update();
 }
 
-void Plane::resetPerfData(void) 
-{
-    perf.mainLoop_count = 0;
-    perf.G_Dt_max       = 0;
-    perf.G_Dt_min       = 0;
-    perf.num_long       = 0;
-    perf.start_ms       = millis();
-    perf.last_log_dropped = DataFlash.num_dropped();
-}
-
-
 // sets notify object flight mode information
 void Plane::notify_flight_mode(enum FlightMode mode)
 {