From a7006a77847fb9e53b27bbef6891ee7457d44bc9 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 21 Apr 2016 11:50:13 +1000 Subject: [PATCH] Plane: refactor perf variables into a structure --- ArduPlane/ArduPlane.cpp | 24 ++++++++++++------------ ArduPlane/Log.cpp | 6 +++--- ArduPlane/Plane.h | 28 +++++++++++++++------------- ArduPlane/failsafe.cpp | 4 ++-- ArduPlane/navigation.cpp | 2 +- ArduPlane/system.cpp | 8 ++++---- ArduPlane/test.cpp | 8 ++++---- 7 files changed, 41 insertions(+), 39 deletions(-) diff --git a/ArduPlane/ArduPlane.cpp b/ArduPlane/ArduPlane.cpp index cc61a502f0..8d5b66b57d 100644 --- a/ArduPlane/ArduPlane.cpp +++ b/ArduPlane/ArduPlane.cpp @@ -112,19 +112,19 @@ void Plane::loop() uint32_t timer = micros(); - delta_us_fast_loop = timer - fast_loopTimer_us; - G_Dt = delta_us_fast_loop * 1.0e-6f; + perf.delta_us_fast_loop = timer - perf.fast_loopTimer_us; + G_Dt = perf.delta_us_fast_loop * 1.0e-6f; - if (delta_us_fast_loop > G_Dt_max && fast_loopTimer_us != 0) { - G_Dt_max = delta_us_fast_loop; + if (perf.delta_us_fast_loop > perf.G_Dt_max && perf.fast_loopTimer_us != 0) { + perf.G_Dt_max = perf.delta_us_fast_loop; } - if (delta_us_fast_loop < G_Dt_min || G_Dt_min == 0) { - G_Dt_min = delta_us_fast_loop; + if (perf.delta_us_fast_loop < perf.G_Dt_min || perf.G_Dt_min == 0) { + perf.G_Dt_min = perf.delta_us_fast_loop; } - fast_loopTimer_us = timer; + perf.fast_loopTimer_us = timer; - mainLoop_count++; + perf.mainLoop_count++; // tell the scheduler one tick has passed scheduler.tick(); @@ -339,16 +339,16 @@ void Plane::log_perf_info() { if (scheduler.debug() != 0) { gcs_send_text_fmt(MAV_SEVERITY_INFO, "G_Dt_max=%lu G_Dt_min=%lu\n", - (unsigned long)G_Dt_max, - (unsigned long)G_Dt_min); + (unsigned long)perf.G_Dt_max, + (unsigned long)perf.G_Dt_min); } if (should_log(MASK_LOG_PM)) { Log_Write_Performance(); } - G_Dt_max = 0; - G_Dt_min = 0; + perf.G_Dt_max = 0; + perf.G_Dt_min = 0; resetPerfData(); } diff --git a/ArduPlane/Log.cpp b/ArduPlane/Log.cpp index 8d0825b4f4..542259d536 100644 --- a/ArduPlane/Log.cpp +++ b/ArduPlane/Log.cpp @@ -210,9 +210,9 @@ void Plane::Log_Write_Performance() struct log_Performance pkt = { LOG_PACKET_HEADER_INIT(LOG_PERFORMANCE_MSG), time_us : AP_HAL::micros64(), - loop_time : millis() - perf_mon_timer, - main_loop_count : mainLoop_count, - g_dt_max : G_Dt_max, + loop_time : millis() - perf.start_ms, + main_loop_count : perf.mainLoop_count, + g_dt_max : perf.G_Dt_max, gyro_drift_x : (int16_t)(ahrs.get_gyro_drift().x * 1000), gyro_drift_y : (int16_t)(ahrs.get_gyro_drift().y * 1000), gyro_drift_z : (int16_t)(ahrs.get_gyro_drift().z * 1000), diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h index cb875b3a0d..1ff165779f 100644 --- a/ArduPlane/Plane.h +++ b/ArduPlane/Plane.h @@ -685,23 +685,25 @@ private: // This is the time between calls to the DCM algorithm and is the Integration time for the gyros. float G_Dt = 0.02f; - // Performance monitoring - // Timer used to accrue data and trigger recording of the performanc monitoring log message - uint32_t perf_mon_timer = 0; + 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 recorded in the current performance monitoring interval - uint32_t G_Dt_max = 0; - uint32_t G_Dt_min = 0; + // The maximum and minimum main loop execution time 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 = 0; + // 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 = 0; + // 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 = 0; + // Counter of main loop executions. Used for performance monitoring and failsafe processing + uint16_t mainLoop_count; + } perf; // Camera/Antenna mount tracking and stabilisation stuff #if MOUNT == ENABLED diff --git a/ArduPlane/failsafe.cpp b/ArduPlane/failsafe.cpp index 98df797977..6486912c24 100644 --- a/ArduPlane/failsafe.cpp +++ b/ArduPlane/failsafe.cpp @@ -23,9 +23,9 @@ void Plane::failsafe_check(void) static bool in_failsafe; uint32_t tnow = micros(); - if (mainLoop_count != last_mainLoop_count) { + if (perf.mainLoop_count != last_mainLoop_count) { // the main loop is running, all is OK - last_mainLoop_count = mainLoop_count; + last_mainLoop_count = perf.mainLoop_count; last_timestamp = tnow; in_failsafe = false; return; diff --git a/ArduPlane/navigation.cpp b/ArduPlane/navigation.cpp index 3358f3b471..817a32601f 100644 --- a/ArduPlane/navigation.cpp +++ b/ArduPlane/navigation.cpp @@ -221,7 +221,7 @@ void Plane::update_fbwb_speed_height(void) elevator_input = -elevator_input; } - change_target_altitude(g.flybywire_climb_rate * elevator_input * delta_us_fast_loop * 0.0001f); + change_target_altitude(g.flybywire_climb_rate * elevator_input * perf.delta_us_fast_loop * 0.0001f); if (is_zero(elevator_input) && !is_zero(last_elevator_input)) { // the user has just released the elevator, lock in diff --git a/ArduPlane/system.cpp b/ArduPlane/system.cpp index f9dcd5b512..45c65d4ac4 100644 --- a/ArduPlane/system.cpp +++ b/ArduPlane/system.cpp @@ -632,10 +632,10 @@ void Plane::update_notify() void Plane::resetPerfData(void) { - mainLoop_count = 0; - G_Dt_max = 0; - G_Dt_min = 0; - perf_mon_timer = millis(); + perf.mainLoop_count = 0; + perf.G_Dt_max = 0; + perf.G_Dt_min = 0; + perf.start_ms = millis(); } diff --git a/ArduPlane/test.cpp b/ArduPlane/test.cpp index 49b7b79d78..2380dfc5a2 100644 --- a/ArduPlane/test.cpp +++ b/ArduPlane/test.cpp @@ -360,8 +360,8 @@ int8_t Plane::test_ins(uint8_t argc, const Menu::arg *argv) while(1) { hal.scheduler->delay(20); - if (micros() - fast_loopTimer_us > 19000UL) { - fast_loopTimer_us = micros(); + if (micros() - perf.fast_loopTimer_us > 19000UL) { + perf.fast_loopTimer_us = micros(); // INS // --- @@ -421,8 +421,8 @@ int8_t Plane::test_mag(uint8_t argc, const Menu::arg *argv) while(1) { hal.scheduler->delay(20); - if (micros() - fast_loopTimer_us > 19000UL) { - fast_loopTimer_us = micros(); + if (micros() - perf.fast_loopTimer_us > 19000UL) { + perf.fast_loopTimer_us = micros(); // INS // ---