[log timing] add time logs for tasks.

(cherry picked from commit 72b51e6)

sources/include/log_timings.h

@@ -0,0 +1,82 @@
+#ifndef LOG_TIMING_H
+#define LOG_TIMING_H
+
+/* The purpose of log timing is to measure:
+ * - time of each interrupt/task (=proc)
+ * - periodicity of procs
+ * It is useful during dev only, as it consums a few resources
+ *
+ * Hardware required: TIM6
+ * serial output    : uart2
+ *
+ * in 'native mode', no behavior
+ */
+
+/* if the following line is commented, there is no log
+ * i.e, #define are expanded in nothing
+ */
+#define LOG_TIME
+
+/* number of entries. 3 bytes per entry */
+#define LOG_TIME_SIZE     1024
+
+#define LOG_TIME_NB_TASKS 4
+
+/* use msb of the id for start/stop */
+#define LOG_TIME_EVENT_START 0x80
+#define LOG_TIME_EVENT_STOP  0x00
+
+/* id. 7 bits allowed */
+#define LOG_TIME_TASK_BREATHING   0x0
+#define LOG_TIME_TASK_MONITORING  0x1
+#define LOG_TIME_TASK_CONTROLLER  0x2
+#define LOG_TIME_TASK_HMI         0x3
+
+#ifdef LOG_TIME
+  #include <FreeRTOS.h> 
+  #include <task.h> /*we need  TaskHandle_t */
+
+  #ifdef native
+    void log_time_initHw()      {}
+    void log_time_start()       {}
+    void log_time_event(int id) {UNUSED(id);}
+    void log_time_dump()        {}
+    int  log_time_full()        {return 0;}
+
+    void log_time_init_task(const char *name, int id)  {UNUSED(name); UNUSED(id)   ;}
+    void log_time_event_task(TaskHandle_t t,int start) {UNUSED(t)   ; UNUSED(start);}
+  #else /* stm32 target => implementation in lowlevel/stm32f303/log_timings.h */
+    void log_time_initHw();
+    void log_time_start();
+    void log_time_event(int id);
+    void log_time_dump();
+    int  log_time_full();
+
+    void log_time_init_task(const char *name, int id);
+    void log_time_event_task(TaskHandle_t t,int start);
+  #endif /* native */
+
+  /* should be called */
+  #define LOG_TIME_INIT_TIM6_HARDWARE() log_time_initHw();
+  #define LOG_TIME_START()              log_time_start();
+  #define LOG_TIME_EVENT(id)            log_time_event(id);
+  #define LOG_TIME_DUMP()               log_time_dump();
+  #define LOG_TIME_FULL()               log_time_full() /* no ';' here */
+
+  #define LOG_TIME_INIT_TASK(name,id)   log_time_init_task(name,id);
+  #define LOG_TIME_EVENT_TASK(t,s)      log_time_event_task(t,s);
+
+#else /* no log */
+
+  #define LOG_TIME_INIT_TIM6_HARDWARE() 
+  #define LOG_TIME_START()
+  #define LOG_TIME_EVENT(id)
+  #define LOG_TIME_DUMP()
+  #define LOG_TIME_FULL()               0
+
+  #define LOG_TIME_INIT_TASK(name,id)
+  #define LOG_TIME_EVENT_TASK(t,s)
+
+#endif /* LOG_TIME */
+
+#endif

sources/platforms/recovid_revB/CMakeLists.txt

@@ -21,6 +21,7 @@ add_library(recovid_revB OBJECT
     pep_valve.c
     gpio.c
     bmp280.c
+	log_timings.c
 )
 
 target_include_directories(recovid_revB PUBLIC 

sources/platforms/recovid_revB/FreeRTOSConfig.h

@@ -67,6 +67,7 @@ to exclude the API function. */
 #define INCLUDE_xQueueGetMutexHolder        1
 #define INCLUDE_uxTaskGetStackHighWaterMark 1
 #define INCLUDE_eTaskGetState               1
+#define INCLUDE_xTaskGetHandle              1
 
 /*
  * The CMSIS-RTOS V2 FreeRTOS wrapper is dependent on the heap implementation used

sources/platforms/recovid_revB/log_timings.c

@@ -0,0 +1,91 @@
+#include <stdint.h>
+#include <stdio.h>
+#include "stm32f3xx.h"
+#include "log_timings.h"
+#include "common.h"
+#include <string.h> /* strlen */
+
+typedef struct
+{
+	uint8_t  id;    /* id: MSB=1 start, else end. 7 low bits = id. */
+	uint16_t date;  /* date in us from TIM6. there are overflows   */
+} log_time_entry;
+
+/* save logs before sending externally (through uart..)*/
+/* 3 bytes per entry */
+volatile log_time_entry log_time_tab[LOG_TIME_SIZE];
+volatile int log_time_current_size; /* current size */
+
+
+void log_time_initHw(){
+    //start TIM6
+    RCC->APB1ENR |= RCC_APB1ENR_TIM6EN;
+    __asm("nop");
+    RCC->APB1RSTR |=  RCC_APB1RSTR_TIM6RST;
+    RCC->APB1RSTR &= ~RCC_APB1RSTR_TIM6RST;
+    __asm("nop");
+
+    //configure TIM6
+    TIM6->PSC = 72-1;     //tick@1us
+    TIM6->EGR = TIM_EGR_UG; //update event => load PSC
+    TIM6->CR1 = TIM_CR1_CEN; //enable, all other fields to 0
+
+	log_time_current_size = 0;
+}
+
+void log_time_event(int id)
+{
+    __disable_irq();
+	const uint16_t date = TIM6->CNT;
+	if(log_time_current_size < LOG_TIME_SIZE) {
+		log_time_tab[log_time_current_size].id   = id;
+		log_time_tab[log_time_current_size].date = date;
+		log_time_current_size++;
+	}
+    __enable_irq();
+}
+
+void log_time_start()
+{
+	log_time_current_size = 0;
+}
+
+void log_time_dump()
+{
+	const int size = log_time_current_size;
+	dbg_printf("dump log timings\r\n");
+	for(int i = 0;i<size;i++)
+	{
+		char c;
+		if(log_time_tab[i].id & LOG_TIME_EVENT_START) c = 's';
+		else c = 'e';
+		dbg_printf("%c\t%d\t%d\r\n",c,log_time_tab[i].id & 0x7F,log_time_tab[i].date);
+	}
+}
+
+int log_time_full()
+{
+	return (log_time_current_size == LOG_TIME_SIZE);
+}
+
+/* store the task handle of tasks */
+TaskHandle_t idToTaskHandle[4];
+void log_time_init_task(const char *name, int id)
+{
+	TaskHandle_t handle = xTaskGetHandle(name);
+    if(id < LOG_TIME_NB_TASKS) {
+		idToTaskHandle[id] = handle;
+	}
+}
+
+void log_time_event_task(TaskHandle_t t,int start)
+{
+	int i=0;
+	int found = 0;
+	while(i<LOG_TIME_NB_TASKS && !found)
+	{
+		found = (t==idToTaskHandle[i]);
+		if(!found) i++;
+	}
+	if(found) log_time_event(start | i);
+}

sources/platforms/recovid_revB/recovid_revB.c

@@ -1,5 +1,7 @@
 #include "recovid_revB.h"
 #include "platform_config.h"
+#include "log_timings.h"
+#include <stdbool.h>
 
 
 I2C_HandleTypeDef hi2c1;
@@ -61,6 +63,7 @@ void main()
   MX_TIM2_Init();
   MX_TIM3_Init();
   MX_TIM7_Init();
+  LOG_TIME_INIT_TIM6_HARDWARE()
 
   // Start the controller
   // This function should never returns

sources/platforms/recovid_revB/sensors.c

@@ -2,6 +2,7 @@
 #include "platform.h"
 #include "platform_config.h"
 #include "bmp280.h"
+#include "log_timings.h"
 #include <string.h>
 
 /*

sources/platforms/recovid_revB/time.c

@@ -1,6 +1,7 @@
 #include "recovid_revB.h"
 #include "platform.h"
 #include "platform_config.h"
+#include "log_timings.h"
 
 #include <FreeRTOS.h>
 #include <task.h>
@@ -12,10 +13,13 @@ uint32_t get_time_ms()
 
 uint32_t wait_ms(uint32_t t_ms)
 {
-  if(xTaskGetCurrentTaskHandle() != NULL)
+  TaskHandle_t handle = xTaskGetCurrentTaskHandle();
+  if(handle != NULL) {
+    LOG_TIME_EVENT_TASK(handle, LOG_TIME_EVENT_STOP)
     vTaskDelay(t_ms/portTICK_PERIOD_MS);
-  else {
+    LOG_TIME_EVENT_TASK(handle, LOG_TIME_EVENT_START)
+  } else {
   	HAL_Delay(t_ms);
   }
   return get_time_ms();
-}
+}

sources/platforms/simu/main.c

@@ -3,7 +3,14 @@
 #include "breathing.h"
 #include "monitoring.h"
 #include "hmi.h"
+#include "platform.h"
+#include "log_timings.h"
 
+#define PRIORITY_LOW            8
+#define PRIORITY_BELOW_NORMAL   16
+#define PRIORITY_NORMAL         24
+#define PRIORITY_ABOVE_NORMAL   32
+#define PRIORITY_HIGH           40
 
 
 TaskHandle_t breathingTaskHandle;
@@ -56,6 +63,11 @@ int main(int argc, char *argv)
     HardFault_Handler();
   }
 
+  LOG_TIME_INIT_TASK("Breathing"  , LOG_TIME_TASK_BREATHING)
+  LOG_TIME_INIT_TASK("Monitoring" , LOG_TIME_TASK_MONITORING)
+  LOG_TIME_INIT_TASK("Controller" , LOG_TIME_TASK_CONTROLLER)
+  LOG_TIME_INIT_TASK("HMI"        , LOG_TIME_TASK_HMI)
+
   // start scheduler
   vTaskStartScheduler();
 

sources/src/breathing.c

@@ -1,6 +1,7 @@
 #include "controller.h"
 #include "breathing.h"
 #include "platform.h"
+#include "log_timings.h"
 
 #include <compute_motor.h>
 #include <math.h>
@@ -54,10 +55,12 @@ static void enter_state(BreathingState newState);
 void breathing_run(void *args) {
   UNUSED(args);
   EventBits_t events;
-
+  LOG_TIME_EVENT(LOG_TIME_EVENT_START | LOG_TIME_TASK_BREATHING)
   while(true) {
     brth_printf("BRTH: Standby\n");
+    LOG_TIME_EVENT(LOG_TIME_EVENT_STOP | LOG_TIME_TASK_BREATHING)
     events= xEventGroupWaitBits(ctrlEventFlags, BREATHING_RUN_FLAG, pdFALSE, pdTRUE, portMAX_DELAY );
+    LOG_TIME_EVENT(LOG_TIME_EVENT_START | LOG_TIME_TASK_BREATHING)
     brth_printf("BRTH: Started\n");
 
     // _flow_samples_count=0;
@@ -177,7 +180,6 @@ void breathing_run(void *args) {
 
     wait_ms(200);
     xEventGroupSetBits(ctrlEventFlags, BREATHING_STOPPED_FLAG);
-
   }
 }
 

sources/src/controller.c

@@ -3,6 +3,7 @@
 #include "monitoring.h"
 #include "platform.h"
 #include "defaults.h"
+#include "log_timings.h"
 
 
 #include <math.h>
@@ -56,6 +57,11 @@ void controller_main()
     return;
   }
 
+  LOG_TIME_INIT_TASK("Breathing"  , LOG_TIME_TASK_BREATHING)
+  LOG_TIME_INIT_TASK("Monitoring" , LOG_TIME_TASK_MONITORING)
+  LOG_TIME_INIT_TASK("Controller" , LOG_TIME_TASK_CONTROLLER)
+  LOG_TIME_INIT_TASK("HMI"        , LOG_TIME_TASK_HMI)
+
   // start scheduler
   vTaskStartScheduler();
 
@@ -111,6 +117,7 @@ static int self_tests();
 
 void controller_run(void* args) {
   UNUSED(args);
+  LOG_TIME_EVENT(LOG_TIME_EVENT_START | LOG_TIME_TASK_CONTROLLER)
 
   EventBits_t events;
 
@@ -119,13 +126,13 @@ void controller_run(void* args) {
 #endif
 
   while(true) {
+
 
     // TODO: Implemente the actual startup process
 
     ctrl_printf("CTRL: Waiting for failsafe signal\n");
     while(is_Failsafe_Enabled()) wait_ms(200);
 
-
     ctrl_printf("CTRL: Initializing system\n");
 
     reset_settings();
@@ -159,7 +166,6 @@ void controller_run(void* args) {
       // TODO Implement controller logic
       // check monitoring process
       // check breathing process
-
       wait_ms(20);
     }
 
@@ -170,7 +176,9 @@ void controller_run(void* args) {
     ctrl_printf("CTRL: Waiting for breathing, monitoring and hmi tasks to stop\n");
     do {
       // TODO implement retry logic and eventually kill tasks !!
+      LOG_TIME_EVENT(LOG_TIME_EVENT_STOP  | LOG_TIME_TASK_CONTROLLER)
       sync = xEventGroupWaitBits(ctrlEventFlags, (BREATHING_STOPPED_FLAG | MONITORING_STOPPED_FLAG | HMI_STOPPED_FLAG), pdTRUE, pdTRUE, 200/portTICK_PERIOD_MS);
+      LOG_TIME_EVENT(LOG_TIME_EVENT_START | LOG_TIME_TASK_CONTROLLER)
     } while( (sync & (BREATHING_STOPPED_FLAG | MONITORING_STOPPED_FLAG | HMI_STOPPED_FLAG)) != (BREATHING_STOPPED_FLAG | MONITORING_STOPPED_FLAG | HMI_STOPPED_FLAG));
 
     ctrl_printf("CTRL: breathing, monitoring and hmi tasks stopped\n");