AP_HAL_ESP32: more scheduler stats to help trying to find why copter…

libraries/AP_HAL_ESP32/Scheduler.cpp

@@ -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
 
@@ -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)
 {

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;