diff --git a/ArduPlane/ArduPlane.cpp b/ArduPlane/ArduPlane.cpp index 6aa6db99d9..bce6e035b1 100644 --- a/ArduPlane/ArduPlane.cpp +++ b/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() diff --git a/ArduPlane/Log.cpp b/ArduPlane/Log.cpp index 63e81c613f..c1afef706a 100644 --- a/ArduPlane/Log.cpp +++ b/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)); } diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h index f2878d47bf..66b02b591b 100644 --- a/ArduPlane/Plane.h +++ b/ArduPlane/Plane.h @@ -95,6 +95,7 @@ #include #include #include +#include // 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); diff --git a/ArduPlane/failsafe.cpp b/ArduPlane/failsafe.cpp index 1380c9c5f5..ff2ca536ee 100644 --- a/ArduPlane/failsafe.cpp +++ b/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; diff --git a/ArduPlane/system.cpp b/ArduPlane/system.cpp index 0f410a3ac3..ba2514f9a8 100644 --- a/ArduPlane/system.cpp +++ b/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) {