diff --git a/ArduCopter/ArduCopter.cpp b/ArduCopter/ArduCopter.cpp index 54c4799cc6..504f5f9b17 100644 --- a/ArduCopter/ArduCopter.cpp +++ b/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; diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index f54d4ec363..23e0f2cae1 100644 --- a/ArduCopter/Copter.h +++ b/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); diff --git a/ArduCopter/perf_info.cpp b/ArduCopter/perf_info.cpp index 2e2dd8b472..a5298f8896 100644 --- a/ArduCopter/perf_info.cpp +++ b/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); +}