From 429f5a166f5802defdcbfd441eacafc721f3f9c3 Mon Sep 17 00:00:00 2001
From: raiden00pl <raiden00@railab.me>
Date: Thu, 18 Aug 2022 11:17:07 +0200
Subject: [PATCH] examples/foc: terminate the control thread if no work to do

---
 examples/foc/foc_fixed16_thr.c |  8 ++++++++
 examples/foc/foc_float_thr.c   |  8 ++++++++
 examples/foc/foc_motor_b16.c   |  7 +++++++
 examples/foc/foc_motor_f32.c   | 13 +++++++++++++
 examples/foc/foc_thr.h         |  3 ++-
 5 files changed, 38 insertions(+), 1 deletion(-)

diff --git a/examples/foc/foc_fixed16_thr.c b/examples/foc/foc_fixed16_thr.c
index 67f31ecdc..a32cfd562 100644
--- a/examples/foc/foc_fixed16_thr.c
+++ b/examples/foc/foc_fixed16_thr.c
@@ -352,6 +352,14 @@ int foc_fixed16_thr(FAR struct foc_ctrl_env_s *envp)
               PRINTF("ERROR: foc_dev_params_set failed %d!\n", ret);
               goto errout;
             }
+
+          /* Terminate control thread */
+
+          if (motor.ctrl_state == FOC_CTRL_STATE_TERMINATE)
+            {
+              PRINTF("TERMINATE CTRL THREAD\n");
+              goto errout;
+            }
         }
       else
         {
diff --git a/examples/foc/foc_float_thr.c b/examples/foc/foc_float_thr.c
index 615899276..30395e269 100644
--- a/examples/foc/foc_float_thr.c
+++ b/examples/foc/foc_float_thr.c
@@ -353,6 +353,14 @@ int foc_float_thr(FAR struct foc_ctrl_env_s *envp)
               PRINTF("ERROR: foc_dev_prams_set failed %d!\n", ret);
               goto errout;
             }
+
+          /* Terminate control thread */
+
+          if (motor.ctrl_state == FOC_CTRL_STATE_TERMINATE)
+            {
+              PRINTF("TERMINATE CTRL THREAD\n");
+              goto errout;
+            }
         }
       else
         {
diff --git a/examples/foc/foc_motor_b16.c b/examples/foc/foc_motor_b16.c
index d803e9a61..44c831ffb 100644
--- a/examples/foc/foc_motor_b16.c
+++ b/examples/foc/foc_motor_b16.c
@@ -1120,6 +1120,13 @@ int foc_motor_control(FAR struct foc_motor_b16_s *motor)
           break;
         }
 
+      case FOC_CTRL_STATE_TERMINATE:
+        {
+          /* Do nothing */
+
+          break;
+        }
+
       default:
         {
           PRINTF("ERROR: invalid ctrl_state=%d\n", motor->ctrl_state);
diff --git a/examples/foc/foc_motor_f32.c b/examples/foc/foc_motor_f32.c
index a7adf5363..aba1f76d5 100644
--- a/examples/foc/foc_motor_f32.c
+++ b/examples/foc/foc_motor_f32.c
@@ -1112,6 +1112,19 @@ int foc_motor_control(FAR struct foc_motor_f32_s *motor)
         {
           motor->foc_mode = FOC_HANDLER_MODE_IDLE;
 
+#ifndef CONFIG_EXAMPLES_FOC_HAVE_RUN
+          /* Terminate */
+
+          motor->ctrl_state += 1;
+#endif
+
+          break;
+        }
+
+      case FOC_CTRL_STATE_TERMINATE:
+        {
+          /* Do nothing */
+
           break;
         }
 
diff --git a/examples/foc/foc_thr.h b/examples/foc/foc_thr.h
index 2a8237e6c..95b41741e 100644
--- a/examples/foc/foc_thr.h
+++ b/examples/foc/foc_thr.h
@@ -88,7 +88,8 @@ enum foc_controller_state_e
   FOC_CTRL_STATE_RUN_INIT,
   FOC_CTRL_STATE_RUN,
 #endif
-  FOC_CTRL_STATE_IDLE
+  FOC_CTRL_STATE_IDLE,
+  FOC_CTRL_STATE_TERMINATE
 };
 
 /* FOC thread data */