Copter: mavlink PERF messages also spit out average and standard deviation

ArduCopter/ArduCopter.cpp

@@ -191,11 +191,13 @@ void Copter::perf_update(void)
     if (should_log(MASK_LOG_PM))
         Log_Write_Performance();
     if (scheduler.debug()) {
-        gcs().send_text(MAV_SEVERITY_WARNING, "PERF: %u/%u %lu %lu",
+        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_min_time(),
+                          (unsigned long)perf_info_get_avg_time(),
+                          (unsigned long)perf_info_get_stddev_time());
     }
     perf_info_reset();
     pmTest1 = 0;

ArduCopter/Copter.h

@@ -1019,6 +1019,8 @@ private:
     uint32_t perf_info_get_min_time();
     uint16_t perf_info_get_num_long_running();
     uint32_t perf_info_get_num_dropped();
+    uint32_t perf_info_get_avg_time();
+    uint32_t perf_info_get_stddev_time();
     Vector3f pv_location_to_vector(const Location& loc);
     float pv_alt_above_origin(float alt_above_home_cm);
     float pv_alt_above_home(float alt_above_origin_cm);

ArduCopter/perf_info.cpp

@@ -12,6 +12,8 @@
 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 uint64_t perf_info_sigma_time;
+static uint64_t perf_info_sigmasquared_time;
 static uint16_t perf_info_long_running;
 static uint32_t perf_info_log_dropped;
 static bool perf_ignore_loop = false;
@@ -24,6 +26,8 @@ void Copter::perf_info_reset()
     perf_info_min_time = 0;
     perf_info_long_running = 0;
     perf_info_log_dropped = DataFlash.num_dropped();
+    perf_info_sigma_time = 0;
+    perf_info_sigmasquared_time = 0;
 }
 
 // perf_ignore_loop - ignore this loop from performance measurements (used to reduce false positive when arming)
@@ -52,6 +56,8 @@ void Copter::perf_info_check_loop_time(uint32_t time_in_micros)
     if( time_in_micros > PERF_INFO_OVERTIME_THRESHOLD_MICROS ) {
         perf_info_long_running++;
     }
+    perf_info_sigma_time += time_in_micros;
+    perf_info_sigmasquared_time += time_in_micros * time_in_micros;
 }
 
 // perf_info_get_long_running_percentage - get number of long running loops as a percentage of the total number of loops
@@ -83,3 +89,15 @@ uint32_t Copter::perf_info_get_num_dropped()
 {
     return perf_info_log_dropped;
 }
+
+// perf_info_get_avg_time - return average loop time (in microseconds)
+uint32_t Copter::perf_info_get_avg_time()
+{
+    return (perf_info_sigma_time / perf_info_loop_count);
+}
+
+// perf_info_get_stddev_time - return stddev of average loop time (in us)
+uint32_t Copter::perf_info_get_stddev_time()
+{
+    return sqrt((perf_info_sigmasquared_time - (perf_info_sigma_time*perf_info_sigma_time)/perf_info_loop_count) / perf_info_loop_count);
+}