From 9dde6983e6c6b0f01ee85863a3f545f9cbdf9d2a Mon Sep 17 00:00:00 2001
From: raiden00pl <raiden00@railab.me>
Date: Thu, 12 Oct 2023 12:40:37 +0200
Subject: [PATCH] examples/foc: add support for feedforward compensation

---
 examples/foc/Kconfig         | 10 ++++++++++
 examples/foc/foc_motor_b16.c |  8 ++++++++
 examples/foc/foc_motor_f32.c |  8 ++++++++
 3 files changed, 26 insertions(+)

diff --git a/examples/foc/Kconfig b/examples/foc/Kconfig
index 35b88a589..0e4de7207 100644
--- a/examples/foc/Kconfig
+++ b/examples/foc/Kconfig
@@ -415,6 +415,16 @@ config EXAMPLES_FOC_OPENLOOP_Q
 	default 200
 	depends on EXAMPLES_FOC_HAVE_OPENLOOP
 
+config EXAMPLES_FOC_FEEDFORWARD
+	bool "FOC use feedforward compensation for current controller"
+	select INDUSTRY_FOC_FEEDFORWARD
+	default n
+	---help---
+		This option enables feed-forward compensation for PI current controller
+		which can help achieve better performace of FOC. This option is not 
+		recomended for sensorless operations and for current controllers that 
+		already has high update frequency.
+
 if EXAMPLES_FOC_CONTROL_PI
 
 config EXAMPLES_FOC_IDQ_KP
diff --git a/examples/foc/foc_motor_b16.c b/examples/foc/foc_motor_b16.c
index 36c66ff21..f039c2f5e 100644
--- a/examples/foc/foc_motor_b16.c
+++ b/examples/foc/foc_motor_b16.c
@@ -30,6 +30,9 @@
 #include "foc_cfg.h"
 #include "foc_debug.h"
 #include "foc_motor_b16.h"
+#ifdef CONFIG_EXAMPLES_FOC_FEEDFORWARD
+#  include "industry/foc/float/foc_feedforward.h"
+#endif
 
 /****************************************************************************
  * Pre-processor Definitions
@@ -862,8 +865,13 @@ static int foc_motor_run(FAR struct foc_motor_b16_s *motor)
 
   /* DQ compensation */
 
+#ifdef CONFIG_EXAMPLES_FOC_FEEDFORWARD
+  foc_feedforward_pmsm_b16(&motor->phy, &motor->foc_state.idq,
+                           motor->vel.now, &motor->vdq_comp);
+#else
   motor->vdq_comp.q = 0;
   motor->vdq_comp.d = 0;
+#endif
 
 errout:
   return ret;
diff --git a/examples/foc/foc_motor_f32.c b/examples/foc/foc_motor_f32.c
index 07cf16d9d..35ef15da5 100644
--- a/examples/foc/foc_motor_f32.c
+++ b/examples/foc/foc_motor_f32.c
@@ -30,6 +30,9 @@
 #include "foc_cfg.h"
 #include "foc_debug.h"
 #include "foc_motor_f32.h"
+#ifdef CONFIG_EXAMPLES_FOC_FEEDFORWARD
+#  include "industry/foc/float/foc_feedforward.h"
+#endif
 
 /****************************************************************************
  * Pre-processor Definitions
@@ -846,8 +849,13 @@ static int foc_motor_run(FAR struct foc_motor_f32_s *motor)
 
   /* DQ compensation */
 
+#ifdef CONFIG_EXAMPLES_FOC_FEEDFORWARD
+  foc_feedforward_pmsm_f32(&motor->phy, &motor->foc_state.idq,
+                           motor->vel.now, &motor->vdq_comp);
+#else
   motor->vdq_comp.q = 0.0f;
   motor->vdq_comp.d = 0.0f;
+#endif
 
 errout:
   return ret;