From 818aa84c4ca5dc6e2183c826d4e2e2df013ccbd6 Mon Sep 17 00:00:00 2001 From: David Buzz Date: Sat, 6 Jan 2024 15:35:58 +1000 Subject: [PATCH] AP_HAL_ESP32: more scheduler stats to help trying to find why copter 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 --- libraries/AP_HAL_ESP32/Scheduler.cpp | 133 +++++++++++++++++++++++++++ libraries/AP_HAL_ESP32/Scheduler.h | 2 + 2 files changed, 135 insertions(+) diff --git a/libraries/AP_HAL_ESP32/Scheduler.cpp b/libraries/AP_HAL_ESP32/Scheduler.cpp index 976b7245fdd484..ef05396ec94b62 100644 --- a/libraries/AP_HAL_ESP32/Scheduler.cpp +++ b/libraries/AP_HAL_ESP32/Scheduler.cpp @@ -30,6 +30,8 @@ #include #include #include +#include + //#define SCHEDULERDEBUG 1 @@ -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) { @@ -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) { diff --git a/libraries/AP_HAL_ESP32/Scheduler.h b/libraries/AP_HAL_ESP32/Scheduler.h index 237f9532eaef2d..18dccf9f31908c 100644 --- a/libraries/AP_HAL_ESP32/Scheduler.h +++ b/libraries/AP_HAL_ESP32/Scheduler.h @@ -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;