[BUG] Control loop timing overflow

[vcrocher]

Description

Timing of main control loop is always slightly larger than set in application.h. Typically 0.15-0.5ms above for a 1ms period with very low computation task.

Expected Behavior

Exact (or very close to exact) timing of control loop in most iterations and when computation is compatible with said timing.

Possible Fix

Possible patch using an active waiting time (100% busy CPU for short amount of control loop) to get a more accurate timing:

diff --git a/src/core/application.cpp b/src/core/application.cpp
index 7d4e322..1c56a02 100644
--- a/src/core/application.cpp
+++ b/src/core/application.cpp
@@ -57,7 +57,6 @@ void app_programControlLoop(void) {
             std::chrono::steady_clock::now() - _t0).count()) / 1e6;
     if(dt>controlLoopPeriodInms/1000.)
         spdlog::warn("Applicaton thread time overflow: {}ms (>{}ms) !", dt*1000., controlLoopPeriodInms);
-
 }
 
 /******************** Runs at the End of rt_control_thread********************/
diff --git a/src/core/application.h b/src/core/application.h
index 7f78398..0c7dedb 100644
--- a/src/core/application.h
+++ b/src/core/application.h
@@ -68,8 +68,11 @@ extern "C" {
 #define INCREMENT_1MS(var) (var++)     /* Increment 1ms variable in taskTmr */
 #define NODEID (80)
 
-const float controlLoopPeriodInms = 2.;   //!< Define the control loop period (in ms): the period of rt_control_thread loop (and so the app update rate). In ms.
-const float CANUpdateLoopPeriodInms = 1.; //!< Define the CAN PDO processing period. SYNCH messages (and so actual PDO update) is set to twice this period (twice slower). In ms.
+const float controlLoopPeriodInms = 2.;     //!< Define the control loop period (in ms): the period of rt_control_thread loop (and so the app update rate). In [ms].
+const float CANUpdateLoopPeriodInms = 1.0;  //!< Define the CAN PDO processing period. SYNCH messages (and so actual PDO update) is set to twice this period (twice slower). In [ms].
+const float activeWaitTimeInms = 0.5;       //!< Define the active wait time (busy CPU) in the control thread to get more accurate timing. Typically between 10%-50% of controlLoopPeriod, 0 for no effect. In [ms].
+
+
 
 /**
  * /brief Function is called on program startup.
@@ -101,4 +104,4 @@ void app_programAsync(uint16_t timer1msDiff);
 void app_programControlLoop(void);
 
 
-#endif /*APP_H*/
\ No newline at end of file
+#endif /*APP_H*/
diff --git a/src/core/main.cpp b/src/core/main.cpp
index b793552..3aea79f 100644
--- a/src/core/main.cpp
+++ b/src/core/main.cpp
@@ -57,6 +57,9 @@ CO_NMT_reset_cmd_t reset_local = CO_RESET_NOT;
 struct period_info {
     struct timespec next_period;
     long period_ns;
+    struct timespec next_period_wo_active_wait;
+    long period_wo_active_wait_ns;
+    long active_wait_ns;
 };
 
 /** @brief Struct to hold arguments for ROS thread*/
@@ -380,25 +383,45 @@ static void *rt_control_thread(void *arg) {
 }
 /* Control thread time functions ********************************/
 static void inc_period(struct period_info *pinfo) {
+    //Overall end period time
     pinfo->next_period.tv_nsec += pinfo->period_ns;
-
-    while (pinfo->next_period.tv_nsec >= 1000000000) {
+    while (pinfo->next_period.tv_nsec >= NSEC_PER_SEC) {
         /* timespec nsec overflow */
         pinfo->next_period.tv_sec++;
-        pinfo->next_period.tv_nsec -= 1000000000;
+        pinfo->next_period.tv_nsec -= NSEC_PER_SEC;
+    }
+
+    //End period time wo active wait time
+    pinfo->next_period_wo_active_wait.tv_nsec += pinfo->period_wo_active_wait_ns;
+    while (pinfo->next_period_wo_active_wait.tv_nsec >= NSEC_PER_SEC) {
+        /* timespec nsec overflow */
+        pinfo->next_period_wo_active_wait.tv_sec++;
+        pinfo->next_period_wo_active_wait.tv_nsec -= NSEC_PER_SEC;
     }
 }
 static void periodic_task_init(struct period_info *pinfo) {
-    /* for simplicity, hardcoding a 1ms period */
-    pinfo->period_ns = controlLoopPeriodInms * 1000000;
+    pinfo->period_ns = controlLoopPeriodInms * NSEC_PER_MSEC;
+    pinfo->period_wo_active_wait_ns = (controlLoopPeriodInms-activeWaitTimeInms) * NSEC_PER_MSEC;
+    pinfo->active_wait_ns = activeWaitTimeInms * NSEC_PER_MSEC;
 
+    clock_gettime(CLOCK_MONOTONIC, &(pinfo->next_period_wo_active_wait));
     clock_gettime(CLOCK_MONOTONIC, &(pinfo->next_period));
 }
+//time1-time0 in s
+double diff_ts(struct timespec *time1, struct timespec *time0) {
+  return (time1->tv_sec - time0->tv_sec) + (time1->tv_nsec - time0->tv_nsec) / (double)NSEC_PER_SEC;
+}
 static void wait_rest_of_period(struct period_info *pinfo) {
     inc_period(pinfo);
 
     /* for simplicity, ignoring possibilities of signal wakes */
-    clock_nanosleep(CLOCK_MONOTONIC, TIMER_ABSTIME, &pinfo->next_period, NULL);
+    clock_nanosleep(CLOCK_MONOTONIC, TIMER_ABSTIME, &pinfo->next_period_wo_active_wait, NULL);
+    //Wait (busy wait) remaining time of period to account for jitter
+    timespec now;
+    clock_gettime(CLOCK_MONOTONIC, &now);
+    while(diff_ts(&pinfo->next_period, &now)>0.) {
+        clock_gettime(CLOCK_MONOTONIC, &now);
+    }
 }
 /* CAN messaging helper functions ********************************/

Steps to Reproduce

Compile and run any demo state machine and inspect log. dt is systematically above requested time. Occurs with or without high priority threads (sudo / non-sudo).