From bd82e8d39f25747d0f80ba2b842d35bf1cd73b53 Mon Sep 17 00:00:00 2001
From: zouboan <fft@feedforward.com.cn>
Date: Sat, 20 Aug 2022 16:07:00 +0800
Subject: [PATCH] improve flux linkage identification method

improve flux linkage identification method
---
 include/industry/foc/float/foc_ident.h | 10 ++++++++-
 industry/foc/float/foc_ident.c         | 28 +++++++++++++++++++-------
 2 files changed, 30 insertions(+), 8 deletions(-)

diff --git a/include/industry/foc/float/foc_ident.h b/include/industry/foc/float/foc_ident.h
index c8e3fbaa9..a2d392ba9 100644
--- a/include/industry/foc/float/foc_ident.h
+++ b/include/industry/foc/float/foc_ident.h
@@ -48,6 +48,10 @@ struct foc_routine_ident_cb_f32_s
 
   FAR void *priv_speed;
 
+  /* Private data for kpki callbacks */
+
+  FAR void *priv_kpki;
+
   /* Openloop angle zero callback */
 
   CODE int (*zero)(FAR void *priv);
@@ -59,6 +63,10 @@ struct foc_routine_ident_cb_f32_s
   /* Identification openloop speed callback */
 
   CODE float (*speed)(FAR void *priv, float des, float now);
+
+  /* Identification calculate kp and ki callback */
+
+  CODE int (*kpki)(FAR void *priv, float res, float ind, float per);
 };
 #endif
 
@@ -70,7 +78,7 @@ struct foc_routine_ident_cfg_f32_s
   struct foc_routine_ident_cb_f32_s cb; /* Identification routine callbacks */
 
   float flux_vel;               /* Flux linkage measurement velocity */
-  float flux_volt;              /* Flux linkage measurement voltage */
+  float flux_curr;              /* Flux linkage measurement current */
   int   flux_steps;             /* Flux linkage measurement steps */
 #endif
   float per;                    /* Routine period in sec */
diff --git a/industry/foc/float/foc_ident.c b/industry/foc/float/foc_ident.c
index 528abecab..1e0788179 100644
--- a/industry/foc/float/foc_ident.c
+++ b/industry/foc/float/foc_ident.c
@@ -407,13 +407,13 @@ int foc_ident_fluxlink_run_f32(FAR struct foc_ident_f32_s *ident,
                                        ident->cfg.flux_vel, ident->vel);
     }
 
-  /* Force q axis voltage = ident->cfg.flux_volt */
+  /* Force q axis current = ident->cfg.flux_curr */
 
-  out->dq_ref.q   = ident->cfg.flux_volt;
+  out->dq_ref.q   = ident->cfg.flux_curr;
   out->dq_ref.d   = 0.0f;
   out->vdq_comp.q = 0.0f;
   out->vdq_comp.d = 0.0f;
-  out->foc_mode   = FOC_HANDLER_MODE_VOLTAGE;
+  out->foc_mode   = FOC_HANDLER_MODE_CURRENT;
 
   /* Increase counter */
 
@@ -437,7 +437,7 @@ int foc_ident_fluxlink_run_f32(FAR struct foc_ident_f32_s *ident,
       /* Get flux linkage */
 
       ident->final.flux = (volt_avg - ident->final.res * curr_avg) /
-                          ident->vel;
+                          ident->vel - ident->final.ind * curr_avg;
 
       /* Force IDLE state */
 
@@ -578,7 +578,7 @@ int foc_routine_ident_cfg_f32(FAR foc_routine_f32_t *r, FAR void *cfg)
 
   if (i->cfg.res_current <= 0.0f || i->cfg.ind_volt <= 0.0f
 #ifdef CONFIG_INDUSTRY_FOC_IDENT_FLUX
-     || i->cfg.flux_volt <= 0.0f
+     || i->cfg.flux_curr <= 0.0f
 #endif
      )
     {
@@ -628,6 +628,9 @@ int foc_routine_ident_run_f32(FAR foc_routine_f32_t *r,
 
   DEBUGASSERT(r->data);
   i = r->data;
+#ifdef CONFIG_INDUSTRY_FOC_IDENT_FLUX
+  DEBUGASSERT(i->cfg.cb.kpki != NULL);
+#endif
 
   /* Force IDLE state at default */
 
@@ -650,12 +653,23 @@ int foc_routine_ident_run_f32(FAR foc_routine_f32_t *r,
           break;
         }
 
-      case FOC_IDENT_RUN_IDLE1:
-      case FOC_IDENT_RUN_IDLE2:
       case FOC_IDENT_RUN_IDLE3:
 #ifdef CONFIG_INDUSTRY_FOC_IDENT_FLUX
+        if (i->cntr == 0)
+          {
+            ret = i->cfg.cb.kpki(i->cfg.cb.priv_kpki, i->final.res,
+                                 i->final.ind, i->cfg.per);
+            if (ret < 0)
+              {
+                FOCLIBERR("ERROR: ident kpki callback failed %d!\n", ret);
+                goto errout;
+              }
+          }
+
       case FOC_IDENT_RUN_IDLE4:
 #endif
+      case FOC_IDENT_RUN_IDLE1:
+      case FOC_IDENT_RUN_IDLE2:
         {
           /* De-energetize motor */