Sub: use AP::PerfInfo library

ArduSub/ArduSub.cpp

@@ -90,7 +90,7 @@ void Sub::setup()
     scheduler.init(&scheduler_tasks[0], ARRAY_SIZE(scheduler_tasks));
 
     // setup initial performance counters
-    perf_info_reset();
+    perf_info.reset();
     fast_loopTimer = AP_HAL::micros();
 }
 
@@ -109,12 +109,12 @@ void Sub::perf_update(void)
     }
     if (scheduler.debug()) {
         gcs().send_text(MAV_SEVERITY_WARNING, "PERF: %u/%u %lu %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)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());
     }
-    perf_info_reset();
+    perf_info.reset();
     pmTest1 = 0;
 }
 
@@ -126,7 +126,7 @@ void Sub::loop()
     uint32_t timer = micros();
 
     // check loop time
-    perf_info_check_loop_time(timer - fast_loopTimer);
+    perf_info.check_loop_time(timer - fast_loopTimer);
 
     // used by PI Loops
     G_Dt                    = (float)(timer - fast_loopTimer) / 1000000.0f;

ArduSub/Log.cpp

@@ -159,13 +159,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 : perf_info.get_num_long_running(),
+        num_loops        : perf_info.get_num_loops(),
+        max_time         : 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() - perf_info.get_num_dropped(),
         hal.util->available_memory()
     };
     DataFlash.WriteCriticalBlock(&pkt, sizeof(pkt));

ArduSub/Sub.h

@@ -66,6 +66,7 @@
 #include <AC_WPNav/AC_Circle.h>          // circle navigation library
 #include <AC_Fence/AC_Fence.h>           // Fence library
 #include <AP_Scheduler/AP_Scheduler.h>       // main loop scheduler
+#include <AP_Scheduler/PerfInfo.h>       // loop perf monitoring
 #include <AP_Notify/AP_Notify.h>          // Notify library
 #include <AP_BattMonitor/AP_BattMonitor.h>     // Battery monitor library
 #include <AP_BoardConfig/AP_BoardConfig.h>     // board configuration library
@@ -345,6 +346,9 @@ private:
     // Flag indicating if we are currently using input hold
     bool input_hold_engaged;
 
+    // loop performance monitoring:
+    AP::PerfInfo perf_info = AP::PerfInfo::create();
+
     // 3D Location vectors
     // Current location of the Sub (altitude is relative to home)
     Location_Class current_loc;
@@ -623,14 +627,6 @@ private:
     bool init_arm_motors(bool arming_from_gcs);
     void init_disarm_motors();
     void motors_output();
-    void perf_info_reset();
-    void perf_ignore_this_loop();
-    void perf_info_check_loop_time(uint32_t time_in_micros);
-    uint16_t perf_info_get_num_loops();
-    uint32_t perf_info_get_max_time();
-    uint32_t perf_info_get_min_time();
-    uint16_t perf_info_get_num_long_running();
-    uint32_t perf_info_get_num_dropped();
     Vector3f pv_location_to_vector(const Location& loc);
     float pv_alt_above_origin(float alt_above_home_cm);
     float pv_get_bearing_cd(const Vector3f &origin, const Vector3f &destination);

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_ignore_this_loop();
+    perf_info.ignore_this_loop();
 
     // flag exiting this function
     in_arm_motors = false;

ArduSub/perf_info.cpp

@@ -1,85 +0,0 @@
-#include "Sub.h"
-
-//
-//  high level performance monitoring
-//
-//  we measure the main loop time
-//
-
-// 400hz loop update rate
-#define PERF_INFO_OVERTIME_THRESHOLD_MICROS 3000
-
-static uint16_t perf_info_loop_count;
-static uint32_t perf_info_max_time; // in microseconds
-static uint32_t perf_info_min_time; // in microseconds
-static uint16_t perf_info_long_running;
-static uint32_t perf_info_log_dropped;
-static bool perf_ignore_loop = false;
-
-// perf_info_reset - reset all records of loop time to zero
-void Sub::perf_info_reset()
-{
-    perf_info_loop_count = 0;
-    perf_info_max_time = 0;
-    perf_info_min_time = 0;
-    perf_info_long_running = 0;
-    perf_info_log_dropped = DataFlash.num_dropped();
-}
-
-// perf_ignore_loop - ignore this loop from performance measurements (used to reduce false positive when arming)
-void Sub::perf_ignore_this_loop()
-{
-    perf_ignore_loop = true;
-}
-
-// perf_info_check_loop_time - check latest loop time vs min, max and overtime threshold
-void Sub::perf_info_check_loop_time(uint32_t time_in_micros)
-{
-    perf_info_loop_count++;
-
-    // exit if this loop should be ignored
-    if (perf_ignore_loop) {
-        perf_ignore_loop = false;
-        return;
-    }
-
-    if (time_in_micros > perf_info_max_time) {
-        perf_info_max_time = time_in_micros;
-    }
-    if (perf_info_min_time == 0 || time_in_micros < perf_info_min_time) {
-        perf_info_min_time = time_in_micros;
-    }
-    if (time_in_micros > PERF_INFO_OVERTIME_THRESHOLD_MICROS) {
-        perf_info_long_running++;
-    }
-}
-
-// perf_info_get_long_running_percentage - get number of long running loops as a percentage of the total number of loops
-uint16_t Sub::perf_info_get_num_loops()
-{
-    return perf_info_loop_count;
-}
-
-// perf_info_get_max_time - return maximum loop time (in microseconds)
-uint32_t Sub::perf_info_get_max_time()
-{
-    return perf_info_max_time;
-}
-
-// perf_info_get_max_time - return maximum loop time (in microseconds)
-uint32_t Sub::perf_info_get_min_time()
-{
-    return perf_info_min_time;
-}
-
-// perf_info_get_num_long_running - get number of long running loops
-uint16_t Sub::perf_info_get_num_long_running()
-{
-    return perf_info_long_running;
-}
-
-// perf_info_get_num_dropped - get number of dropped log messages
-uint32_t Sub::perf_info_get_num_dropped()
-{
-    return perf_info_log_dropped;
-}