Line data    Source code

       1             : #include "AP_Scheduler_config.h"
       2             : 
       3             : #if AP_SCHEDULER_ENABLED
       4             : 
       5             : #include "PerfInfo.h"
       6             : 
       7             : #include <AP_Logger/AP_Logger.h>
       8             : #include <GCS_MAVLink/GCS.h>
       9             : #include <AP_InternalError/AP_InternalError.h>
      10             : #include "AP_Scheduler.h"
      11             : 
      12             : extern const AP_HAL::HAL& hal;
      13             : 
      14             : //
      15             : //  high level performance monitoring
      16             : //
      17             : //  we measure the main loop time
      18             : //
      19             : 
      20             : // loops over time by this amount or more won't be counted in filtered loop time (and thus loop rate)
      21             : #ifndef AP_SCHEDULER_OVERTIME_MARGIN_US
      22             : #define AP_SCHEDULER_OVERTIME_MARGIN_US 10000UL
      23             : #endif
      24             : 
      25             : // reset - reset all records of loop time to zero
      26       21508 : void AP::PerfInfo::reset()
      27             : {
      28       21508 :     loop_count = 0;
      29       21508 :     max_time = 0;
      30       21508 :     min_time = 0;
      31       21508 :     long_running = 0;
      32       21508 :     sigma_time = 0;
      33       21508 :     sigmasquared_time = 0;
      34       21508 :     if (_task_info != nullptr) {
      35          12 :         memset(_task_info, 0, (_num_tasks) * sizeof(TaskInfo));
      36             :     }
      37       21508 : }
      38             : 
      39             : // ignore_loop - ignore this loop from performance measurements (used to reduce false positive when arming)
      40         599 : void AP::PerfInfo::ignore_this_loop()
      41             : {
      42         599 :     ignore_loop = true;
      43         599 : }
      44             : 
      45             : // allocate the array of task statistics for use by @SYS/tasks.txt
      46           1 : void AP::PerfInfo::allocate_task_info(uint8_t num_tasks)
      47             : {
      48           1 :     _task_info = NEW_NOTHROW TaskInfo[num_tasks];
      49           1 :     if (_task_info == nullptr) {
      50           0 :         DEV_PRINTF("Unable to allocate scheduler TaskInfo\n");
      51           0 :         _num_tasks = 0;
      52           0 :         return;
      53             :     }
      54           1 :     _num_tasks = num_tasks;
      55             : }
      56             : 
      57           1 : void AP::PerfInfo::free_task_info()
      58             : {
      59           1 :     delete[] _task_info;
      60           1 :     _task_info = nullptr;
      61           1 :     _num_tasks = 0;
      62           1 : }
      63             : 
      64             : // called after each run of a task to update its statistics based on measurements taken by the scheduler
      65  1133017028 : void AP::PerfInfo::update_task_info(uint8_t task_index, uint16_t task_time_us, bool overrun)
      66             : {
      67  1133017028 :     if (_task_info == nullptr) {
      68  1131646844 :         return;
      69             :     }
      70             : 
      71     1370184 :     if (task_index >= _num_tasks) {
      72           0 :         INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
      73           0 :         return;
      74             :     }
      75     1370184 :     TaskInfo& ti = _task_info[task_index];
      76     1370184 :     ti.update(task_time_us, overrun);
      77             : }
      78             : 
      79     1370184 : void AP::PerfInfo::TaskInfo::update(uint16_t task_time_us, bool overrun)
      80             : {
      81     1370184 :     max_time_us = MAX(max_time_us, task_time_us);
      82     1370184 :     if (min_time_us == 0) {
      83     1370184 :         min_time_us = task_time_us;
      84             :     } else {
      85           0 :         min_time_us = MIN(min_time_us, task_time_us);
      86             :     }
      87     1370184 :     elapsed_time_us += task_time_us;
      88     1370184 :     tick_count++;
      89     1370184 :     if (overrun) {
      90           0 :         overrun_count++;
      91             :     }
      92     1370184 : }
      93             : 
      94         170 : void AP::PerfInfo::TaskInfo::print(const char* task_name, uint32_t total_time, ExpandingString& str) const
      95             : {
      96         170 :     uint16_t avg = 0;
      97         170 :     float pct = 0.0f;
      98         170 :     if (tick_count > 0) {
      99         170 :         pct = elapsed_time_us * 100.0f / total_time;
     100         170 :         avg = MIN(uint16_t(elapsed_time_us / tick_count), 9999);
     101             :     }
     102             : #if AP_SCHEDULER_EXTENDED_TASKINFO_ENABLED
     103         170 :     const char* fmt = "%-32.32s MIN=%4u MAX=%4u AVG=%4u OVR=%3u SLP=%3u, TOT=%4.1f%%\n";
     104             : #else
     105             :     const char* fmt = "%-16.16s MIN=%4u MAX=%4u AVG=%4u OVR=%3u SLP=%3u, TOT=%4.1f%%\n";
     106             : #endif
     107         170 :     str.printf(fmt, task_name,
     108         170 :                 unsigned(MIN(min_time_us, 9999)), unsigned(MIN(max_time_us, 9999)), unsigned(avg),
     109         170 :                 unsigned(MIN(overrun_count, 999)), unsigned(MIN(slip_count, 999)), pct);
     110         170 : }
     111             : 
     112             : // check_loop_time - check latest loop time vs min, max and overtime threshold
     113    41684622 : void AP::PerfInfo::check_loop_time(uint32_t time_in_micros)
     114             : {
     115    41684622 :     loop_count++;
     116             : 
     117             :     // exit if this loop should be ignored
     118    41684622 :     if (ignore_loop) {
     119         599 :         ignore_loop = false;
     120         599 :         return;
     121             :     }
     122             : 
     123    41684023 :     if( time_in_micros > max_time) {
     124       40636 :         max_time = time_in_micros;
     125             :     }
     126    41684023 :     if( min_time == 0 || time_in_micros < min_time) {
     127       25876 :         min_time = time_in_micros;
     128             :     }
     129    41684023 :     if (time_in_micros > overtime_threshold_micros) {
     130       59673 :         long_running++;
     131             :     }
     132    41684023 :     sigma_time += time_in_micros;
     133    41684023 :     sigmasquared_time += time_in_micros * time_in_micros;
     134             : 
     135             :     /* we keep a filtered loop time for use as G_Dt which is the
     136             :        predicted time for the next loop. We remove really excessive
     137             :        times from this calculation so as not to throw it off too far
     138             :        in case we get a single long loop
     139             : 
     140             :        Note that the time we use here is the time between calls to
     141             :        check_loop_time() not the time from loop start to loop
     142             :        end. This is because we are using the time for time between
     143             :        calls to controllers, which has nothing to do with cpu speed.
     144             :     */
     145    41684023 :     const uint32_t now = AP_HAL::micros();
     146    41684023 :     const uint32_t loop_time_us = now - last_check_us;
     147    41684023 :     last_check_us = now;
     148    41684023 :     if (loop_time_us < overtime_threshold_micros + AP_SCHEDULER_OVERTIME_MARGIN_US) {
     149    41678872 :         filtered_loop_time = 0.99f * filtered_loop_time + 0.01f * loop_time_us * 1.0e-6f;
     150             :     } else {
     151             :         // esp32 is most likely to regularly trigger long loops, might be
     152             :         // helpful for bringup of other boards too
     153             : #if CONFIG_HAL_BOARD == HAL_BOARD_ESP32
     154             : #ifdef SCHEDDEBUG
     155             :         DEV_PRINTF("way overtime: %dus\n", loop_time_us);
     156             : #endif
     157             : #endif
     158             :     }
     159             : }
     160             : 
     161             : // get_num_loops: return number of loops used for recording performance
     162       18657 : uint16_t AP::PerfInfo::get_num_loops() const
     163             : {
     164       18657 :     return loop_count;
     165             : }
     166             : 
     167             : // get_max_time - return maximum loop time (in microseconds)
     168       18657 : uint32_t AP::PerfInfo::get_max_time() const
     169             : {
     170       18657 :     return max_time;
     171             : }
     172             : 
     173             : // get_min_time - return minumum loop time (in microseconds)
     174           0 : uint32_t AP::PerfInfo::get_min_time() const
     175             : {
     176           0 :     return min_time;
     177             : }
     178             : 
     179             : // get_num_long_running - get number of long running loops
     180       18657 : uint16_t AP::PerfInfo::get_num_long_running() const
     181             : {
     182       18657 :     return long_running;
     183             : }
     184             : 
     185             : // get_avg_time - return average loop time (in microseconds)
     186           0 : uint32_t AP::PerfInfo::get_avg_time() const
     187             : {
     188           0 :     return (sigma_time / loop_count);
     189             : }
     190             : 
     191             : // get_stddev_time - return stddev of average loop time (in us)
     192           0 : uint32_t AP::PerfInfo::get_stddev_time() const
     193             : {
     194           0 :     return sqrtf((sigmasquared_time - (sigma_time*sigma_time)/loop_count) / loop_count);
     195             : }
     196             : 
     197             : // get_filtered_time - return low pass filtered loop time in seconds
     198     1588453 : float AP::PerfInfo::get_filtered_time() const
     199             : {
     200     1588453 :     return filtered_loop_time;
     201             : }
     202             : 
     203             : // return low pass filtered loop rate in hz
     204     1584089 : float AP::PerfInfo::get_filtered_loop_rate_hz() const
     205             : {
     206     1584089 :     const float filt_time_s = get_filtered_time();
     207     1584089 :     if (filt_time_s <= 0) {
     208           0 :         return loop_rate_hz;
     209             :     }
     210     1584089 :     return 1.0 / filt_time_s;
     211             : }
     212             : 
     213             : 
     214           0 : void AP::PerfInfo::update_logging() const
     215             : {
     216           0 :     GCS_SEND_TEXT(MAV_SEVERITY_INFO,
     217             :                     "PERF: %u/%u [%lu:%lu] F=%uHz sd=%lu Ex=%lu",
     218             :                     (unsigned)get_num_long_running(),
     219             :                     (unsigned)get_num_loops(),
     220             :                     (unsigned long)get_max_time(),
     221             :                     (unsigned long)get_min_time(),
     222             :                     (unsigned)(0.5+get_filtered_loop_rate_hz()),
     223             :                     (unsigned long)get_stddev_time(),
     224             :                     (unsigned long)AP::scheduler().get_extra_loop_us());
     225           0 : }
     226             : 
     227       21508 : void AP::PerfInfo::set_loop_rate(uint16_t rate_hz)
     228             : {
     229             :     // allow a 20% overrun before we consider a loop "slow":
     230       21508 :     overtime_threshold_micros = 1000000/rate_hz * 1.2f;
     231             : 
     232       21508 :     if (loop_rate_hz != rate_hz) {
     233        2094 :         loop_rate_hz = rate_hz;
     234        2094 :         filtered_loop_time = 1.0f / rate_hz;
     235             :     }
     236       21508 : }
     237             : 
     238             : #endif  // AP_SCHEDULER_ENABLED