examples/foc: protect control loop with critical section

If the controller frequency is high, system timer interrupts will
eventually interrupt the controller function, thereby increasing the
execution time. This may lead to skipping the control cycle, which
negatively affects the control algorithm.

With this option enabled, interrupts are disabled for the duration
of the controller function execution.

Here example results from CONFIG_EXAMPLES_FOC_PERF output
for b-g431b-esc1 board with CONFIG_EXAMPLES_FOC_NOTIFIER_FREQ=10000:

1. CONFIG_EXAMPLES_FOC_CONTROL_CRITSEC=n

  exec ticks=5258
    nsec=30929
  per ticks=21268
    nsec=125105

2. CONFIG_EXAMPLES_FOC_CONTROL_CRITSEC=y

  exec ticks=3428
    nsec=20164
  per ticks=19203
    nsec=112958

The difference is ~12us!

examples/foc/Kconfig

@@ -47,6 +47,24 @@ config EXAMPLES_FOC_PERF
 	bool "Enable performance meassurements"
 	default n
 
+config EXAMPLES_FOC_CONTROL_CRITSEC
+	bool "Protect controller thread with critical section"
+	default y
+	depends on BUILD_FLAT
+	---help---
+		Protect controller thread with critical section.
+
+		If the controller frequency is high, system timer interrupts will
+		eventually interrupt the controller function, thereby increasing the
+		execution time. This may lead to skipping the control cycle, which
+		negatively affects the control algorithm.
+
+		With this option enabled, interrupts are disabled for the duration
+		of the controller function execution.
+
+		This option uses the kernel internal API directly, which means
+		it won't work outside of FLAT build.
+
 if EXAMPLES_FOC_PERF
 
 config EXAMPLES_FOC_PERF_LIVE

examples/foc/foc_fixed16_thr.c

@@ -50,6 +50,16 @@
 #  error
 #endif
 
+/* Critical section */
+
+#ifdef CONFIG_EXAMPLES_FOC_CONTROL_CRITSEC
+#  define foc_enter_critical() irqstate_t intflags = enter_critical_section()
+#  define foc_leave_critical() leave_critical_section(intflags)
+#else
+#  define foc_enter_critical()
+#  define foc_leave_critical()
+#endif
+
 /****************************************************************************
  * Private Type Definition
  ****************************************************************************/
@@ -340,6 +350,8 @@ int foc_fixed16_thr(FAR struct foc_ctrl_env_s *envp)
 
   while (motor.mq.quit == false)
     {
+      foc_enter_critical();
+
       if (motor.mq.start == true)
         {
           /* Get FOC device state */
@@ -390,13 +402,15 @@ int foc_fixed16_thr(FAR struct foc_ctrl_env_s *envp)
 
           /* Start from the beginning of the control loop */
 
+          foc_leave_critical();
           continue;
         }
 
       /* Ignore control logic if controller not started yet */
 
       if (motor.mq.start == false)
         {
+          foc_leave_critical();
           usleep(1000);
           continue;
         }
@@ -503,6 +517,8 @@ int foc_fixed16_thr(FAR struct foc_ctrl_env_s *envp)
       /* Increase counter */
 
       motor.time += 1;
+
+      foc_leave_critical();
     }
 
 errout:

examples/foc/foc_float_thr.c

@@ -50,6 +50,16 @@
 #  error
 #endif
 
+/* Critical section */
+
+#ifdef CONFIG_EXAMPLES_FOC_CONTROL_CRITSEC
+#  define foc_enter_critical() irqstate_t intflags = enter_critical_section()
+#  define foc_leave_critical() leave_critical_section(intflags)
+#else
+#  define foc_enter_critical()
+#  define foc_leave_critical()
+#endif
+
 /****************************************************************************
  * Private Type Definition
  ****************************************************************************/
@@ -353,6 +363,8 @@ int foc_float_thr(FAR struct foc_ctrl_env_s *envp)
 
   while (motor.mq.quit == false)
     {
+      foc_enter_critical();
+
       if (motor.mq.start == true)
         {
           /* Get FOC device state */
@@ -403,13 +415,15 @@ int foc_float_thr(FAR struct foc_ctrl_env_s *envp)
 
           /* Start from the beginning of the control loop */
 
+          foc_leave_critical();
           continue;
         }
 
       /* Ignore control logic if controller not started yet */
 
       if (motor.mq.start == false)
         {
+          foc_leave_critical();
           usleep(1000);
           continue;
         }
@@ -516,6 +530,8 @@ int foc_float_thr(FAR struct foc_ctrl_env_s *envp)
       /* Increase counter */
 
       motor.time += 1;
+
+      foc_leave_critical();
     }
 
 errout: