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;