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 2022/02/20 13:55:23 UTC
[incubator-nuttx-apps] 02/02: foclib/fixed16: add support for observers
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 fbb09ff12884a63e3c60e11234b88fa57d1e8a73
Author: raiden00pl <ra...@railab.me>
AuthorDate: Sun Feb 20 12:54:51 2022 +0100
foclib/fixed16: add support for observers
---
include/industry/foc/fixed16/foc_angle.h | 32 +++
include/industry/foc/fixed16/foc_handler.h | 1 +
include/industry/foc/fixed16/foc_velocity.h | 38 ++++
industry/foc/Makefile | 12 ++
industry/foc/fixed16/foc_ang_onfo.c | 321 ++++++++++++++++++++++++++++
industry/foc/fixed16/foc_ang_osmo.c | 283 ++++++++++++++++++++++++
industry/foc/fixed16/foc_picontrol.c | 4 +
industry/foc/fixed16/foc_vel_odiv.c | 286 +++++++++++++++++++++++++
industry/foc/fixed16/foc_vel_opll.c | 284 ++++++++++++++++++++++++
9 files changed, 1261 insertions(+)
diff --git a/include/industry/foc/fixed16/foc_angle.h b/include/industry/foc/fixed16/foc_angle.h
index 55e2a3a..ba6cd22 100644
--- a/include/industry/foc/fixed16/foc_angle.h
+++ b/include/industry/foc/fixed16/foc_angle.h
@@ -106,6 +106,26 @@ struct foc_openloop_cfg_b16_s
};
#endif
+#ifdef CONFIG_INDUSTRY_FOC_ANGLE_ONFO
+struct foc_angle_onfo_cfg_b16_s
+{
+ b16_t per; /* Controller period */
+ b16_t gain;
+ b16_t gain_slow;
+ struct motor_phy_params_b16_s phy;
+};
+#endif
+
+#ifdef CONFIG_INDUSTRY_FOC_ANGLE_OSMO
+struct foc_angle_osmo_cfg_b16_s
+{
+ b16_t per; /* Controller period */
+ b16_t k_slide; /* Bang-bang controller gain */
+ b16_t err_max; /* Linear mode threshold */
+ struct motor_phy_params_b16_s phy;
+};
+#endif
+
#ifdef CONFIG_INDUSTRY_FOC_ANGLE_QENCO
/* Qencoder configuration data */
@@ -136,6 +156,18 @@ struct foc_hall_cfg_b16_s
extern struct foc_angle_ops_b16_s g_foc_angle_ol_b16;
#endif
+#ifdef CONFIG_INDUSTRY_FOC_ANGLE_ONFO
+/* NFO oberver angle operations (fixed16) */
+
+extern struct foc_angle_ops_b16_s g_foc_angle_onfo_b16;
+#endif
+
+#ifdef CONFIG_INDUSTRY_FOC_ANGLE_OSMO
+/* SMO oberver angle operations (fixed16) */
+
+extern struct foc_angle_ops_b16_s g_foc_angle_osmo_b16;
+#endif
+
#ifdef CONFIG_INDUSTRY_FOC_ANGLE_QENCO
/* Qencoder angle operations (fixed16) */
diff --git a/include/industry/foc/fixed16/foc_handler.h b/include/industry/foc/fixed16/foc_handler.h
index f618f2f..793aeaf 100644
--- a/include/industry/foc/fixed16/foc_handler.h
+++ b/include/industry/foc/fixed16/foc_handler.h
@@ -66,6 +66,7 @@ struct foc_state_b16_s
ab_frame_b16_t vab;
dq_frame_b16_t vdq;
dq_frame_b16_t idq;
+ b16_t mod_scale;
};
/* Forward declaration */
diff --git a/include/industry/foc/fixed16/foc_velocity.h b/include/industry/foc/fixed16/foc_velocity.h
index fdab3f1..14119e9 100644
--- a/include/industry/foc/fixed16/foc_velocity.h
+++ b/include/industry/foc/fixed16/foc_velocity.h
@@ -95,6 +95,44 @@ struct foc_velocity_b16_s
FAR void *data;
};
+#ifdef CONFIG_INDUSTRY_FOC_VELOCITY_ODIV
+/* Velocity DIV observer */
+
+struct foc_vel_div_b16_cfg_s
+{
+ uint8_t samples;
+ b16_t filter;
+ b16_t per;
+};
+#endif
+
+#ifdef CONFIG_INDUSTRY_FOC_VELOCITY_OPLL
+/* Velocity PLL observer */
+
+struct foc_vel_pll_b16_cfg_s
+{
+ b16_t kp;
+ b16_t ki;
+ b16_t per;
+};
+#endif
+
+/****************************************************************************
+ * Public Data
+ ****************************************************************************/
+
+#ifdef CONFIG_INDUSTRY_FOC_VELOCITY_ODIV
+/* Velocity DIV observer (fixed16) */
+
+extern struct foc_velocity_ops_b16_s g_foc_velocity_odiv_b16;
+#endif
+
+#ifdef CONFIG_INDUSTRY_FOC_VELOCITY_OPLL
+/* Velocity PLL observer (fixed16) */
+
+extern struct foc_velocity_ops_b16_s g_foc_velocity_opll_b16;
+#endif
+
/****************************************************************************
* Public Function Prototypes
****************************************************************************/
diff --git a/industry/foc/Makefile b/industry/foc/Makefile
index e1797f1..4923de1 100644
--- a/industry/foc/Makefile
+++ b/industry/foc/Makefile
@@ -90,6 +90,12 @@ CSRCS += fixed16/foc_routine.c
ifeq ($(CONFIG_INDUSTRY_FOC_ANGLE_OPENLOOP),y)
CSRCS += fixed16/foc_ang_openloop.c
endif
+ifeq ($(CONFIG_INDUSTRY_FOC_ANGLE_ONFO),y)
+CSRCS += fixed16/foc_ang_onfo.c
+endif
+ifeq ($(CONFIG_INDUSTRY_FOC_ANGLE_OSMO),y)
+CSRCS += fixed16/foc_ang_osmo.c
+endif
ifeq ($(CONFIG_INDUSTRY_FOC_ANGLE_QENCO),y)
CSRCS += fixed16/foc_ang_qenco.c
endif
@@ -117,6 +123,12 @@ endif
ifeq ($(CONFIG_INDUSTRY_FOC_IDENT),y)
CSRCS += fixed16/foc_ident.c
endif
+ifeq ($(CONFIG_INDUSTRY_FOC_VELOCITY_ODIV),y)
+CSRCS += fixed16/foc_vel_odiv.c
+endif
+ifeq ($(CONFIG_INDUSTRY_FOC_VELOCITY_OPLL),y)
+CSRCS += fixed16/foc_vel_opll.c
+endif
endif
diff --git a/industry/foc/fixed16/foc_ang_onfo.c b/industry/foc/fixed16/foc_ang_onfo.c
new file mode 100644
index 0000000..35fbe63
--- /dev/null
+++ b/industry/foc/fixed16/foc_ang_onfo.c
@@ -0,0 +1,321 @@
+/****************************************************************************
+ * apps/industry/foc/fixed16/foc_ang_onfo.c
+ *
+ * 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_log.h"
+#include "industry/foc/fixed16/foc_angle.h"
+
+/****************************************************************************
+ * Pre-processor Definitions
+ ****************************************************************************/
+
+#define SIGN(x) ((x > 0) ? b16ONE : -b16ONE)
+
+#define LINEAR_MAP(x, in_min, in_max, out_min, out_max) \
+ (b16divb16(b16mulb16((x - in_min), (out_max - out_min)), \
+ (in_max - in_min)) + out_min)
+
+#ifndef ABS
+# define ABS(a) (a < 0 ? -a : a)
+#endif
+
+/****************************************************************************
+ * Private Data Types
+ ****************************************************************************/
+
+/* sensorless observer data */
+
+struct foc_ang_onfo_b16_s
+{
+ struct foc_angle_onfo_cfg_b16_s cfg;
+ struct motor_aobserver_nfo_b16_s data;
+ struct motor_aobserver_b16_s o;
+ b16_t sensor_dir;
+};
+
+/****************************************************************************
+ * Private Function Prototypes
+ ****************************************************************************/
+
+static int foc_angle_onfo_init_b16(FAR foc_angle_b16_t *h);
+static void foc_angle_onfo_deinit_b16(FAR foc_angle_b16_t *h);
+static int foc_angle_onfo_cfg_b16(FAR foc_angle_b16_t *h, FAR void *cfg);
+static int foc_angle_onfo_zero_b16(FAR foc_angle_b16_t *h);
+static int foc_angle_onfo_dir_b16(FAR foc_angle_b16_t *h, b16_t dir);
+static int foc_angle_onfo_run_b16(FAR foc_angle_b16_t *h,
+ FAR struct foc_angle_in_b16_s *in,
+ FAR struct foc_angle_out_b16_s *out);
+
+/****************************************************************************
+ * Public Data
+ ****************************************************************************/
+
+/* FOC angle b16_t interface */
+
+struct foc_angle_ops_b16_s g_foc_angle_onfo_b16 =
+{
+ .init = foc_angle_onfo_init_b16,
+ .deinit = foc_angle_onfo_deinit_b16,
+ .cfg = foc_angle_onfo_cfg_b16,
+ .zero = foc_angle_onfo_zero_b16,
+ .dir = foc_angle_onfo_dir_b16,
+ .run = foc_angle_onfo_run_b16,
+};
+
+/****************************************************************************
+ * Private Functions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Name: foc_angle_onfo_init_b16
+ *
+ * Description:
+ * Initialize the sensorless observer FOC angle handler (fixed16)
+ *
+ * Input Parameter:
+ * h - pointer to FOC angle handler
+ *
+ ****************************************************************************/
+
+static int foc_angle_onfo_init_b16(FAR foc_angle_b16_t *h)
+{
+ int ret = OK;
+
+ DEBUGASSERT(h);
+
+ /* Connect angle data */
+
+ h->data = zalloc(sizeof(struct foc_ang_onfo_b16_s));
+ if (h->data == NULL)
+ {
+ ret = -ENOMEM;
+ goto errout;
+ }
+
+errout:
+ return ret;
+}
+
+/****************************************************************************
+ * Name: foc_angle_onfo_deinit_b16
+ *
+ * Description:
+ * De-initialize the sensorless observer FOC angle handler (fixed16)
+ *
+ * Input Parameter:
+ * h - pointer to FOC angle handler
+ *
+ ****************************************************************************/
+
+static void foc_angle_onfo_deinit_b16(FAR foc_angle_b16_t *h)
+{
+ DEBUGASSERT(h);
+
+ if (h->data)
+ {
+ /* Free angle data */
+
+ free(h->data);
+ }
+}
+
+/****************************************************************************
+ * Name: foc_angle_onfo_cfg_b16
+ *
+ * Description:
+ * Configure the sensorless observer FOC angle handler (fixed16)
+ *
+ * Input Parameter:
+ * h - pointer to FOC angle handler
+ * cfg - pointer to angle handler configuration data
+ * (struct foc_ang_onfo_b16_s)
+ *
+ ****************************************************************************/
+
+static int foc_angle_onfo_cfg_b16(FAR foc_angle_b16_t *h, FAR void *cfg)
+{
+ FAR struct foc_ang_onfo_b16_s *ob = NULL;
+ int ret = OK;
+
+ DEBUGASSERT(h);
+
+ /* Get sensorless observer data */
+
+ DEBUGASSERT(h->data);
+ ob = h->data;
+
+ /* Copy configuration */
+
+ memcpy(&ob->cfg, cfg, sizeof(struct foc_angle_onfo_cfg_b16_s));
+
+ /* Initialize sensorless observer controller data */
+
+ DEBUGASSERT(ob->cfg.per > 0);
+
+ /* Initialize nolinear fluxlink angle observer */
+
+ motor_aobserver_nfo_init_b16(&ob->data);
+
+ /* Initialize sensorless observer */
+
+ motor_aobserver_init_b16(&ob->o, &ob->data, ob->cfg.per);
+
+ /* Initialize with CW direction */
+
+ ob->sensor_dir = DIR_CW_B16;
+
+ return ret;
+}
+
+/****************************************************************************
+ * Name: foc_angle_onfo_zero_b16
+ *
+ * Description:
+ * Zero the sensorless observer FOC angle handler (fixed16)
+ *
+ * Input Parameter:
+ * h - pointer to FOC angle handler
+ *
+ ****************************************************************************/
+
+static int foc_angle_onfo_zero_b16(FAR foc_angle_b16_t *h)
+{
+ FAR struct foc_ang_onfo_b16_s *ob = NULL;
+
+ DEBUGASSERT(h);
+
+ /* Get sensorless observer data */
+
+ DEBUGASSERT(h->data);
+ ob = h->data;
+
+ /* Reinitialize observer data */
+
+ motor_aobserver_nfo_init_b16(&ob->data);
+
+ /* Reset angle */
+
+ ob->o.angle = 0;
+
+ return OK;
+}
+
+/****************************************************************************
+ * Name: foc_angle_onfo_dir_b16
+ *
+ * Description:
+ * Set the sensorless observer FOC angle handler direction (fixed16)
+ *
+ * Input Parameter:
+ * h - pointer to FOC angle handler
+ * dir - sensor direction (1 if normal -1 if inverted)
+ *
+ ****************************************************************************/
+
+static int foc_angle_onfo_dir_b16(FAR foc_angle_b16_t *h, b16_t dir)
+{
+ FAR struct foc_ang_onfo_b16_s *ob = NULL;
+
+ DEBUGASSERT(h);
+
+ /* Get sensorless observer data */
+
+ DEBUGASSERT(h->data);
+ ob = h->data;
+
+ /* Configure direction */
+
+ ob->sensor_dir = dir;
+
+ return OK;
+}
+
+/****************************************************************************
+ * Name: foc_angle_onfo_run_b16
+ *
+ * Description:
+ * Process the FOC sensorless observer angle data (fixed16)
+ *
+ * Input Parameter:
+ * h - pointer to FOC angle handler
+ * in - pointer to FOC angle handler input data
+ * out - pointer to FOC angle handler output data
+ *
+ ****************************************************************************/
+
+static int foc_angle_onfo_run_b16(FAR foc_angle_b16_t *h,
+ FAR struct foc_angle_in_b16_s *in,
+ FAR struct foc_angle_out_b16_s *out)
+{
+ FAR struct foc_ang_onfo_b16_s *ob = NULL;
+ FAR dq_frame_b16_t v_dq_mod;
+ b16_t duty_now;
+ b16_t dyn_gain;
+
+ DEBUGASSERT(h);
+
+ /* Get sensorless observer data */
+
+ DEBUGASSERT(h->data);
+ ob = h->data;
+
+ /* Normalize the d-q voltage to get the d-q modulation
+ * voltage
+ */
+
+ v_dq_mod.d = b16mulb16(in->state->vdq.d, in->state->mod_scale);
+ v_dq_mod.q = b16mulb16(in->state->vdq.q, in->state->mod_scale);
+
+ /* Update duty cycle now */
+
+ duty_now = b16mulb16(SIGN(in->state->vdq.q),
+ vector2d_mag_b16(v_dq_mod.d, v_dq_mod.q));
+
+ /* Update and the observer gain. */
+
+ dyn_gain = b16mulb16(LINEAR_MAP(ABS(duty_now),
+ 0,
+ b16ONE,
+ b16mulb16(ob->cfg.gain, ob->cfg.gain_slow),
+ ob->cfg.gain),
+ b16HALF);
+
+ /* Update observer */
+
+ motor_aobserver_nfo_b16(&ob->o, &in->state->iab, &in->state->vab,
+ &ob->cfg.phy, dyn_gain);
+
+ /* Copy data */
+
+ out->type = FOC_ANGLE_TYPE_ELE;
+ out->angle = b16mulb16(ob->sensor_dir,
+ motor_aobserver_angle_get_b16(&ob->o));
+
+ return OK;
+}
diff --git a/industry/foc/fixed16/foc_ang_osmo.c b/industry/foc/fixed16/foc_ang_osmo.c
new file mode 100644
index 0000000..90acd2a
--- /dev/null
+++ b/industry/foc/fixed16/foc_ang_osmo.c
@@ -0,0 +1,283 @@
+/****************************************************************************
+ * apps/industry/foc/fixed16/foc_ang_osmo.c
+ *
+ * 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_log.h"
+#include "industry/foc/fixed16/foc_angle.h"
+
+/****************************************************************************
+ * Private Data Types
+ ****************************************************************************/
+
+/* sensorless observer data */
+
+struct foc_ang_osmo_b16_s
+{
+ struct foc_angle_osmo_cfg_b16_s cfg;
+ struct motor_aobserver_smo_b16_s data;
+ struct motor_aobserver_b16_s o;
+ b16_t sensor_dir;
+};
+
+/****************************************************************************
+ * Private Function Prototypes
+ ****************************************************************************/
+
+static int foc_angle_osmo_init_b16(FAR foc_angle_b16_t *h);
+static void foc_angle_osmo_deinit_b16(FAR foc_angle_b16_t *h);
+static int foc_angle_osmo_cfg_b16(FAR foc_angle_b16_t *h, FAR void *cfg);
+static int foc_angle_osmo_zero_b16(FAR foc_angle_b16_t *h);
+static int foc_angle_osmo_dir_b16(FAR foc_angle_b16_t *h, b16_t dir);
+static int foc_angle_osmo_run_b16(FAR foc_angle_b16_t *h,
+ FAR struct foc_angle_in_b16_s *in,
+ FAR struct foc_angle_out_b16_s *out);
+
+/****************************************************************************
+ * Public Data
+ ****************************************************************************/
+
+/* FOC angle b16_t interface */
+
+struct foc_angle_ops_b16_s g_foc_angle_osmo_b16 =
+{
+ .init = foc_angle_osmo_init_b16,
+ .deinit = foc_angle_osmo_deinit_b16,
+ .cfg = foc_angle_osmo_cfg_b16,
+ .zero = foc_angle_osmo_zero_b16,
+ .dir = foc_angle_osmo_dir_b16,
+ .run = foc_angle_osmo_run_b16,
+};
+
+/****************************************************************************
+ * Private Functions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Name: foc_angle_osmo_init_b16
+ *
+ * Description:
+ * Initialize the sensorless observer FOC angle handler (fixed16)
+ *
+ * Input Parameter:
+ * h - pointer to FOC angle handler
+ *
+ ****************************************************************************/
+
+static int foc_angle_osmo_init_b16(FAR foc_angle_b16_t *h)
+{
+ int ret = OK;
+
+ DEBUGASSERT(h);
+
+ /* Connect angle data */
+
+ h->data = zalloc(sizeof(struct foc_ang_osmo_b16_s));
+ if (h->data == NULL)
+ {
+ ret = -ENOMEM;
+ goto errout;
+ }
+
+errout:
+ return ret;
+}
+
+/****************************************************************************
+ * Name: foc_angle_osmo_deinit_b16
+ *
+ * Description:
+ * De-initialize the sensorless observer FOC angle handler (fixed16)
+ *
+ * Input Parameter:
+ * h - pointer to FOC angle handler
+ *
+ ****************************************************************************/
+
+static void foc_angle_osmo_deinit_b16(FAR foc_angle_b16_t *h)
+{
+ DEBUGASSERT(h);
+
+ if (h->data)
+ {
+ /* Free angle data */
+
+ free(h->data);
+ }
+}
+
+/****************************************************************************
+ * Name: foc_angle_osmo_cfg_b16
+ *
+ * Description:
+ * Configure the sensorless observer FOC angle handler (fixed16)
+ *
+ * Input Parameter:
+ * h - pointer to FOC angle handler
+ * cfg - pointer to angle handler configuration data
+ * (struct foc_ang_osmo_b16_s)
+ *
+ ****************************************************************************/
+
+static int foc_angle_osmo_cfg_b16(FAR foc_angle_b16_t *h, FAR void *cfg)
+{
+ FAR struct foc_ang_osmo_b16_s *ob = NULL;
+ int ret = OK;
+
+ DEBUGASSERT(h);
+
+ /* Get sensorless observer data */
+
+ DEBUGASSERT(h->data);
+ ob = h->data;
+
+ /* Copy configuration */
+
+ memcpy(&ob->cfg, cfg, sizeof(struct foc_angle_osmo_cfg_b16_s));
+
+ /* Initialize sensorless observer controller data */
+
+ DEBUGASSERT(ob->cfg.per > 0);
+
+ /* Initialize SMO angle observer */
+
+ motor_aobserver_smo_init_b16(&ob->data, ob->cfg.k_slide, ob->cfg.err_max);
+
+ /* Initialize sensorless observer */
+
+ motor_aobserver_init_b16(&ob->o, &ob->data, ob->cfg.per);
+
+ /* Initialize with CW direction */
+
+ ob->sensor_dir = DIR_CW_B16;
+
+ return ret;
+}
+
+/****************************************************************************
+ * Name: foc_angle_osmo_zero_b16
+ *
+ * Description:
+ * Zero the sensorless observer FOC angle handler (fixed16)
+ *
+ * Input Parameter:
+ * h - pointer to FOC angle handler
+ *
+ ****************************************************************************/
+
+static int foc_angle_osmo_zero_b16(FAR foc_angle_b16_t *h)
+{
+ FAR struct foc_ang_osmo_b16_s *ob = NULL;
+
+ DEBUGASSERT(h);
+
+ /* Get sensorless observer data */
+
+ DEBUGASSERT(h->data);
+ ob = h->data;
+
+ /* Reinitialize observer data */
+
+ motor_aobserver_smo_init_b16(&ob->data, ob->cfg.k_slide, ob->cfg.err_max);
+
+ /* Reset angle */
+
+ ob->o.angle = 0;
+
+ return OK;
+}
+
+/****************************************************************************
+ * Name: foc_angle_osmo_dir_b16
+ *
+ * Description:
+ * Set the sensorless observer FOC angle handler direction (fixed16)
+ *
+ * Input Parameter:
+ * h - pointer to FOC angle handler
+ * dir - sensor direction (1 if normal -1 if inverted)
+ *
+ ****************************************************************************/
+
+static int foc_angle_osmo_dir_b16(FAR foc_angle_b16_t *h, b16_t dir)
+{
+ FAR struct foc_ang_osmo_b16_s *ob = NULL;
+
+ DEBUGASSERT(h);
+
+ /* Get sensorless observer data */
+
+ DEBUGASSERT(h->data);
+ ob = h->data;
+
+ /* Configure direction */
+
+ ob->sensor_dir = dir;
+
+ return OK;
+}
+
+/****************************************************************************
+ * Name: foc_angle_osmo_run_b16
+ *
+ * Description:
+ * Process the FOC sensorless observer angle data (fixed16)
+ *
+ * Input Parameter:
+ * h - pointer to FOC angle handler
+ * in - pointer to FOC angle handler input data
+ * out - pointer to FOC angle handler output data
+ *
+ ****************************************************************************/
+
+static int foc_angle_osmo_run_b16(FAR foc_angle_b16_t *h,
+ FAR struct foc_angle_in_b16_s *in,
+ FAR struct foc_angle_out_b16_s *out)
+{
+ FAR struct foc_ang_osmo_b16_s *ob = NULL;
+
+ DEBUGASSERT(h);
+
+ /* Get sensorless observer data */
+
+ DEBUGASSERT(h->data);
+ ob = h->data;
+
+ /* Update observer */
+
+ motor_aobserver_smo_b16(&ob->o, &in->state->iab, &in->state->vab,
+ &ob->cfg.phy, in->dir, in->vel);
+
+ /* Copy data */
+
+ out->type = FOC_ANGLE_TYPE_ELE;
+ out->angle = b16mulb16(ob->sensor_dir,
+ motor_aobserver_angle_get_b16(&ob->o));
+
+ return OK;
+}
diff --git a/industry/foc/fixed16/foc_picontrol.c b/industry/foc/fixed16/foc_picontrol.c
index 2540aa4..66ee265 100644
--- a/industry/foc/fixed16/foc_picontrol.c
+++ b/industry/foc/fixed16/foc_picontrol.c
@@ -415,4 +415,8 @@ static void foc_control_state_get_b16(FAR foc_handler_b16_t *h,
state->volt[0] = foc->data.v_abc.a;
state->volt[1] = foc->data.v_abc.b;
state->volt[2] = foc->data.v_abc.c;
+
+ /* Copy modulation scale */
+
+ state->mod_scale = foc->data.vab_mod_scale;
}
diff --git a/industry/foc/fixed16/foc_vel_odiv.c b/industry/foc/fixed16/foc_vel_odiv.c
new file mode 100644
index 0000000..5df8518
--- /dev/null
+++ b/industry/foc/fixed16/foc_vel_odiv.c
@@ -0,0 +1,286 @@
+/****************************************************************************
+ * apps/industry/foc/fixed16/foc_vel_odiv.c
+ *
+ * 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 <dspb16.h>
+
+#include "industry/foc/foc_common.h"
+#include "industry/foc/foc_log.h"
+#include "industry/foc/fixed16/foc_velocity.h"
+
+/****************************************************************************
+ * Pre-processor Definitions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Data Types
+ ****************************************************************************/
+
+/* Div private data */
+
+struct foc_div_b16_s
+{
+ struct foc_vel_div_b16_cfg_s cfg;
+ struct motor_sobserver_div_b16_s data;
+ struct motor_sobserver_b16_s o;
+ b16_t sensor_dir;
+};
+
+/****************************************************************************
+ * Private Function Prototypes
+ ****************************************************************************/
+
+static int foc_velocity_div_init_b16(FAR foc_velocity_b16_t *h);
+static void foc_velocity_div_deinit_b16(FAR foc_velocity_b16_t *h);
+static int foc_velocity_div_cfg_b16(FAR foc_velocity_b16_t *h,
+ FAR void *cfg);
+static int foc_velocity_div_zero_b16(FAR foc_velocity_b16_t *h);
+static int foc_velocity_div_dir_b16(FAR foc_velocity_b16_t *h, b16_t dir);
+static int foc_velocity_div_run_b16(FAR foc_velocity_b16_t *h,
+ FAR struct foc_velocity_in_b16_s *in,
+ FAR struct foc_velocity_out_b16_s *out);
+
+/****************************************************************************
+ * Public Data
+ ****************************************************************************/
+
+/* FOC velocity b16_t interface */
+
+struct foc_velocity_ops_b16_s g_foc_velocity_odiv_b16 =
+{
+ .init = foc_velocity_div_init_b16,
+ .deinit = foc_velocity_div_deinit_b16,
+ .cfg = foc_velocity_div_cfg_b16,
+ .zero = foc_velocity_div_zero_b16,
+ .dir = foc_velocity_div_dir_b16,
+ .run = foc_velocity_div_run_b16,
+};
+
+/****************************************************************************
+ * Private Functions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Name: foc_velocity_div_init_b16
+ *
+ * Description:
+ * Initialize the DIV velocity observer (fixed16)
+ *
+ * Input Parameter:
+ * h - pointer to FOC velocity handler
+ *
+ ****************************************************************************/
+
+static int foc_velocity_div_init_b16(FAR foc_velocity_b16_t *h)
+{
+ int ret = OK;
+
+ DEBUGASSERT(h);
+
+ /* Connect velocity data */
+
+ h->data = zalloc(sizeof(struct foc_div_b16_s));
+ if (h->data == NULL)
+ {
+ ret = -ENOMEM;
+ goto errout;
+ }
+
+errout:
+ return ret;
+}
+
+/****************************************************************************
+ * Name: foc_velocity_div_deinit_b16
+ *
+ * Description:
+ * De-initialize the DIV velocity observer (fixed16)
+ *
+ * Input Parameter:
+ * h - pointer to FOC velocity handler
+ *
+ ****************************************************************************/
+
+static void foc_velocity_div_deinit_b16(FAR foc_velocity_b16_t *h)
+{
+ DEBUGASSERT(h);
+
+ if (h->data)
+ {
+ /* Free velocity data */
+
+ free(h->data);
+ }
+}
+
+/****************************************************************************
+ * Name: foc_velocity_div_cfg_b16
+ *
+ * Description:
+ * Configure the DIV velocity observer (fixed16)
+ *
+ * Input Parameter:
+ * h - pointer to FOC velocity handler
+ * cfg - pointer to velocity handler configuration data
+ * (struct foc_div_b16_s)
+ *
+ ****************************************************************************/
+
+static int foc_velocity_div_cfg_b16(FAR foc_velocity_b16_t *h, FAR void *cfg)
+{
+ FAR struct foc_div_b16_s *div = NULL;
+ int ret = OK;
+
+ DEBUGASSERT(h);
+
+ /* Get div data */
+
+ DEBUGASSERT(h->data);
+ div = h->data;
+
+ /* Copy configuration */
+
+ memcpy(&div->cfg, cfg, sizeof(struct foc_vel_div_b16_cfg_s));
+
+ /* Configure observer */
+
+ motor_sobserver_div_init_b16(&div->data,
+ div->cfg.samples,
+ div->cfg.filter,
+ div->cfg.per);
+
+ motor_sobserver_init_b16(&div->o, &div->data, div->cfg.per);
+
+ /* Initialize with CW direction */
+
+ div->sensor_dir = DIR_CW_B16;
+
+ return ret;
+}
+
+/****************************************************************************
+ * Name: foc_velocity_div_zero_b16
+ *
+ * Description:
+ * Zero the DIV velocity observer (fixed16)
+ *
+ * Input Parameter:
+ * h - pointer to FOC velocity handler
+ *
+ ****************************************************************************/
+
+static int foc_velocity_div_zero_b16(FAR foc_velocity_b16_t *h)
+{
+ FAR struct foc_div_b16_s *div = NULL;
+ int ret = OK;
+
+ DEBUGASSERT(h);
+
+ /* Get div data */
+
+ DEBUGASSERT(h->data);
+ div = h->data;
+
+ /* Reinitialize observer */
+
+ motor_sobserver_div_init_b16(&div->data,
+ div->cfg.samples,
+ div->cfg.filter,
+ div->cfg.per);
+
+ return ret;
+}
+
+/****************************************************************************
+ * Name: foc_velocity_div_dir_b16
+ *
+ * Description:
+ * Set the DIV velocity observer direction (fixed16)
+ *
+ * Input Parameter:
+ * h - pointer to FOC velocity handler
+ * dir - sensor direction (1 if normal -1 if inverted)
+ *
+ ****************************************************************************/
+
+static int foc_velocity_div_dir_b16(FAR foc_velocity_b16_t *h, b16_t dir)
+{
+ FAR struct foc_div_b16_s *div = NULL;
+
+ DEBUGASSERT(h);
+
+ /* Get div data */
+
+ DEBUGASSERT(h->data);
+ div = h->data;
+
+ /* Set direction */
+
+ div->sensor_dir = dir;
+
+ return OK;
+}
+
+/****************************************************************************
+ * Name: foc_velocity_div_run_b16
+ *
+ * Description:
+ * Process the DIV velocity observer (fixed16)
+ *
+ * Input Parameter:
+ * h - pointer to FOC velocity handler
+ * in - pointer to FOC velocity handler input data
+ * out - pointer to FOC velocity handler output data
+ *
+ ****************************************************************************/
+
+static int foc_velocity_div_run_b16(FAR foc_velocity_b16_t *h,
+ FAR struct foc_velocity_in_b16_s *in,
+ FAR struct foc_velocity_out_b16_s *out)
+{
+ FAR struct foc_div_b16_s *div = NULL;
+
+ DEBUGASSERT(h);
+
+ /* Get div data */
+
+ DEBUGASSERT(h->data);
+ div = h->data;
+
+ /* Run observer */
+
+ motor_sobserver_div_b16(&div->o, in->angle);
+
+ /* Copy data */
+
+ out->velocity = b16mulb16(div->sensor_dir,
+ motor_sobserver_speed_get_b16(&div->o));
+
+ return OK;
+}
diff --git a/industry/foc/fixed16/foc_vel_opll.c b/industry/foc/fixed16/foc_vel_opll.c
new file mode 100644
index 0000000..b16cbee
--- /dev/null
+++ b/industry/foc/fixed16/foc_vel_opll.c
@@ -0,0 +1,284 @@
+/****************************************************************************
+ * apps/industry/foc/fixed16/foc_vel_opll.c
+ *
+ * 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 <dspb16.h>
+
+#include "industry/foc/foc_common.h"
+#include "industry/foc/foc_log.h"
+#include "industry/foc/fixed16/foc_velocity.h"
+
+/****************************************************************************
+ * Pre-processor Definitions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Data Types
+ ****************************************************************************/
+
+/* PLL observer private data */
+
+struct foc_pll_b16_s
+{
+ struct foc_vel_pll_b16_cfg_s cfg;
+ struct motor_sobserver_pll_b16_s data;
+ struct motor_sobserver_b16_s o;
+ b16_t sensor_dir;
+};
+
+/****************************************************************************
+ * Private Function Prototypes
+ ****************************************************************************/
+
+static int foc_velocity_pll_init_b16(FAR foc_velocity_b16_t *h);
+static void foc_velocity_pll_deinit_b16(FAR foc_velocity_b16_t *h);
+static int foc_velocity_pll_cfg_b16(FAR foc_velocity_b16_t *h,
+ FAR void *cfg);
+static int foc_velocity_pll_zero_b16(FAR foc_velocity_b16_t *h);
+static int foc_velocity_pll_dir_b16(FAR foc_velocity_b16_t *h, b16_t dir);
+static int foc_velocity_pll_run_b16(FAR foc_velocity_b16_t *h,
+ FAR struct foc_velocity_in_b16_s *in,
+ FAR struct foc_velocity_out_b16_s *out);
+
+/****************************************************************************
+ * Public Data
+ ****************************************************************************/
+
+/* FOC velocity b16_t interface */
+
+struct foc_velocity_ops_b16_s g_foc_velocity_opll_b16 =
+{
+ .init = foc_velocity_pll_init_b16,
+ .deinit = foc_velocity_pll_deinit_b16,
+ .cfg = foc_velocity_pll_cfg_b16,
+ .zero = foc_velocity_pll_zero_b16,
+ .dir = foc_velocity_pll_dir_b16,
+ .run = foc_velocity_pll_run_b16,
+};
+
+/****************************************************************************
+ * Private Functions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Name: foc_velocity_pll_init_b16
+ *
+ * Description:
+ * Initialize the PLL velocity observer (fixed16)
+ *
+ * Input Parameter:
+ * h - pointer to FOC velocity handler
+ *
+ ****************************************************************************/
+
+static int foc_velocity_pll_init_b16(FAR foc_velocity_b16_t *h)
+{
+ int ret = OK;
+
+ DEBUGASSERT(h);
+
+ /* Connect velocity data */
+
+ h->data = zalloc(sizeof(struct foc_pll_b16_s));
+ if (h->data == NULL)
+ {
+ ret = -ENOMEM;
+ goto errout;
+ }
+
+errout:
+ return ret;
+}
+
+/****************************************************************************
+ * Name: foc_velocity_pll_deinit_b16
+ *
+ * Description:
+ * De-initialize the PLL velocity observer (fixed16)
+ *
+ * Input Parameter:
+ * h - pointer to FOC velocity handler
+ *
+ ****************************************************************************/
+
+static void foc_velocity_pll_deinit_b16(FAR foc_velocity_b16_t *h)
+{
+ DEBUGASSERT(h);
+
+ if (h->data)
+ {
+ /* Free velocity data */
+
+ free(h->data);
+ }
+}
+
+/****************************************************************************
+ * Name: foc_velocity_pll_cfg_b16
+ *
+ * Description:
+ * Configure the PLL velocity observer (fixed16)
+ *
+ * Input Parameter:
+ * h - pointer to FOC velocity handler
+ * cfg - pointer to velocity handler configuration data
+ * (struct foc_pll_b16_s)
+ *
+ ****************************************************************************/
+
+static int foc_velocity_pll_cfg_b16(FAR foc_velocity_b16_t *h, FAR void *cfg)
+{
+ FAR struct foc_pll_b16_s *pll = NULL;
+ int ret = OK;
+
+ DEBUGASSERT(h);
+
+ /* Get pll data */
+
+ DEBUGASSERT(h->data);
+ pll = h->data;
+
+ /* Copy configuration */
+
+ memcpy(&pll->cfg, cfg, sizeof(struct foc_vel_pll_b16_cfg_s));
+
+ /* Configure observer */
+
+ motor_sobserver_pll_init_b16(&pll->data,
+ pll->cfg.kp,
+ pll->cfg.ki);
+
+ motor_sobserver_init_b16(&pll->o, &pll->data, pll->cfg.per);
+
+ /* Initialize with CW direction */
+
+ pll->sensor_dir = DIR_CW_B16;
+
+ return ret;
+}
+
+/****************************************************************************
+ * Name: foc_velocity_pll_zero_b16
+ *
+ * Description:
+ * Zero the DIV velocity observer (fixed16)
+ *
+ * Input Parameter:
+ * h - pointer to FOC velocity handler
+ *
+ ****************************************************************************/
+
+static int foc_velocity_pll_zero_b16(FAR foc_velocity_b16_t *h)
+{
+ FAR struct foc_pll_b16_s *pll = NULL;
+ int ret = OK;
+
+ DEBUGASSERT(h);
+
+ /* Get pll data */
+
+ DEBUGASSERT(h->data);
+ pll = h->data;
+
+ /* Reinitialize observer */
+
+ motor_sobserver_pll_init_b16(&pll->data,
+ pll->cfg.kp,
+ pll->cfg.ki);
+
+ return ret;
+}
+
+/****************************************************************************
+ * Name: foc_velocity_pll_dir_b16
+ *
+ * Description:
+ * Set the PLL velocity observer direction (fixed16)
+ *
+ * Input Parameter:
+ * h - pointer to FOC velocity handler
+ * dir - sensor direction (1 if normal -1 if inverted)
+ *
+ ****************************************************************************/
+
+static int foc_velocity_pll_dir_b16(FAR foc_velocity_b16_t *h, b16_t dir)
+{
+ FAR struct foc_pll_b16_s *pll = NULL;
+
+ DEBUGASSERT(h);
+
+ /* Get pll data */
+
+ DEBUGASSERT(h->data);
+ pll = h->data;
+
+ /* Set direction */
+
+ pll->sensor_dir = dir;
+
+ return OK;
+}
+
+/****************************************************************************
+ * Name: foc_velocity_pll_run_b16
+ *
+ * Description:
+ * Process the PLL velocity observer (fixed16)
+ *
+ * Input Parameter:
+ * h - pointer to FOC velocity handler
+ * in - pointer to FOC velocity handler input data
+ * out - pointer to FOC velocity handler output data
+ *
+ ****************************************************************************/
+
+static int foc_velocity_pll_run_b16(FAR foc_velocity_b16_t *h,
+ FAR struct foc_velocity_in_b16_s *in,
+ FAR struct foc_velocity_out_b16_s *out)
+{
+ FAR struct foc_pll_b16_s *pll = NULL;
+
+ DEBUGASSERT(h);
+
+ /* Get pll data */
+
+ DEBUGASSERT(h->data);
+ pll = h->data;
+
+ /* Run observer */
+
+ motor_sobserver_pll_b16(&pll->o, in->angle);
+
+ /* Copy data */
+
+ out->velocity = b16mulb16(pll->sensor_dir,
+ motor_sobserver_speed_get_b16(&pll->o));
+
+ return OK;
+}