You are viewing a plain text version of this content. The canonical link for it is here.
Posted to commits@nuttx.apache.org by xi...@apache.org on 2021/10/30 06:18:19 UTC
[incubator-nuttx-apps] 03/03: industry/foc: add motor
identification routine
This is an automated email from the ASF dual-hosted git repository.
xiaoxiang pushed a commit to branch master
in repository https://gitbox.apache.org/repos/asf/incubator-nuttx-apps.git
commit f009d68a914da4d9981ca21527e10db33abf0339
Author: raiden00pl <ra...@railab.me>
AuthorDate: Fri Oct 29 13:35:48 2021 +0200
industry/foc: add motor identification routine
---
include/industry/foc/fixed16/foc_ident.h | 65 ++++
include/industry/foc/float/foc_ident.h | 65 ++++
industry/foc/Kconfig | 6 +
industry/foc/Makefile | 6 +
industry/foc/fixed16/foc_ident.c | 608 +++++++++++++++++++++++++++++++
industry/foc/float/foc_ident.c | 601 ++++++++++++++++++++++++++++++
6 files changed, 1351 insertions(+)
diff --git a/include/industry/foc/fixed16/foc_ident.h b/include/industry/foc/fixed16/foc_ident.h
new file mode 100644
index 0000000..276fa63
--- /dev/null
+++ b/include/industry/foc/fixed16/foc_ident.h
@@ -0,0 +1,65 @@
+/****************************************************************************
+ * apps/include/industry/foc/fixed16/foc_ident.h
+ *
+ * Licensed to the Apache Software Foundation (ASF) under one or more
+ * contributor license agreements. See the NOTICE file distributed with
+ * this work for additional information regarding copyright ownership. The
+ * ASF licenses this file to you under the Apache License, Version 2.0 (the
+ * "License"); you may not use this file except in compliance with the
+ * License. You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
+ * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
+ * License for the specific language governing permissions and limitations
+ * under the License.
+ *
+ ****************************************************************************/
+
+#ifndef __INDUSTRY_FOC_FIXED16_FOC_IDENT_H
+#define __INDUSTRY_FOC_FIXED16_FOC_IDENT_H
+
+/****************************************************************************
+ * Included Files
+ ****************************************************************************/
+
+#include <nuttx/config.h>
+
+#include <dspb16.h>
+
+#include "industry/foc/fixed16/foc_routine.h"
+
+/****************************************************************************
+ * Public Type Definition
+ ****************************************************************************/
+
+/* Identification routine configuration */
+
+struct foc_routine_ident_cfg_b16_s
+{
+ b16_t per; /* Routine period in sec */
+ b16_t res_current; /* Resistance measurement current */
+ b16_t ind_volt; /* Inductance measurement current */
+ int res_steps; /* Resistance measurement steps */
+ int ind_steps; /* Inductance measurement steps */
+ int idle_steps; /* IDLE steps */
+};
+
+/* Identification routine final data */
+
+struct foc_routine_ident_final_b16_s
+{
+ bool ready; /* Result ready */
+ b16_t res; /* Phase resistance */
+ b16_t ind; /* Phase inductance */
+};
+
+/****************************************************************************
+ * Public Data
+ ****************************************************************************/
+
+extern struct foc_routine_ops_b16_s g_foc_routine_ident_b16;
+
+#endif /* __INDUSTRY_FOC_FIXED16_FOC_IDENT_H */
diff --git a/include/industry/foc/float/foc_ident.h b/include/industry/foc/float/foc_ident.h
new file mode 100644
index 0000000..d7172f8
--- /dev/null
+++ b/include/industry/foc/float/foc_ident.h
@@ -0,0 +1,65 @@
+/****************************************************************************
+ * apps/include/industry/foc/float/foc_ident.h
+ *
+ * Licensed to the Apache Software Foundation (ASF) under one or more
+ * contributor license agreements. See the NOTICE file distributed with
+ * this work for additional information regarding copyright ownership. The
+ * ASF licenses this file to you under the Apache License, Version 2.0 (the
+ * "License"); you may not use this file except in compliance with the
+ * License. You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
+ * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
+ * License for the specific language governing permissions and limitations
+ * under the License.
+ *
+ ****************************************************************************/
+
+#ifndef __INDUSTRY_FOC_FLOAT_FOC_IDENT_H
+#define __INDUSTRY_FOC_FLOAT_FOC_IDENT_H
+
+/****************************************************************************
+ * Included Files
+ ****************************************************************************/
+
+#include <nuttx/config.h>
+
+#include <dsp.h>
+
+#include "industry/foc/float/foc_routine.h"
+
+/****************************************************************************
+ * Public Type Definition
+ ****************************************************************************/
+
+/* Identification routine configuration */
+
+struct foc_routine_ident_cfg_f32_s
+{
+ float per; /* Routine period in sec */
+ float res_current; /* Resistance measurement current */
+ float ind_volt; /* Inductance measurement current */
+ int res_steps; /* Resistance measurement steps */
+ int ind_steps; /* Inductance measurement steps */
+ int idle_steps; /* IDLE steps */
+};
+
+/* Identification routine final data */
+
+struct foc_routine_ident_final_f32_s
+{
+ bool ready; /* Result ready */
+ float res; /* Phase resistance */
+ float ind; /* Phase inductance */
+};
+
+/****************************************************************************
+ * Public Data
+ ****************************************************************************/
+
+extern struct foc_routine_ops_f32_s g_foc_routine_ident_f32;
+
+#endif /* __INDUSTRY_FOC_FLOAT_FOC_IDENT_H */
diff --git a/industry/foc/Kconfig b/industry/foc/Kconfig
index 5124cea..aff1209 100644
--- a/industry/foc/Kconfig
+++ b/industry/foc/Kconfig
@@ -87,4 +87,10 @@ config INDUSTRY_FOC_ALIGN
---help---
Enable support for angle sensor alignment routine (zero offset + direction)
+config INDUSTRY_FOC_IDENT
+ bool "FOC motor identification routine"
+ default n
+ ---help---
+ Enable support for motor identification routine (phase resistance and phase inductance)
+
endif
diff --git a/industry/foc/Makefile b/industry/foc/Makefile
index 83489f8..35583ac 100644
--- a/industry/foc/Makefile
+++ b/industry/foc/Makefile
@@ -54,6 +54,9 @@ endif
ifeq ($(CONFIG_INDUSTRY_FOC_ALIGN),y)
CSRCS += float/foc_align.c
endif
+ifeq ($(CONFIG_INDUSTRY_FOC_IDENT),y)
+CSRCS += float/foc_ident.c
+endif
endif
@@ -87,6 +90,9 @@ endif
ifeq ($(CONFIG_INDUSTRY_FOC_ALIGN),y)
CSRCS += fixed16/foc_align.c
endif
+ifeq ($(CONFIG_INDUSTRY_FOC_IDENT),y)
+CSRCS += fixed16/foc_ident.c
+endif
endif
diff --git a/industry/foc/fixed16/foc_ident.c b/industry/foc/fixed16/foc_ident.c
new file mode 100644
index 0000000..65dab35
--- /dev/null
+++ b/industry/foc/fixed16/foc_ident.c
@@ -0,0 +1,608 @@
+/****************************************************************************
+ * apps/industry/foc/fixed16/foc_ident.c
+ * This file implements motor ident routine for fixed16
+ *
+ * Licensed to the Apache Software Foundation (ASF) under one or more
+ * contributor license agreements. See the NOTICE file distributed with
+ * this work for additional information regarding copyright ownership. The
+ * ASF licenses this file to you under the Apache License, Version 2.0 (the
+ * "License"); you may not use this file except in compliance with the
+ * License. You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
+ * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
+ * License for the specific language governing permissions and limitations
+ * under the License.
+ *
+ ****************************************************************************/
+
+/****************************************************************************
+ * Included Files
+ ****************************************************************************/
+
+#include <nuttx/config.h>
+
+#include <assert.h>
+#include <errno.h>
+#include <stdlib.h>
+
+#include "industry/foc/foc_common.h"
+#include "industry/foc/foc_log.h"
+
+#include "industry/foc/fixed16/foc_ident.h"
+
+/****************************************************************************
+ * Pre-processor Definitions
+ ****************************************************************************/
+
+#define IDENT_PI_KP (ftob16(0.0f))
+#define IDENT_PI_KI (ftob16(0.05f))
+
+/****************************************************************************
+ * Private Data Types
+ ****************************************************************************/
+
+/* Identification stages */
+
+enum foc_ident_run_stage_e
+{
+ FOC_IDENT_RUN_INIT,
+ FOC_IDENT_RUN_IDLE1,
+ FOC_IDENT_RUN_RES,
+ FOC_IDENT_RUN_IDLE2,
+ FOC_IDENT_RUN_IND,
+ FOC_IDENT_RUN_IDLE3,
+ FOC_IDENT_RUN_DONE
+};
+
+/* Ident routine private data */
+
+struct foc_ident_b16_s
+{
+ struct foc_routine_ident_cfg_b16_s cfg; /* Ident configuration */
+ struct foc_routine_ident_final_b16_s final; /* Ident final result */
+ pid_controller_b16_t pi; /* PI controller for res */
+ int cntr; /* Helper counter */
+ int stage; /* Ident stage */
+};
+
+/****************************************************************************
+ * Private Function Prototypes
+ ****************************************************************************/
+
+int foc_routine_ident_init_b16(FAR foc_routine_b16_t *r);
+void foc_routine_ident_deinit_b16(FAR foc_routine_b16_t *r);
+int foc_routine_ident_cfg_b16(FAR foc_routine_b16_t *r, FAR void *cfg);
+int foc_routine_ident_run_b16(FAR foc_routine_b16_t *r,
+ FAR struct foc_routine_in_b16_s *in,
+ FAR struct foc_routine_out_b16_s *out);
+int foc_routine_ident_final_b16(FAR foc_routine_b16_t *r, FAR void *data);
+
+/****************************************************************************
+ * Public Data
+ ****************************************************************************/
+
+struct foc_routine_ops_b16_s g_foc_routine_ident_b16 =
+{
+ .init = foc_routine_ident_init_b16,
+ .deinit = foc_routine_ident_deinit_b16,
+ .cfg = foc_routine_ident_cfg_b16,
+ .run = foc_routine_ident_run_b16,
+ .final = foc_routine_ident_final_b16,
+};
+
+/****************************************************************************
+ * Private Functions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Name: foc_ident_idle_run_b16
+ *
+ * Description:
+ * Force IDLE state
+ *
+ * Input Parameter:
+ * ident - pointer to FOC ident routine
+ * in - pointer to FOC routine input data
+ * out - pointer to FOC routine output data
+ *
+ ****************************************************************************/
+
+int foc_ident_idle_run_b16(FAR struct foc_ident_b16_s *ident,
+ FAR struct foc_routine_in_b16_s *in,
+ FAR struct foc_routine_out_b16_s *out)
+{
+ int ret = FOC_ROUTINE_RUN_NOTDONE;
+
+ /* Get output */
+
+ out->dq_ref.q = 0;
+ out->dq_ref.d = 0;
+ out->vdq_comp.q = 0;
+ out->vdq_comp.d = 0;
+ out->angle = 0;
+ out->foc_mode = FOC_HANDLER_MODE_IDLE;
+
+ /* Increase counter */
+
+ ident->cntr += 1;
+
+ if (ident->cntr > ident->cfg.idle_steps)
+ {
+ /* Done */
+
+ ret = FOC_ROUTINE_RUN_DONE;
+
+ /* Reset counter */
+
+ ident->cntr = 0;
+ }
+
+ return ret;
+}
+
+/****************************************************************************
+ * Name: foc_ident_res_run_b16
+ *
+ * Description:
+ * Run resistance identification routine
+ *
+ * Input Parameter:
+ * ident - pointer to FOC ident routine
+ * in - pointer to FOC routine input data
+ * out - pointer to FOC routine output data
+ *
+ ****************************************************************************/
+
+int foc_ident_res_run_b16(FAR struct foc_ident_b16_s *ident,
+ FAR struct foc_routine_in_b16_s *in,
+ FAR struct foc_routine_out_b16_s *out)
+{
+ int ret = FOC_ROUTINE_RUN_NOTDONE;
+ b16_t err = 0;
+ b16_t vref = 0;
+
+ /* Initialize PI controller */
+
+ if (ident->cntr == 0)
+ {
+ pi_controller_init_b16(&ident->pi, IDENT_PI_KP, IDENT_PI_KI);
+ }
+
+ /* PI saturation */
+
+ pi_saturation_set_b16(&ident->pi, 0, in->vbus);
+
+ /* Lock motor with given current */
+
+ err = ident->cfg.res_current - in->foc_state->idq.d;
+ vref = pi_controller_b16(&ident->pi, err);
+
+ /* Force alpha voltage = vref */
+
+ out->dq_ref.q = 0;
+ out->dq_ref.d = vref;
+ out->vdq_comp.q = 0;
+ out->vdq_comp.d = 0;
+ out->angle = 0;
+ out->foc_mode = FOC_HANDLER_MODE_VOLTAGE;
+
+ /* Increase counter */
+
+ ident->cntr += 1;
+
+ if (ident->cntr > ident->cfg.res_steps)
+ {
+ /* Get resistance */
+
+ ident->final.res = b16divb16(vref, ident->cfg.res_current);
+
+ /* Force IDLE state */
+
+ out->dq_ref.q = 0;
+ out->dq_ref.d = 0;
+ out->vdq_comp.q = 0;
+ out->vdq_comp.d = 0;
+ out->angle = 0;
+ out->foc_mode = FOC_HANDLER_MODE_IDLE;
+
+ /* Resistance identification done */
+
+ ret = FOC_ROUTINE_RUN_DONE;
+
+ /* Reset counter */
+
+ ident->cntr = 0;
+ }
+
+ return ret;
+}
+
+/****************************************************************************
+ * Name: foc_ident_ind_run_b16
+ *
+ * Description:
+ * Run inductance identification routine
+ *
+ * Input Parameter:
+ * ident - pointer to FOC ident routine
+ * in - pointer to FOC routine input data
+ * out - pointer to FOC routine output data
+ *
+ ****************************************************************************/
+
+int foc_ident_ind_run_b16(FAR struct foc_ident_b16_s *ident,
+ FAR struct foc_routine_in_b16_s *in,
+ FAR struct foc_routine_out_b16_s *out)
+{
+ int ret = FOC_ROUTINE_RUN_NOTDONE;
+ b16_t vref = 0;
+ b16_t curr1_avg = 0;
+ b16_t curr2_avg = 0;
+ b16_t delta_curr = 0;
+ static b16_t sign = b16ONE;
+ static b16_t curr1_sum = 0;
+ static b16_t curr2_sum = 0;
+ b16_t tmp1 = 0;
+ b16_t tmp2 = 0;
+ b16_t tmp3 = 0;
+
+ /* If previous sign was -1 then we have top current,
+ * if previous sing was +1 then we have bottom current.
+ */
+
+ if (sign > 0)
+ {
+ /* Average bottm current */
+
+ curr1_sum += in->foc_state->idq.d;
+ }
+ else
+ {
+ /* Average top current */
+
+ curr2_sum += in->foc_state->idq.d;
+ }
+
+ /* Invert voltage to generate square wave D voltage */
+
+ sign = -sign;
+ vref = b16mulb16(sign, ident->cfg.ind_volt);
+
+ /* Force alpha voltage = vref */
+
+ out->dq_ref.q = 0;
+ out->dq_ref.d = vref;
+ out->vdq_comp.q = 0;
+ out->vdq_comp.d = 0;
+ out->angle = 0;
+ out->foc_mode = FOC_HANDLER_MODE_VOLTAGE;
+
+ /* Increase counter */
+
+ ident->cntr += 1;
+
+ if (ident->cntr > ident->cfg.ind_steps)
+ {
+ /* Half samples from curr1, other half from curr2 */
+
+ tmp1 = b16muli(curr1_sum, 2);
+ tmp2 = b16muli(curr2_sum, 2);
+
+ curr1_avg = b16divb16(tmp1, ident->cntr);
+ curr2_avg = b16divb16(tmp2, ident->cntr);
+
+ /* Average delta current */
+
+ delta_curr = curr1_avg - curr2_avg;
+
+ /* Get inductance
+ * L = V * t / dI
+ *
+ * where:
+ * t = per
+ * V = ind_volt
+ * dI = avg(curr_bottom) - avg(curr_top)
+ */
+
+ tmp3 = b16mulb16(ident->cfg.ind_volt, ident->cfg.per);
+ ident->final.ind = b16divb16(tmp3, delta_curr);
+
+ /* Force IDLE state */
+
+ out->dq_ref.q = 0;
+ out->dq_ref.d = 0;
+ out->vdq_comp.q = 0;
+ out->vdq_comp.d = 0;
+ out->angle = 0;
+ out->foc_mode = FOC_HANDLER_MODE_IDLE;
+
+ /* Inductance identification done */
+
+ ret = FOC_ROUTINE_RUN_DONE;
+
+ /* Reset counter */
+
+ ident->cntr = 0;
+
+ /* Reset static data */
+
+ sign = b16ONE;
+ curr1_sum = 0;
+ curr2_sum = 0;
+ }
+
+ return ret;
+}
+
+/****************************************************************************
+ * Name: foc_routine_ident_init_b16
+ *
+ * Description:
+ * Initialize the FOC ident routine (float32)
+ *
+ * Input Parameter:
+ * r - pointer to FOC routine
+ *
+ ****************************************************************************/
+
+int foc_routine_ident_init_b16(FAR foc_routine_b16_t *r)
+{
+ int ret = OK;
+
+ DEBUGASSERT(r);
+
+ /* Connect angle data */
+
+ r->data = zalloc(sizeof(struct foc_ident_b16_s));
+ if (r->data == NULL)
+ {
+ ret = -ENOMEM;
+ goto errout;
+ }
+
+errout:
+ return ret;
+}
+
+/****************************************************************************
+ * Name: foc_routine_ident_deinit_b16
+ *
+ * Description:
+ * Deinitialize the FOC ident routine (float32)
+ *
+ * Input Parameter:
+ * r - pointer to FOC routine
+ *
+ ****************************************************************************/
+
+void foc_routine_ident_deinit_b16(FAR foc_routine_b16_t *r)
+{
+ DEBUGASSERT(r);
+
+ if (r->data)
+ {
+ /* Free routine data */
+
+ free(r->data);
+ }
+}
+
+/****************************************************************************
+ * Name: foc_routine_ident_cfg_b16
+ *
+ * Description:
+ * Configure the FOC ident routine (float32)
+ *
+ * Input Parameter:
+ * r - pointer to FOC routine
+ * cfg - pointer to ident routine configuration data
+ *
+ ****************************************************************************/
+
+int foc_routine_ident_cfg_b16(FAR foc_routine_b16_t *r, FAR void *cfg)
+{
+ FAR struct foc_ident_b16_s *i = NULL;
+ int ret = OK;
+
+ DEBUGASSERT(r);
+
+ /* Get ident data */
+
+ DEBUGASSERT(r->data);
+ i = r->data;
+
+ /* Copy configuration */
+
+ memcpy(&i->cfg, cfg, sizeof(struct foc_routine_ident_cfg_b16_s));
+
+ /* Verify configuration */
+
+ if (i->cfg.per <= 0)
+ {
+ ret = -EINVAL;
+ goto errout;
+ }
+
+ if (i->cfg.res_current <= 0 || i->cfg.ind_volt <= 0)
+ {
+ ret = -EINVAL;
+ goto errout;
+ }
+
+ if (i->cfg.res_steps <= 0 || i->cfg.ind_steps <= 0 ||
+ i->cfg.idle_steps <= 0)
+ {
+ ret = -EINVAL;
+ goto errout;
+ }
+
+errout:
+ return ret;
+}
+
+/****************************************************************************
+ * Name: foc_routine_ident_run_b16
+ *
+ * Description:
+ * Run the FOC ident routine step (float32)
+ *
+ * Input Parameter:
+ * r - pointer to FOC routine
+ * in - pointer to FOC routine input data
+ * out - pointer to FOC routine output data
+ *
+ ****************************************************************************/
+
+int foc_routine_ident_run_b16(FAR foc_routine_b16_t *r,
+ FAR struct foc_routine_in_b16_s *in,
+ FAR struct foc_routine_out_b16_s *out)
+{
+ FAR struct foc_ident_b16_s *i = NULL;
+ int ret = FOC_ROUTINE_RUN_NOTDONE;
+
+ DEBUGASSERT(r);
+ DEBUGASSERT(in);
+ DEBUGASSERT(out);
+
+ /* Get ident data */
+
+ DEBUGASSERT(r->data);
+ i = r->data;
+
+ /* Force IDLE state at default */
+
+ out->dq_ref.q = 0;
+ out->dq_ref.d = 0;
+ out->vdq_comp.q = 0;
+ out->vdq_comp.d = 0;
+ out->angle = 0;
+ out->foc_mode = FOC_HANDLER_MODE_IDLE;
+
+ /* Handle ident stage */
+
+ switch (i->stage)
+ {
+ case FOC_IDENT_RUN_INIT:
+ {
+ i->stage += 1;
+ ret = FOC_ROUTINE_RUN_NOTDONE;
+
+ break;
+ }
+
+ case FOC_IDENT_RUN_IDLE1:
+ case FOC_IDENT_RUN_IDLE2:
+ case FOC_IDENT_RUN_IDLE3:
+ {
+ /* De-energetize motor */
+
+ ret = foc_ident_idle_run_b16(i, in, out);
+ if (ret < 0)
+ {
+ goto errout;
+ }
+
+ if (ret == FOC_ROUTINE_RUN_DONE)
+ {
+ i->stage += 1;
+ ret = FOC_ROUTINE_RUN_NOTDONE;
+ }
+
+ break;
+ }
+
+ case FOC_IDENT_RUN_RES:
+ {
+ /* Resistance */
+
+ ret = foc_ident_res_run_b16(i, in, out);
+ if (ret < 0)
+ {
+ goto errout;
+ }
+
+ if (ret == FOC_ROUTINE_RUN_DONE)
+ {
+ FOCLIBLOG("IDENT RES done!\n");
+
+ i->stage += 1;
+ ret = FOC_ROUTINE_RUN_NOTDONE;
+ }
+
+ break;
+ }
+
+ case FOC_IDENT_RUN_IND:
+ {
+ /* Inductance */
+
+ ret = foc_ident_ind_run_b16(i, in, out);
+ if (ret < 0)
+ {
+ goto errout;
+ }
+
+ if (ret == FOC_ROUTINE_RUN_DONE)
+ {
+ FOCLIBLOG("IDENT IND done!\n");
+
+ i->stage += 1;
+ ret = FOC_ROUTINE_RUN_NOTDONE;
+ }
+
+ break;
+ }
+
+ case FOC_IDENT_RUN_DONE:
+ {
+ ret = FOC_ROUTINE_RUN_DONE;
+
+ break;
+ }
+
+ default:
+ {
+ FOCLIBERR("ERROR: invalid ident stage %d\n", i->stage);
+ ret = -EINVAL;
+ goto errout;
+ }
+ }
+
+errout:
+ return ret;
+}
+
+/****************************************************************************
+ * Name: foc_routine_ident_final_b16
+ *
+ * Description:
+ * Finalize the FOC routine data (float32)
+ *
+ * Input Parameter:
+ * r - pointer to FOC routine
+ * data - pointer to FOC ident routine final data
+ *
+ ****************************************************************************/
+
+int foc_routine_ident_final_b16(FAR foc_routine_b16_t *r, FAR void *data)
+{
+ FAR struct foc_ident_b16_s *i = NULL;
+
+ DEBUGASSERT(r);
+ DEBUGASSERT(data);
+
+ /* Get ident data */
+
+ DEBUGASSERT(r->data);
+ i = r->data;
+
+ /* Get final data */
+
+ memcpy(data, &i->final, sizeof(struct foc_routine_ident_final_b16_s));
+
+ return OK;
+}
diff --git a/industry/foc/float/foc_ident.c b/industry/foc/float/foc_ident.c
new file mode 100644
index 0000000..f0a6117
--- /dev/null
+++ b/industry/foc/float/foc_ident.c
@@ -0,0 +1,601 @@
+/****************************************************************************
+ * apps/industry/foc/float/foc_ident.c
+ * This file implements motor ident routine for float32
+ *
+ * Licensed to the Apache Software Foundation (ASF) under one or more
+ * contributor license agreements. See the NOTICE file distributed with
+ * this work for additional information regarding copyright ownership. The
+ * ASF licenses this file to you under the Apache License, Version 2.0 (the
+ * "License"); you may not use this file except in compliance with the
+ * License. You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
+ * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
+ * License for the specific language governing permissions and limitations
+ * under the License.
+ *
+ ****************************************************************************/
+
+/****************************************************************************
+ * Included Files
+ ****************************************************************************/
+
+#include <nuttx/config.h>
+
+#include <assert.h>
+#include <errno.h>
+#include <stdlib.h>
+
+#include "industry/foc/foc_common.h"
+#include "industry/foc/foc_log.h"
+
+#include "industry/foc/float/foc_ident.h"
+
+/****************************************************************************
+ * Pre-processor Definitions
+ ****************************************************************************/
+
+#define IDENT_PI_KP (0.0f)
+#define IDENT_PI_KI (0.05f)
+
+/****************************************************************************
+ * Private Data Types
+ ****************************************************************************/
+
+/* Identification stages */
+
+enum foc_ident_run_stage_e
+{
+ FOC_IDENT_RUN_INIT,
+ FOC_IDENT_RUN_IDLE1,
+ FOC_IDENT_RUN_RES,
+ FOC_IDENT_RUN_IDLE2,
+ FOC_IDENT_RUN_IND,
+ FOC_IDENT_RUN_IDLE3,
+ FOC_IDENT_RUN_DONE
+};
+
+/* Ident routine private data */
+
+struct foc_ident_f32_s
+{
+ struct foc_routine_ident_cfg_f32_s cfg; /* Ident configuration */
+ struct foc_routine_ident_final_f32_s final; /* Ident final result */
+ pid_controller_f32_t pi; /* PI controller for res */
+ int cntr; /* Helper counter */
+ int stage; /* Ident stage */
+};
+
+/****************************************************************************
+ * Private Function Prototypes
+ ****************************************************************************/
+
+int foc_routine_ident_init_f32(FAR foc_routine_f32_t *r);
+void foc_routine_ident_deinit_f32(FAR foc_routine_f32_t *r);
+int foc_routine_ident_cfg_f32(FAR foc_routine_f32_t *r, FAR void *cfg);
+int foc_routine_ident_run_f32(FAR foc_routine_f32_t *r,
+ FAR struct foc_routine_in_f32_s *in,
+ FAR struct foc_routine_out_f32_s *out);
+int foc_routine_ident_final_f32(FAR foc_routine_f32_t *r, FAR void *data);
+
+/****************************************************************************
+ * Public Data
+ ****************************************************************************/
+
+struct foc_routine_ops_f32_s g_foc_routine_ident_f32 =
+{
+ .init = foc_routine_ident_init_f32,
+ .deinit = foc_routine_ident_deinit_f32,
+ .cfg = foc_routine_ident_cfg_f32,
+ .run = foc_routine_ident_run_f32,
+ .final = foc_routine_ident_final_f32,
+};
+
+/****************************************************************************
+ * Private Functions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Name: foc_ident_idle_run_f32
+ *
+ * Description:
+ * Force IDLE state
+ *
+ * Input Parameter:
+ * ident - pointer to FOC ident routine
+ * in - pointer to FOC routine input data
+ * out - pointer to FOC routine output data
+ *
+ ****************************************************************************/
+
+int foc_ident_idle_run_f32(FAR struct foc_ident_f32_s *ident,
+ FAR struct foc_routine_in_f32_s *in,
+ FAR struct foc_routine_out_f32_s *out)
+{
+ int ret = FOC_ROUTINE_RUN_NOTDONE;
+
+ /* Get output */
+
+ out->dq_ref.q = 0.0f;
+ out->dq_ref.d = 0.0f;
+ out->vdq_comp.q = 0.0f;
+ out->vdq_comp.d = 0.0f;
+ out->angle = 0.0f;
+ out->foc_mode = FOC_HANDLER_MODE_IDLE;
+
+ /* Increase counter */
+
+ ident->cntr += 1;
+
+ if (ident->cntr > ident->cfg.idle_steps)
+ {
+ /* Done */
+
+ ret = FOC_ROUTINE_RUN_DONE;
+
+ /* Reset counter */
+
+ ident->cntr = 0;
+ }
+
+ return ret;
+}
+
+/****************************************************************************
+ * Name: foc_ident_res_run_f32
+ *
+ * Description:
+ * Run resistance identification routine
+ *
+ * Input Parameter:
+ * ident - pointer to FOC ident routine
+ * in - pointer to FOC routine input data
+ * out - pointer to FOC routine output data
+ *
+ ****************************************************************************/
+
+int foc_ident_res_run_f32(FAR struct foc_ident_f32_s *ident,
+ FAR struct foc_routine_in_f32_s *in,
+ FAR struct foc_routine_out_f32_s *out)
+{
+ int ret = FOC_ROUTINE_RUN_NOTDONE;
+ float err = 0.0f;
+ float vref = 0.0f;
+
+ /* Initialize PI controller */
+
+ if (ident->cntr == 0)
+ {
+ pi_controller_init(&ident->pi, IDENT_PI_KP, IDENT_PI_KI);
+ }
+
+ /* PI saturation */
+
+ pi_saturation_set(&ident->pi, 0.0f, in->vbus);
+
+ /* Lock motor with given current */
+
+ err = ident->cfg.res_current - in->foc_state->idq.d;
+ vref = pi_controller(&ident->pi, err);
+
+ /* Force alpha voltage = vref */
+
+ out->dq_ref.q = 0.0f;
+ out->dq_ref.d = vref;
+ out->vdq_comp.q = 0.0f;
+ out->vdq_comp.d = 0.0f;
+ out->angle = 0.0f;
+ out->foc_mode = FOC_HANDLER_MODE_VOLTAGE;
+
+ /* Increase counter */
+
+ ident->cntr += 1;
+
+ if (ident->cntr > ident->cfg.res_steps)
+ {
+ /* Get resistance */
+
+ ident->final.res = vref / ident->cfg.res_current;
+
+ /* Force IDLE state */
+
+ out->dq_ref.q = 0.0f;
+ out->dq_ref.d = 0.0f;
+ out->vdq_comp.q = 0.0f;
+ out->vdq_comp.d = 0.0f;
+ out->angle = 0.0f;
+ out->foc_mode = FOC_HANDLER_MODE_IDLE;
+
+ /* Resistance identification done */
+
+ ret = FOC_ROUTINE_RUN_DONE;
+
+ /* Reset counter */
+
+ ident->cntr = 0;
+ }
+
+ return ret;
+}
+
+/****************************************************************************
+ * Name: foc_ident_ind_run_f32
+ *
+ * Description:
+ * Run inductance identification routine
+ *
+ * Input Parameter:
+ * ident - pointer to FOC ident routine
+ * in - pointer to FOC routine input data
+ * out - pointer to FOC routine output data
+ *
+ ****************************************************************************/
+
+int foc_ident_ind_run_f32(FAR struct foc_ident_f32_s *ident,
+ FAR struct foc_routine_in_f32_s *in,
+ FAR struct foc_routine_out_f32_s *out)
+{
+ int ret = FOC_ROUTINE_RUN_NOTDONE;
+ float vref = 0.0f;
+ float curr1_avg = 0.0f;
+ float curr2_avg = 0.0f;
+ float delta_curr = 0.0f;
+ static float sign = 1.0f;
+ static float curr1_sum = 0.0f;
+ static float curr2_sum = 0.0f;
+
+ /* If previous sign was -1 then we have top current,
+ * if previous sing was +1 then we have bottom current.
+ */
+
+ if (sign > 0)
+ {
+ /* Average bottm current */
+
+ curr1_sum += in->foc_state->idq.d;
+ }
+ else
+ {
+ /* Average top current */
+
+ curr2_sum += in->foc_state->idq.d;
+ }
+
+ /* Invert voltage to generate square wave D voltage */
+
+ sign = -sign;
+ vref = sign * ident->cfg.ind_volt;
+
+ /* Force alpha voltage = vref */
+
+ out->dq_ref.q = 0.0f;
+ out->dq_ref.d = vref;
+ out->vdq_comp.q = 0.0f;
+ out->vdq_comp.d = 0.0f;
+ out->angle = 0.0f;
+ out->foc_mode = FOC_HANDLER_MODE_VOLTAGE;
+
+ /* Increase counter */
+
+ ident->cntr += 1;
+
+ if (ident->cntr > ident->cfg.ind_steps)
+ {
+ /* Half samples from curr1, other half from curr2 */
+
+ curr1_avg = 2 * curr1_sum / ident->cntr;
+ curr2_avg = 2 * curr2_sum / ident->cntr;
+
+ /* Average delta current */
+
+ delta_curr = curr1_avg - curr2_avg;
+
+ /* Get inductance
+ * L = V * t / dI
+ *
+ * where:
+ * t = per
+ * V = ind_volt
+ * dI = avg(curr_bottom) - avg(curr_top)
+ */
+
+ ident->final.ind = ident->cfg.ind_volt * ident->cfg.per / delta_curr;
+
+ /* Force IDLE state */
+
+ out->dq_ref.q = 0.0f;
+ out->dq_ref.d = 0.0f;
+ out->vdq_comp.q = 0.0f;
+ out->vdq_comp.d = 0.0f;
+ out->angle = 0.0f;
+ out->foc_mode = FOC_HANDLER_MODE_IDLE;
+
+ /* Inductance identification done */
+
+ ret = FOC_ROUTINE_RUN_DONE;
+
+ /* Reset counter */
+
+ ident->cntr = 0;
+
+ /* Reset static data */
+
+ sign = 1.0f;
+ curr1_sum = 0.0f;
+ curr2_sum = 0.0f;
+ }
+
+ return ret;
+}
+
+/****************************************************************************
+ * Name: foc_routine_ident_init_f32
+ *
+ * Description:
+ * Initialize the FOC ident routine (float32)
+ *
+ * Input Parameter:
+ * r - pointer to FOC routine
+ *
+ ****************************************************************************/
+
+int foc_routine_ident_init_f32(FAR foc_routine_f32_t *r)
+{
+ int ret = OK;
+
+ DEBUGASSERT(r);
+
+ /* Connect angle data */
+
+ r->data = zalloc(sizeof(struct foc_ident_f32_s));
+ if (r->data == NULL)
+ {
+ ret = -ENOMEM;
+ goto errout;
+ }
+
+errout:
+ return ret;
+}
+
+/****************************************************************************
+ * Name: foc_routine_ident_deinit_f32
+ *
+ * Description:
+ * Deinitialize the FOC ident routine (float32)
+ *
+ * Input Parameter:
+ * r - pointer to FOC routine
+ *
+ ****************************************************************************/
+
+void foc_routine_ident_deinit_f32(FAR foc_routine_f32_t *r)
+{
+ DEBUGASSERT(r);
+
+ if (r->data)
+ {
+ /* Free routine data */
+
+ free(r->data);
+ }
+}
+
+/****************************************************************************
+ * Name: foc_routine_ident_cfg_f32
+ *
+ * Description:
+ * Configure the FOC ident routine (float32)
+ *
+ * Input Parameter:
+ * r - pointer to FOC routine
+ * cfg - pointer to ident routine configuration data
+ *
+ ****************************************************************************/
+
+int foc_routine_ident_cfg_f32(FAR foc_routine_f32_t *r, FAR void *cfg)
+{
+ FAR struct foc_ident_f32_s *i = NULL;
+ int ret = OK;
+
+ DEBUGASSERT(r);
+
+ /* Get ident data */
+
+ DEBUGASSERT(r->data);
+ i = r->data;
+
+ /* Copy configuration */
+
+ memcpy(&i->cfg, cfg, sizeof(struct foc_routine_ident_cfg_f32_s));
+
+ /* Verify configuration */
+
+ if (i->cfg.per <= 0.0f)
+ {
+ ret = -EINVAL;
+ goto errout;
+ }
+
+ if (i->cfg.res_current <= 0.0f || i->cfg.ind_volt <= 0.0f)
+ {
+ ret = -EINVAL;
+ goto errout;
+ }
+
+ if (i->cfg.res_steps <= 0 || i->cfg.ind_steps <= 0 ||
+ i->cfg.idle_steps <= 0)
+ {
+ ret = -EINVAL;
+ goto errout;
+ }
+
+errout:
+ return ret;
+}
+
+/****************************************************************************
+ * Name: foc_routine_ident_run_f32
+ *
+ * Description:
+ * Run the FOC ident routine step (float32)
+ *
+ * Input Parameter:
+ * r - pointer to FOC routine
+ * in - pointer to FOC routine input data
+ * out - pointer to FOC routine output data
+ *
+ ****************************************************************************/
+
+int foc_routine_ident_run_f32(FAR foc_routine_f32_t *r,
+ FAR struct foc_routine_in_f32_s *in,
+ FAR struct foc_routine_out_f32_s *out)
+{
+ FAR struct foc_ident_f32_s *i = NULL;
+ int ret = FOC_ROUTINE_RUN_NOTDONE;
+
+ DEBUGASSERT(r);
+ DEBUGASSERT(in);
+ DEBUGASSERT(out);
+
+ /* Get ident data */
+
+ DEBUGASSERT(r->data);
+ i = r->data;
+
+ /* Force IDLE state at default */
+
+ out->dq_ref.q = 0.0f;
+ out->dq_ref.d = 0.0f;
+ out->vdq_comp.q = 0.0f;
+ out->vdq_comp.d = 0.0f;
+ out->angle = 0.0f;
+ out->foc_mode = FOC_HANDLER_MODE_IDLE;
+
+ /* Handle ident stage */
+
+ switch (i->stage)
+ {
+ case FOC_IDENT_RUN_INIT:
+ {
+ i->stage += 1;
+ ret = FOC_ROUTINE_RUN_NOTDONE;
+
+ break;
+ }
+
+ case FOC_IDENT_RUN_IDLE1:
+ case FOC_IDENT_RUN_IDLE2:
+ case FOC_IDENT_RUN_IDLE3:
+ {
+ /* De-energetize motor */
+
+ ret = foc_ident_idle_run_f32(i, in, out);
+ if (ret < 0)
+ {
+ goto errout;
+ }
+
+ if (ret == FOC_ROUTINE_RUN_DONE)
+ {
+ i->stage += 1;
+ ret = FOC_ROUTINE_RUN_NOTDONE;
+ }
+
+ break;
+ }
+
+ case FOC_IDENT_RUN_RES:
+ {
+ /* Resistance */
+
+ ret = foc_ident_res_run_f32(i, in, out);
+ if (ret < 0)
+ {
+ goto errout;
+ }
+
+ if (ret == FOC_ROUTINE_RUN_DONE)
+ {
+ FOCLIBLOG("IDENT RES done!\n");
+
+ i->stage += 1;
+ ret = FOC_ROUTINE_RUN_NOTDONE;
+ }
+
+ break;
+ }
+
+ case FOC_IDENT_RUN_IND:
+ {
+ /* Inductance */
+
+ ret = foc_ident_ind_run_f32(i, in, out);
+ if (ret < 0)
+ {
+ goto errout;
+ }
+
+ if (ret == FOC_ROUTINE_RUN_DONE)
+ {
+ FOCLIBLOG("IDENT IND done!\n");
+
+ i->stage += 1;
+ ret = FOC_ROUTINE_RUN_NOTDONE;
+ }
+
+ break;
+ }
+
+ case FOC_IDENT_RUN_DONE:
+ {
+ ret = FOC_ROUTINE_RUN_DONE;
+
+ break;
+ }
+
+ default:
+ {
+ FOCLIBERR("ERROR: invalid ident stage %d\n", i->stage);
+ ret = -EINVAL;
+ goto errout;
+ }
+ }
+
+errout:
+ return ret;
+}
+
+/****************************************************************************
+ * Name: foc_routine_ident_final_f32
+ *
+ * Description:
+ * Finalize the FOC routine data (float32)
+ *
+ * Input Parameter:
+ * r - pointer to FOC routine
+ * data - pointer to FOC ident routine final data
+ *
+ ****************************************************************************/
+
+int foc_routine_ident_final_f32(FAR foc_routine_f32_t *r, FAR void *data)
+{
+ FAR struct foc_ident_f32_s *i = NULL;
+
+ DEBUGASSERT(r);
+ DEBUGASSERT(data);
+
+ /* Get ident data */
+
+ DEBUGASSERT(r->data);
+ i = r->data;
+
+ /* Get final data */
+
+ memcpy(data, &i->final, sizeof(struct foc_routine_ident_final_f32_s));
+
+ return OK;
+}