From dc4330476d09ef6bd1aaa357480d060e5cb3f1f0 Mon Sep 17 00:00:00 2001
From: raiden00pl <raiden00@railab.me>
Date: Thu, 9 Nov 2023 12:05:28 +0100
Subject: [PATCH] examples/foc: add velocity saturation

---
 examples/foc/Kconfig         |  6 ++++++
 examples/foc/foc_motor_b16.c | 11 +++++++++++
 examples/foc/foc_motor_b16.h |  1 +
 examples/foc/foc_motor_f32.c | 11 +++++++++++
 examples/foc/foc_motor_f32.h |  1 +
 5 files changed, 30 insertions(+)

diff --git a/examples/foc/Kconfig b/examples/foc/Kconfig
index d95112e86..cabd09d37 100644
--- a/examples/foc/Kconfig
+++ b/examples/foc/Kconfig
@@ -209,6 +209,12 @@ config EXAMPLES_FOC_VELNOW_FILTER
 	int "FOC example velocity controller (x1000)"
 	default 990
 
+config EXAMPLES_FOC_VEL_MAX
+	int "FOC example velocity maximum (x1)"
+	default 1000
+	---help---
+		The unit is rad/s
+
 choice
 	prompt "FOC velocity controller selection"
 	default EXAMPLES_FOC_VELCTRL_PI
diff --git a/examples/foc/foc_motor_b16.c b/examples/foc/foc_motor_b16.c
index dcfe80b0a..f5bcf8b42 100644
--- a/examples/foc/foc_motor_b16.c
+++ b/examples/foc/foc_motor_b16.c
@@ -26,6 +26,7 @@
 
 #include <assert.h>
 #include <string.h>
+#include <dspb16.h>
 
 #include "foc_cfg.h"
 #include "foc_debug.h"
@@ -945,6 +946,13 @@ static int foc_motor_run(FAR struct foc_motor_b16_s *motor)
 #ifdef CONFIG_EXAMPLES_FOC_HAVE_VEL
       case FOC_MMODE_VEL:
         {
+          /* Saturate velocity */
+
+          f_saturate_b16(&motor->vel.des, -motor->vel_sat,
+                         motor->vel_sat);
+
+          /* Velocity controller */
+
           if (motor->time % VEL_CONTROL_PRESCALER == 0)
             {
               /* Run velocity ramp controller */
@@ -1325,6 +1333,9 @@ int foc_motor_init(FAR struct foc_motor_b16_s *motor,
   motor->ol_thr     = ftob16(motor->envp->cfg->ol_thr / 1.0f);
   motor->ol_hys     = ftob16(motor->envp->cfg->ol_hys / 1.0f);
 #endif
+#ifdef CONFIG_EXAMPLES_FOC_HAVE_VEL
+  motor->vel_sat    = ftob16(CONFIG_EXAMPLES_FOC_VEL_MAX / 1.0f);
+#endif
 
 #ifdef CONFIG_EXAMPLES_FOC_HAVE_RUN
   /* Initialize controller run mode */
diff --git a/examples/foc/foc_motor_b16.h b/examples/foc/foc_motor_b16.h
index 3ae905ceb..407c042e5 100644
--- a/examples/foc/foc_motor_b16.h
+++ b/examples/foc/foc_motor_b16.h
@@ -136,6 +136,7 @@ struct foc_motor_b16_s
 #endif
 #ifdef CONFIG_EXAMPLES_FOC_HAVE_VEL
   struct foc_setpoint_b16_s     vel;          /* Velocity setpoint */
+  b16_t                         vel_sat;      /* Velocity saturation */
 #endif
 #ifdef CONFIG_EXAMPLES_FOC_HAVE_POS
   struct foc_setpoint_b16_s     pos;          /* Position setpoint */
diff --git a/examples/foc/foc_motor_f32.c b/examples/foc/foc_motor_f32.c
index 11b816ffb..12d8ae229 100644
--- a/examples/foc/foc_motor_f32.c
+++ b/examples/foc/foc_motor_f32.c
@@ -26,6 +26,7 @@
 
 #include <assert.h>
 #include <string.h>
+#include <dsp.h>
 
 #include "foc_cfg.h"
 #include "foc_debug.h"
@@ -928,6 +929,13 @@ static int foc_motor_run(FAR struct foc_motor_f32_s *motor)
 #ifdef CONFIG_EXAMPLES_FOC_HAVE_VEL
       case FOC_MMODE_VEL:
         {
+          /* Saturate velocity */
+
+          f_saturate(&motor->vel.des, -motor->vel_sat,
+                     motor->vel_sat);
+
+          /* Velocity controller */
+
           if (motor->time % VEL_CONTROL_PRESCALER == 0)
             {
               /* Run velocity ramp controller */
@@ -1314,6 +1322,9 @@ int foc_motor_init(FAR struct foc_motor_f32_s *motor,
   motor->ol_thr     = (motor->envp->cfg->ol_thr / 1.0f);
   motor->ol_hys     = (motor->envp->cfg->ol_hys / 1.0f);
 #endif
+#ifdef CONFIG_EXAMPLES_FOC_HAVE_VEL
+  motor->vel_sat    = (CONFIG_EXAMPLES_FOC_VEL_MAX / 1.0f);
+#endif
 
 #ifdef CONFIG_EXAMPLES_FOC_HAVE_RUN
   /* Initialize controller run mode */
diff --git a/examples/foc/foc_motor_f32.h b/examples/foc/foc_motor_f32.h
index 71bdd36ff..e22758698 100644
--- a/examples/foc/foc_motor_f32.h
+++ b/examples/foc/foc_motor_f32.h
@@ -136,6 +136,7 @@ struct foc_motor_f32_s
 #endif
 #ifdef CONFIG_EXAMPLES_FOC_HAVE_VEL
   struct foc_setpoint_f32_s     vel;          /* Velocity setpoint */
+  float                         vel_sat;      /* Velocity saturation */
 #endif
 #ifdef CONFIG_EXAMPLES_FOC_HAVE_POS
   struct foc_setpoint_f32_s     pos;          /* Position setpoint */