diff --git a/ArduSub/ArduSub.cpp b/ArduSub/ArduSub.cpp index 3b53a3c256..04f3e10133 100644 --- a/ArduSub/ArduSub.cpp +++ b/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; diff --git a/ArduSub/Log.cpp b/ArduSub/Log.cpp index 6913fc9d2f..422fc1ac40 100644 --- a/ArduSub/Log.cpp +++ b/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)); diff --git a/ArduSub/Sub.h b/ArduSub/Sub.h index 4a929352f8..4011ea9def 100644 --- a/ArduSub/Sub.h +++ b/ArduSub/Sub.h @@ -66,6 +66,7 @@ #include // circle navigation library #include // Fence library #include // main loop scheduler +#include // loop perf monitoring #include // Notify library #include // Battery monitor library #include // 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); diff --git a/ArduSub/motors.cpp b/ArduSub/motors.cpp index 5180cf075c..40ce3c25ef 100644 --- a/ArduSub/motors.cpp +++ b/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; diff --git a/ArduSub/perf_info.cpp b/ArduSub/perf_info.cpp deleted file mode 100644 index bd39db7d24..0000000000 --- a/ArduSub/perf_info.cpp +++ /dev/null @@ -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; -}