From 1095aec1623fe0559c2c1a472fa61befa7bd2f71 Mon Sep 17 00:00:00 2001
From: raiden00pl <raiden00@railab.me>
Date: Wed, 8 Nov 2023 12:37:41 +0100
Subject: [PATCH] examples/foc: start from the beginning if control loop is
 started

After the start request to FOC device, we should return to the
beginning of the loop and wait for the first synchronization event.

This also fixes the problem with the extended first loop cycle,
so we can remove the workaround in foc_perf.
---
 examples/foc/foc_device.c      |  9 ---------
 examples/foc/foc_fixed16_thr.c |  4 ++++
 examples/foc/foc_float_thr.c   |  4 ++++
 examples/foc/foc_perf.c        | 22 +++-------------------
 examples/foc/foc_perf.h        |  2 --
 5 files changed, 11 insertions(+), 30 deletions(-)

diff --git a/examples/foc/foc_device.c b/examples/foc/foc_device.c
index 01c49d62c..5c39315be 100644
--- a/examples/foc/foc_device.c
+++ b/examples/foc/foc_device.c
@@ -144,15 +144,6 @@ int foc_device_start(FAR struct foc_device_s *dev, bool state)
           PRINTFV("ERROR: foc_dev_start failed %d!\n", ret);
           goto errout;
         }
-
-#ifdef CONFIG_EXAMPLES_FOC_PERF
-      /* Skip this cycle in stats. When the dev is started, many components
-       * are initialized, which significantly increases the cycle time and
-       * disturbs the statistics.
-       */
-
-      foc_perf_skip(&dev->perf);
-#endif
     }
   else
     {
diff --git a/examples/foc/foc_fixed16_thr.c b/examples/foc/foc_fixed16_thr.c
index 63ccd3312..6e8abd0e9 100644
--- a/examples/foc/foc_fixed16_thr.c
+++ b/examples/foc/foc_fixed16_thr.c
@@ -386,6 +386,10 @@ int foc_fixed16_thr(FAR struct foc_ctrl_env_s *envp)
             }
 
           motor.startstop = false;
+
+          /* Start from the beginning of the control loop */
+
+          continue;
         }
 
       /* Ignore control logic if controller not started yet */
diff --git a/examples/foc/foc_float_thr.c b/examples/foc/foc_float_thr.c
index e169d892d..39aa5e481 100644
--- a/examples/foc/foc_float_thr.c
+++ b/examples/foc/foc_float_thr.c
@@ -399,6 +399,10 @@ int foc_float_thr(FAR struct foc_ctrl_env_s *envp)
             }
 
           motor.startstop = false;
+
+          /* Start from the beginning of the control loop */
+
+          continue;
         }
 
       /* Ignore control logic if controller not started yet */
diff --git a/examples/foc/foc_perf.c b/examples/foc/foc_perf.c
index 005fe970e..74e378f2c 100644
--- a/examples/foc/foc_perf.c
+++ b/examples/foc/foc_perf.c
@@ -56,15 +56,6 @@ void foc_perf_start(struct foc_perf_s *p)
   p->now = perf_gettime();
 }
 
-/****************************************************************************
- * Name: foc_perf_skip
- ****************************************************************************/
-
-void foc_perf_skip(struct foc_perf_s *p)
-{
-  p->skip = true;
-}
-
 /****************************************************************************
  * Name: foc_perf_end
  ****************************************************************************/
@@ -75,16 +66,9 @@ void foc_perf_end(struct foc_perf_s *p)
 
   p->max_changed = false;
 
-  if (p->skip == false)
+  if (p->now > p->max)
     {
-      if (p->now > p->max)
-        {
-          p->max = p->now;
-          p->max_changed = true;
-        }
+      p->max = p->now;
+      p->max_changed = true;
     }
-
-  /* Reset skip flag */
-
-  p->skip = false;
 }
diff --git a/examples/foc/foc_perf.h b/examples/foc/foc_perf.h
index 3cfb2a724..9303c2b9f 100644
--- a/examples/foc/foc_perf.h
+++ b/examples/foc/foc_perf.h
@@ -40,7 +40,6 @@
 struct foc_perf_s
 {
   bool     max_changed;
-  bool     skip;
   uint32_t max;
   uint32_t now;
 };
@@ -51,7 +50,6 @@ struct foc_perf_s
 
 int foc_perf_init(struct foc_perf_s *p);
 void foc_perf_start(struct foc_perf_s *p);
-void foc_perf_skip(struct foc_perf_s *p);
 void foc_perf_end(struct foc_perf_s *p);
 
 #endif /* __APPS_EXAMPLES_FOC_FOC_PERF_H */