Skip to content

Commit

Permalink
AP_HAL_ESP32: more scheduler stats to help trying to find why copter …
Browse files Browse the repository at this point in the history 
…loops are slow

emits equivalent of tasks.txt to console after approx 2 minutes after boot.

eg:
TasksV2
AP_InertialSensor::update*       MIN=  63 MAX= 979 AVG= 157 OVR=  0 SLP=  0, TOT= 3.4%
Copter::run_rate_controller*     MIN= 148 MAX=1328 AVG= 247 OVR=  0 SLP=  0, TOT= 5.4%
Copter::motors_output*           MIN= 294 MAX=1369 AVG= 327 OVR=  0 SLP=  0, TOT= 7.1%
Copter::read_AHRS*               MIN=1237 MAX=6024 AVG=1991 OVR=389 SLP=  0, TOT=43.0%
Copter::read_inertia*            MIN=  64 MAX= 980 AVG= 130 OVR=  0 SLP=  0, TOT= 2.8%
Copter::check_ekf_reset*         MIN=  20 MAX= 925 AVG=  48 OVR=  0 SLP=  0, TOT= 1.1%
Copter::update_flight_mode*      MIN= 397 MAX=1884 AVG= 665 OVR=  0 SLP=  0, TOT=14.4%
Copter::update_home_from_EKF*    MIN=  10 MAX=1007 AVG=  21 OVR=  0 SLP=  0, TOT= 0.4%
Copter::update_land_and_crash_de MIN=  45 MAX=1045 AVG=  63 OVR=  0 SLP=  0, TOT= 1.4%
Copter::update_rangefinder_terra MIN=  12 MAX=1015 AVG=  22 OVR=  0 SLP=  0, TOT= 0.5%
Copter::Log_Video_Stabilisation* MIN=  17 MAX=1038 AVG=  33 OVR=  0 SLP=  0, TOT= 0.7%
Copter::rc_loop                  MIN=  57 MAX=1203 AVG=  90 OVR=  0 SLP=  0, TOT= 2.0%
Copter::throttle_loop            MIN=  47 MAX=1105 AVG=  92 OVR= 23 SLP=  0, TOT= 0.2%
AP_GPS::update                   MIN=  87 MAX=1505 AVG= 195 OVR= 56 SLP=  0, TOT= 0.5%
Copter::update_batt_compass      MIN=  68 MAX=1073 AVG= 160 OVR= 11 SLP=  0, TOT= 0.1%
RC_Channels::read_aux_all        MIN=  15 MAX= 178 AVG=  21 OVR=  1 SLP=  0, TOT= 0.0%
Copter::arm_motors_check         MIN=  14 MAX=1001 AVG=  67 OVR=  3 SLP=  0, TOT= 0.0%
Copter::auto_disarm_check        MIN=   7 MAX=  19 AVG=   8 OVR=  0 SLP=  0, TOT= 0.0%
Copter::auto_trim                MIN=   7 MAX= 154 AVG=  14 OVR=  2 SLP=  0, TOT= 0.0%
Copter::read_rangefinder         MIN=  56 MAX=1052 AVG= 126 OVR= 12 SLP=  0, TOT= 0.1%
AP_Proximity::update             MIN=  10 MAX=1075 AVG=  54 OVR= 50 SLP=  0, TOT= 0.5%
AP_Beacon::update                MIN=  10 MAX=1084 AVG=  41 OVR=162 SLP=  0, TOT= 0.8%
Copter::update_altitude          MIN= 165 MAX=1479 AVG= 310 OVR= 45 SLP=  0, TOT= 0.1%
Copter::run_nav_updates          MIN=   9 MAX=1065 AVG=  19 OVR=  4 SLP=  0, TOT= 0.1%
Copter::update_throttle_hover    MIN=   6 MAX= 992 AVG=  18 OVR=  8 SLP=  0, TOT= 0.1%
ModeSmartRTL::save_position      MIN=  13 MAX=  43 AVG=  32 OVR=  0 SLP=  0, TOT= 0.0%
AC_Sprayer::update               MIN=  10 MAX=  11 AVG=  10 OVR=  0 SLP=  0, TOT= 0.0%
Copter::three_hz_loop            MIN=  31 MAX=  35 AVG=  33 OVR=  0 SLP=  0, TOT= 0.0%
AP_ServoRelayEvents::update_even MIN=   7 MAX=1022 AVG=  23 OVR=  7 SLP=  0, TOT= 0.1%
AP_Baro::accumulate              MIN=   8 MAX= 149 AVG=  11 OVR=  2 SLP=  0, TOT= 0.0%
Copter::update_precland          MIN=  11 MAX=1083 AVG=  38 OVR=162 SLP=  0, TOT= 0.7%
Copter::loop_rate_logging        MIN=   9 MAX=1041 AVG=  17 OVR=119 SLP=  0, TOT= 0.3%
Compass::cal_update              MIN=  16 MAX= 995 AVG=  31 OVR=  7 SLP=  0, TOT= 0.1%
AP_Notify::update                MIN=   6 MAX= 627 AVG=  13 OVR=  1 SLP=  0, TOT= 0.0%
Copter::one_hz_loop              MIN= 187 MAX= 663 AVG= 284 OVR=  5 SLP=  0, TOT= 0.0%
Copter::ekf_check                MIN=  11 MAX=1056 AVG=  60 OVR=  3 SLP=  0, TOT= 0.0%
Copter::check_vibration          MIN=  48 MAX=1031 AVG= 104 OVR= 26 SLP=  0, TOT= 0.1%
Copter::gpsglitch_check          MIN=  13 MAX= 179 AVG=  23 OVR=  1 SLP=  0, TOT= 0.0%
Copter::takeoff_check            MIN=   3 MAX= 164 AVG=  11 OVR=  2 SLP=  0, TOT= 0.0%
Copter::standby_update           MIN=   6 MAX= 318 AVG=  16 OVR=  9 SLP=  0, TOT= 0.1%
Copter::lost_vehicle_check       MIN=  17 MAX= 960 AVG=  40 OVR=  1 SLP=  0, TOT= 0.0%
GCS::update_receive              MIN=  90 MAX=2048 AVG= 201 OVR=442 SLP=  0, TOT= 4.1%
GCS::update_send                 MIN= 157 MAX=1688 AVG= 379 OVR=705 SLP=  0, TOT= 5.9%
Copter::ten_hz_logging_loop      MIN= 456 MAX=1654 AVG= 631 OVR= 45 SLP=  0, TOT= 0.3%
Copter::twentyfive_hz_logging    MIN=  14 MAX= 370 AVG=  32 OVR=  5 SLP=  0, TOT= 0.0%
AP_Logger::periodic_tasks        MIN=  74 MAX=1142 AVG= 125 OVR=447 SLP=  0, TOT= 2.2%
AP_InertialSensor::periodic      MIN=   5 MAX= 357 AVG=  12 OVR=133 SLP=  0, TOT= 0.2%
AP_Scheduler::update_logging     MIN=  23 MAX=  23 AVG=  23 OVR=  0 SLP=  0, TOT= 0.0%
AP_TempCalibration::update       MIN=   6 MAX=   8 AVG=   6 OVR=  0 SLP=  0, TOT= 0.0%
AP_Winch::update                 MIN=   9 MAX=  29 AVG=  13 OVR=  0 SLP=  0, TOT= 0.0%
AP_Stats::update                 MIN=  26 MAX=  31 AVG=  28 OVR=  0 SLP=  0, TOT= 0.0%
AP_NMEA_Output::update           MIN=   5 MAX= 338 AVG=  10 OVR=  2 SLP=  0, TOT= 0.0%
AP_RunCam::update                MIN=   5 MAX= 128 AVG=   6 OVR=  1 SLP=  0, TOT= 0.0%
AP_Vehicle::update_dynamic_notch MIN=  10 MAX=1020 AVG=  22 OVR=299 SLP=  0, TOT= 0.4%
AP_VideoTX::update               MIN=   5 MAX=   7 AVG=   6 OVR=  0 SLP=  0, TOT= 0.0%
AP_Tramp::update                 MIN=   7 MAX= 944 AVG=  17 OVR=  5 SLP=  0, TOT= 0.0%
AP_Vehicle::send_watchdog_reset_ MIN=   9 MAX=   9 AVG=   9 OVR=  0 SLP=  0, TOT= 0.0%
AP_Vehicle::publish_osd_info     MIN=  37 MAX=  41 AVG=  39 OVR=  5 SLP=  0, TOT= 0.0%
AP_TemperatureSensor::update     MIN=   6 MAX=  16 AVG=   8 OVR=  0 SLP=  0, TOT= 0.0%
AP_Vehicle::accel_cal_update     MIN=  16 MAX=  27 AVG=  19 OVR=  0 SLP=  0, TOT= 0.0%
AP_AIS::update                   MIN=   7 MAX=  16 AVG=   9 OVR=  0 SLP=  0, TOT= 0.0%
AP_EFI::update                   MIN=   4 MAX= 152 AVG=   9 OVR=  0 SLP=  0, TOT= 0.0%
AP_Vehicle::one_Hz_update        MIN=  13 MAX= 130 AVG=  37 OVR=  1 SLP=  0, TOT= 0.0%
AP_Filters::update               MIN=   9 MAX=  10 AVG=   9 OVR=  0 SLP=  0, TOT= 0.0%
AP_Vehicle::update_arming        MIN=  79 MAX=  83 AVG=  81 OVR=  5 SLP=  0, TOT= 0.0%

loop_rate: actual: 161Hz, expected: 400Hz
  • Loading branch information
@davidbuzz
davidbuzz committed Jan 8, 2024
1 parent 3a37796 commit 818aa84
Show file tree
Hide file tree
Showing 2 changed files with 135 additions and 0 deletions.
133 changes: 133 additions & 0 deletions libraries/AP_HAL_ESP32/Scheduler.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -30,6 +30,8 @@
#include <AP_HAL/AP_HAL.h>
#include <AP_Scheduler/AP_Scheduler.h>
#include <stdio.h>
#include <AP_Common/ExpandingString.h>


//#define SCHEDULERDEBUG 1

Expand Down Expand Up @@ -527,6 +529,9 @@ uint16_t Scheduler::get_loop_rate_hz(void)
return _active_loop_rate_hz;
}


#define STATS_TICKS pdMS_TO_TICKS(1000)

// once every 60 seconds, print some stats...
void Scheduler::print_stats(void)
{
Expand All @@ -537,11 +542,139 @@ void Scheduler::print_stats(void)
printf("\n\n%s\n", buffer);
heap_caps_print_heap_info(0);
last_run = AP_HAL::millis64();

print_real_time_stats(STATS_TICKS);
printf("Real time stats obtained\n");

ExpandingString *sstr = new ExpandingString;
AP::scheduler().task_info(*sstr);
printf("\n\n%s\n", sstr->get_string());
}

// printf("loop_rate_hz: %d",get_loop_rate_hz());
}

#define ARRAY_SIZE_OFFSET 5 //Increase this if print_real_time_stats returns ESP_ERR_INVALID_SIZE

/**
* @brief Function to print the CPU usage of tasks over a given duration.
*
* This function will measure and print the CPU usage of tasks over a specified
* number of ticks (i.e. real time stats). This is implemented by simply calling
* uxTaskGetSystemState() twice separated by a delay, then calculating the
* differences of task run times before and after the delay.
*
* @note If any tasks are added or removed during the delay, the stats of
* those tasks will not be printed.
* @note This function should be called from a high priority task to minimize
* inaccuracies with delays.
* @note When running in dual core mode, each core will correspond to 50% of
* the run time.
*
* @param xTicksToWait Period of stats measurement
*
* @return
* - ESP_OK Success
* - ESP_ERR_NO_MEM Insufficient memory to allocated internal arrays
* - ESP_ERR_INVALID_SIZE Insufficient array size for uxTaskGetSystemState. Trying increasing ARRAY_SIZE_OFFSET
* - ESP_ERR_INVALID_STATE Delay duration too short
*/
// from :
// https://github.com/espressif/esp-idf/blob/56123c52aaa08f1b53350c7af30c91320b352ef4/examples/system/freertos/real_time_stats/main/real_time_stats_example_main.c#L160
void Scheduler::print_real_time_stats(TickType_t xTicksToWait)
{
TaskStatus_t *start_array = NULL, *end_array = NULL;
UBaseType_t start_array_size, end_array_size;
uint32_t start_run_time, end_run_time;
//esp_err_t ret;

//Allocate array to store current task states
start_array_size = uxTaskGetNumberOfTasks() + ARRAY_SIZE_OFFSET;
start_array = (TaskStatus_t*)malloc(sizeof(TaskStatus_t) * start_array_size);
if (start_array == NULL) {
printf("ESP_ERR_NO_MEM\n");
free(start_array);
free(end_array);
return;
}
//Get current task states
start_array_size = uxTaskGetSystemState(start_array, start_array_size, &start_run_time);
if (start_array_size == 0) {
printf("ESP_ERR_INVALID_SIZE\n");
free(start_array);
free(end_array);
return;
}

vTaskDelay(xTicksToWait);

//Allocate array to store tasks states post delay
end_array_size = uxTaskGetNumberOfTasks() + ARRAY_SIZE_OFFSET;
end_array = (TaskStatus_t*)malloc(sizeof(TaskStatus_t) * end_array_size);
if (end_array == NULL) {
printf("ESP_ERR_NO_MEM\n");
free(start_array);
free(end_array);
return;
}
//Get post delay task states
end_array_size = uxTaskGetSystemState(end_array, end_array_size, &end_run_time);
if (end_array_size == 0) {
printf("ESP_ERR_INVALID_SIZE\n");
free(start_array);
free(end_array);
return;
}

//Calculate total_elapsed_time in units of run time stats clock period.
uint32_t total_elapsed_time = (end_run_time - start_run_time);
if (total_elapsed_time == 0) {
printf("ESP_ERR_INVALID_STATE\n");
free(start_array);
free(end_array);
return;
}

printf("| Task | Run Time | Percentage\n");
//Match each task in start_array to those in the end_array
for (int i = 0; i < start_array_size; i++) {
int k = -1;
for (int j = 0; j < end_array_size; j++) {
if (start_array[i].xHandle == end_array[j].xHandle) {
k = j;
//Mark that task have been matched by overwriting their handles
start_array[i].xHandle = NULL;
end_array[j].xHandle = NULL;
break;
}
}
//Check if matching task found
if (k >= 0) {
uint32_t task_elapsed_time = end_array[k].ulRunTimeCounter - start_array[i].ulRunTimeCounter;
uint32_t percentage_time = (task_elapsed_time * 100UL) / (total_elapsed_time * portNUM_PROCESSORS);
printf("| %s | %" PRIu32 " | %" PRIu32 "%%\n", start_array[i].pcTaskName, task_elapsed_time, percentage_time);
}
}

//Print unmatched tasks
for (int i = 0; i < start_array_size; i++) {
if (start_array[i].xHandle != NULL) {
printf("| %s | Deleted\n", start_array[i].pcTaskName);
}
}
for (int i = 0; i < end_array_size; i++) {
if (end_array[i].xHandle != NULL) {
printf("| %s | Created\n", end_array[i].pcTaskName);
}
}
//ret = ESP_OK;

//exit: //Common return path
free(start_array);
free(end_array);
//return ret;
}

// Run every 10s
void Scheduler::print_main_loop_rate(void)
{
Expand Down
2 changes: 2 additions & 0 deletions libraries/AP_HAL_ESP32/Scheduler.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -50,6 +50,8 @@ class ESP32::Scheduler : public AP_HAL::Scheduler

void print_stats(void) ;
void print_main_loop_rate(void);
void print_real_time_stats(TickType_t xTicksToWait);


uint16_t get_loop_rate_hz(void);
AP_Int16 _active_loop_rate_hz;
Expand Down

0 comments on commit 818aa84

Please sign in to comment.