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/04/10 05:39:18 UTC
[incubator-nuttx-apps] branch master updated: Add FOC library
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
The following commit(s) were added to refs/heads/master by this push:
new d75d6b7 Add FOC library
d75d6b7 is described below
commit d75d6b7af0d4d79dbf79980605ffeca0277ca38c
Author: raiden00pl <ra...@railab.me>
AuthorDate: Sat Mar 20 21:06:54 2021 +0100
Add FOC library
Supported features:
- float and fixed16 operations
- helper interface to interact with the kernel-side FOC device
- general interface for FOC controller
- PI current/voltage controller
- 3-phase space vector modulation
- general interface for FOC model
- PMSM model
- general interface for phase angle data
- open-loop handler
- general interface for phase velocity data
- general interface for ramp (eg. velocity ramp, torque ramp)
- (WIP) CORDIC interface for hardware acceleration
---
include/industry/foc/fixed16/foc_angle.h | 140 ++++++++++
include/industry/foc/fixed16/foc_cordic.h | 48 ++++
include/industry/foc/fixed16/foc_handler.h | 247 ++++++++++++++++
include/industry/foc/fixed16/foc_model.h | 160 +++++++++++
include/industry/foc/fixed16/foc_ramp.h | 68 +++++
include/industry/foc/fixed16/foc_velocity.h | 121 ++++++++
include/industry/foc/float/foc_angle.h | 140 ++++++++++
include/industry/foc/float/foc_cordic.h | 48 ++++
include/industry/foc/float/foc_handler.h | 248 +++++++++++++++++
include/industry/foc/float/foc_model.h | 160 +++++++++++
include/industry/foc/float/foc_ramp.h | 68 +++++
include/industry/foc/float/foc_velocity.h | 121 ++++++++
include/industry/foc/foc_common.h | 78 ++++++
include/industry/foc/foc_log.h | 42 +++
include/industry/foc/foc_utils.h | 92 ++++++
industry/foc/Kconfig | 69 +++++
industry/foc/Make.defs | 24 ++
industry/foc/Makefile | 85 ++++++
industry/foc/fixed16/foc_angle.c | 157 +++++++++++
industry/foc/fixed16/foc_cordic.c | 81 ++++++
industry/foc/fixed16/foc_handler.c | 345 +++++++++++++++++++++++
industry/foc/fixed16/foc_model.c | 188 +++++++++++++
industry/foc/fixed16/foc_model_pmsm.c | 300 ++++++++++++++++++++
industry/foc/fixed16/foc_openloop.c | 195 +++++++++++++
industry/foc/fixed16/foc_picontrol.c | 417 +++++++++++++++++++++++++++
industry/foc/fixed16/foc_ramp.c | 178 ++++++++++++
industry/foc/fixed16/foc_svm3.c | 287 +++++++++++++++++++
industry/foc/fixed16/foc_velocity.c | 157 +++++++++++
industry/foc/float/foc_angle.c | 157 +++++++++++
industry/foc/float/foc_cordic.c | 81 ++++++
industry/foc/float/foc_handler.c | 345 +++++++++++++++++++++++
industry/foc/float/foc_model.c | 188 +++++++++++++
industry/foc/float/foc_model_pmsm.c | 297 ++++++++++++++++++++
industry/foc/float/foc_openloop.c | 197 +++++++++++++
industry/foc/float/foc_picontrol.c | 418 ++++++++++++++++++++++++++++
industry/foc/float/foc_ramp.c | 178 ++++++++++++
industry/foc/float/foc_svm3.c | 287 +++++++++++++++++++
industry/foc/float/foc_velocity.c | 157 +++++++++++
industry/foc/foc_utils.c | 175 ++++++++++++
39 files changed, 6744 insertions(+)
diff --git a/include/industry/foc/fixed16/foc_angle.h b/include/industry/foc/fixed16/foc_angle.h
new file mode 100644
index 0000000..02abb0d
--- /dev/null
+++ b/include/industry/foc/fixed16/foc_angle.h
@@ -0,0 +1,140 @@
+/****************************************************************************
+ * apps/include/industry/foc/fixed16/foc_angle.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_ANGLE_H
+#define __INDUSTRY_FOC_FIXED16_FOC_ANGLE_H
+
+/****************************************************************************
+ * Included Files
+ ****************************************************************************/
+
+#include <nuttx/config.h>
+
+#include <dspb16.h>
+
+#include "industry/foc/fixed16/foc_handler.h"
+
+/****************************************************************************
+ * Public Type Definition
+ ****************************************************************************/
+
+/* Input to angle handler */
+
+struct foc_angle_in_b16_s
+{
+ FAR struct foc_state_b16_s *state; /* FOC state */
+ b16_t angle; /* Last angle */
+ b16_t vel; /* Last velocity */
+ b16_t dir; /* Movement direction */
+};
+
+/* Output from angle handler */
+
+struct foc_angle_out_b16_s
+{
+ b16_t angle;
+};
+
+/* Forward declaration */
+
+typedef struct foc_angle_b16_s foc_angle_b16_t;
+
+/* Angle operations */
+
+struct foc_angle_ops_b16_s
+{
+ /* Initialize */
+
+ CODE int (*init)(FAR foc_angle_b16_t *h);
+
+ /* Deinitialize */
+
+ CODE void (*deinit)(FAR foc_angle_b16_t *h);
+
+ /* Configure */
+
+ CODE int (*cfg)(FAR foc_angle_b16_t *h, FAR void *cfg);
+
+ /* Run */
+
+ CODE void (*run)(FAR foc_angle_b16_t *h,
+ FAR struct foc_angle_in_b16_s *in,
+ FAR struct foc_angle_out_b16_s *out);
+};
+
+/* Angle handler - sensor or sensorless */
+
+struct foc_angle_b16_s
+{
+ FAR struct foc_angle_ops_b16_s *ops;
+ FAR void *data;
+};
+
+#ifdef CONFIG_INDUSTRY_FOC_ANGLE_OPENLOOP
+/* Open-loop configuration data */
+
+struct foc_openloop_cfg_b16_s
+{
+ b16_t per; /* Controller period */
+};
+#endif
+
+/****************************************************************************
+ * Public Data
+ ****************************************************************************/
+
+#ifdef CONFIG_INDUSTRY_FOC_ANGLE_OPENLOOP
+/* Open-loop angle operations (fixed16) */
+
+extern struct foc_angle_ops_b16_s g_foc_angle_ol_b16;
+#endif
+
+/****************************************************************************
+ * Public Function Prototypes
+ ****************************************************************************/
+
+/****************************************************************************
+ * Name: foc_angle_init_b16
+ ****************************************************************************/
+
+int foc_angle_init_b16(FAR foc_angle_b16_t *h,
+ FAR struct foc_angle_ops_b16_s *ops);
+
+/****************************************************************************
+ * Name: foc_angle_deinit_b16
+ ****************************************************************************/
+
+int foc_angle_deinit_b16(FAR foc_angle_b16_t *h);
+
+/****************************************************************************
+ * Name: foc_angle_cfg_b16
+ ****************************************************************************/
+
+int foc_angle_cfg_b16(FAR foc_angle_b16_t *h, FAR void *cfg);
+
+/****************************************************************************
+ * Name: foc_angle_run_b16
+ ****************************************************************************/
+
+void foc_angle_run_b16(FAR foc_angle_b16_t *h,
+ FAR struct foc_angle_in_b16_s *in,
+ FAR struct foc_angle_out_b16_s *out);
+
+#endif /* __INDUSTRY_FOC_FIXED16_FOC_ANGLE_H */
diff --git a/include/industry/foc/fixed16/foc_cordic.h b/include/industry/foc/fixed16/foc_cordic.h
new file mode 100644
index 0000000..0790019
--- /dev/null
+++ b/include/industry/foc/fixed16/foc_cordic.h
@@ -0,0 +1,48 @@
+/****************************************************************************
+ * apps/include/industry/foc/fixed16/foc_cordic.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_CORDIC_H
+#define __INDUSTRY_FOC_FIXED16_FOC_CORDIC_H
+
+/****************************************************************************
+ * Included Files
+ ****************************************************************************/
+
+#include <nuttx/config.h>
+
+#include <dspb16.h>
+
+/****************************************************************************
+ * Public Function Prototypes
+ ****************************************************************************/
+
+/****************************************************************************
+ * Name: foc_cordic_dqsat_b16
+ ****************************************************************************/
+
+int foc_cordic_dqsat_b16(int fd, FAR dq_frame_b16_t *dq_ref, b16_t mag_max);
+
+/****************************************************************************
+ * Name: foc_cordic_angle_b16
+ ****************************************************************************/
+
+int foc_cordic_angle_b16(int fd, FAR phase_angle_b16_t *angle, b16_t a);
+
+#endif /* __INDUSTRY_FOC_FIXED16_FOC_CORDIC_H */
diff --git a/include/industry/foc/fixed16/foc_handler.h b/include/industry/foc/fixed16/foc_handler.h
new file mode 100644
index 0000000..3c445c0
--- /dev/null
+++ b/include/industry/foc/fixed16/foc_handler.h
@@ -0,0 +1,247 @@
+/****************************************************************************
+ * apps/include/industry/foc/fixed16/foc_handler.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_HANDLER_H
+#define __INDUSTRY_FOC_FIXED16_FOC_HANDLER_H
+
+/****************************************************************************
+ * Included Files
+ ****************************************************************************/
+
+#include <nuttx/config.h>
+
+#include <dspb16.h>
+
+#ifdef CONFIG_INDUSTRY_FOC_CORDIC
+# include "industry/foc/fixed16/foc_cordic.h"
+#endif
+
+/****************************************************************************
+ * Public Type Definition
+ ****************************************************************************/
+
+/* Input to FOC controller */
+
+struct foc_handler_input_b16_s
+{
+ FAR b16_t *current; /* Phase current samples */
+ FAR dq_frame_b16_t *dq_ref; /* DQ reference frame */
+ FAR dq_frame_b16_t *vdq_comp; /* DQ voltage compensation */
+ FAR b16_t angle; /* Phase angle */
+ FAR b16_t vbus; /* Bus voltage */
+ FAR int mode; /* Controller mode (enum foc_handler_mode_e) */
+};
+
+/* Output from FOC controller */
+
+struct foc_handler_output_b16_s
+{
+ b16_t duty[CONFIG_MOTOR_FOC_PHASES]; /* New duty cycle for PWM */
+};
+
+/* Controller state */
+
+struct foc_state_b16_s
+{
+ b16_t curr[CONFIG_MOTOR_FOC_PHASES];
+ b16_t volt[CONFIG_MOTOR_FOC_PHASES];
+ ab_frame_b16_t iab;
+ ab_frame_b16_t vab;
+ dq_frame_b16_t vdq;
+ dq_frame_b16_t idq;
+};
+
+/* Forward declaration */
+
+typedef struct foc_handler_b16_s foc_handler_b16_t;
+
+/* Modulation operations */
+
+struct foc_modulation_ops_b16_s
+{
+ /* Initialize */
+
+ CODE int (*init)(FAR foc_handler_b16_t *h);
+
+ /* Deinitialzie */
+
+ CODE void (*deinit)(FAR foc_handler_b16_t *h);
+
+ /* Configure modulation */
+
+ CODE void (*cfg)(FAR foc_handler_b16_t *h, FAR void *cfg);
+
+ /* Modulation specific current correction */
+
+ CODE void (*current)(FAR foc_handler_b16_t *h, FAR b16_t *curr);
+
+ /* Get the base voltage for a given modulation scheme */
+
+ CODE void (*vbase_get)(FAR foc_handler_b16_t *h,
+ b16_t vbus,
+ FAR b16_t *vbase);
+
+ /* Run modulation */
+
+ CODE void (*run)(FAR foc_handler_b16_t *h,
+ FAR ab_frame_b16_t *v_ab_mod,
+ FAR b16_t *duty);
+};
+
+/* Current/voltage controller operations */
+
+struct foc_control_ops_b16_s
+{
+ /* Initialize */
+
+ CODE int (*init)(FAR foc_handler_b16_t *h);
+
+ /* Deinitialize */
+
+ CODE void (*deinit)(FAR foc_handler_b16_t *h);
+
+ /* Configure controller */
+
+ CODE void (*cfg)(FAR foc_handler_b16_t *h, FAR void *cfg);
+
+ /* Feed controller with input data */
+
+ CODE void (*input_set)(FAR foc_handler_b16_t *h,
+ FAR b16_t *current,
+ b16_t vbase,
+ b16_t angle);
+
+ /* Run voltage controller */
+
+ CODE void (*voltage_run)(FAR foc_handler_b16_t *h,
+ FAR dq_frame_b16_t *dq_ref,
+ FAR ab_frame_b16_t *v_ab_mod);
+
+ /* Run current controller */
+
+ CODE void (*current_run)(FAR foc_handler_b16_t *h,
+ FAR dq_frame_b16_t *dq_ref,
+ FAR dq_frame_b16_t *vdq_comp,
+ FAR ab_frame_b16_t *v_ab_mod);
+
+ /* Get controller state data */
+
+ CODE void (*state_get)(FAR foc_handler_b16_t *h,
+ FAR struct foc_state_b16_s *state);
+};
+
+/* FOC handler operations */
+
+struct foc_handler_ops_b16_s
+{
+ /* Current/voltage controller interface */
+
+ struct foc_control_ops_b16_s *ctrl;
+
+ /* Modulation interface */
+
+ struct foc_modulation_ops_b16_s *mod;
+};
+
+/* FOC handler data */
+
+struct foc_handler_b16_s
+{
+#ifdef CONFIG_INDUSTRY_FOC_CORDIC
+ int fd; /* CORDIC device */
+#endif
+ struct foc_handler_ops_b16_s ops; /* Handler operations */
+ FAR void *modulation; /* Modulation data */
+ FAR void *control; /* Controller data */
+};
+
+/* Modulation configuration */
+
+struct foc_mod_cfg_b16_s
+{
+ b16_t pwm_duty_max; /* Maximum allowed PWM duty cycle */
+};
+
+/****************************************************************************
+ * Public Data
+ ****************************************************************************/
+
+#ifdef CONFIG_INDUSTRY_FOC_CONTROL_PI
+/* FOC PI controller (fixed16) */
+
+extern struct foc_control_ops_b16_s g_foc_control_pi_b16;
+#endif
+
+#ifdef CONFIG_INDUSTRY_FOC_MODULATION_SVM3
+/* 3-phase space vector modulation Modulation handler (fixed16) */
+
+extern struct foc_modulation_ops_b16_s g_foc_mod_svm3_b16;
+#endif
+
+/****************************************************************************
+ * Public Function Prototypes
+ ****************************************************************************/
+
+/****************************************************************************
+ * Name: foc_handler_init_b16
+ ****************************************************************************/
+
+int foc_handler_init_b16(FAR foc_handler_b16_t *h,
+ FAR struct foc_control_ops_b16_s *ctrl,
+ FAR struct foc_modulation_ops_b16_s *mod);
+
+/****************************************************************************
+ * Name: foc_handler_deinit_b16
+ ****************************************************************************/
+
+int foc_handler_deinit_b16(FAR foc_handler_b16_t *h);
+
+/****************************************************************************
+ * Name: foc_handler_run_b16
+ ****************************************************************************/
+
+int foc_handler_run_b16(FAR foc_handler_b16_t *h,
+ FAR struct foc_handler_input_b16_s *in,
+ FAR struct foc_handler_output_b16_s *out);
+
+/****************************************************************************
+ * Name: foc_handler_cfg_b16
+ ****************************************************************************/
+
+void foc_handler_cfg_b16(FAR foc_handler_b16_t *h,
+ FAR void *ctrl_cfg,
+ FAR void *mod_cfg);
+
+/****************************************************************************
+ * Name: foc_handler_state_b16
+ ****************************************************************************/
+
+void foc_handler_state_b16(FAR foc_handler_b16_t *h,
+ FAR struct foc_state_b16_s *state);
+
+#ifdef CONFIG_INDUSTRY_FOC_HANDLER_PRINT
+/****************************************************************************
+ * Name: foc_handler_state_print_b16
+ ****************************************************************************/
+
+void foc_handler_state_print_b16(FAR struct foc_state_b16_s *state);
+#endif
+
+#endif /* __INDUSTRY_FOC_FIXED16_FOC_HANDLER_H */
diff --git a/include/industry/foc/fixed16/foc_model.h b/include/industry/foc/fixed16/foc_model.h
new file mode 100644
index 0000000..ce0c8c8
--- /dev/null
+++ b/include/industry/foc/fixed16/foc_model.h
@@ -0,0 +1,160 @@
+/****************************************************************************
+ * apps/include/industry/foc/fixed16/foc_model.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_MODEL_H
+#define __INDUSTRY_FOC_FIXED16_FOC_MODEL_H
+
+/****************************************************************************
+ * Included Files
+ ****************************************************************************/
+
+#include <nuttx/config.h>
+
+#include <dspb16.h>
+
+/****************************************************************************
+ * Public Type Definition
+ ****************************************************************************/
+
+#ifdef CONFIG_INDUSTRY_FOC_MODEL_PMSM
+/* PMSM model configuration */
+
+struct foc_model_pmsm_cfg_b16_s
+{
+ uint8_t poles;
+ b16_t res;
+ b16_t ind;
+ b16_t iner;
+ b16_t flux_link;
+ b16_t ind_d;
+ b16_t ind_q;
+ b16_t per;
+ b16_t iphase_adc;
+};
+#endif /* CONFIG_INDUSTRY_FOC_MODEL_PMSM */
+
+/* FOC model state */
+
+struct foc_model_state_b16_s
+{
+ int32_t curr_raw[CONFIG_MOTOR_FOC_PHASES];
+ b16_t curr[CONFIG_MOTOR_FOC_PHASES];
+ b16_t volt[CONFIG_MOTOR_FOC_PHASES];
+ ab_frame_b16_t iab;
+ ab_frame_b16_t vab;
+ dq_frame_b16_t vdq;
+ dq_frame_b16_t idq;
+ b16_t omega_e;
+ b16_t omega_m;
+};
+
+/* Forward declaration */
+
+typedef struct foc_model_b16_s foc_model_b16_t;
+
+/* FOC model operations */
+
+struct foc_model_ops_b16_s
+{
+ /* Initialize */
+
+ CODE int (*init)(FAR foc_model_b16_t *h);
+
+ /* Deinitialize */
+
+ CODE void (*deinit)(FAR foc_model_b16_t *h);
+
+ /* Configure model */
+
+ CODE int (*cfg)(FAR foc_model_b16_t *h, FAR void *cfg);
+
+ /* Run electrical model */
+
+ CODE void (*ele_run)(FAR foc_model_b16_t *h,
+ FAR ab_frame_b16_t *v_ab);
+
+ /* Run mechanical model */
+
+ CODE void (*mech_run)(FAR foc_model_b16_t *h,
+ b16_t load);
+
+ /* Get model state */
+
+ CODE void (*state)(FAR foc_model_b16_t *h,
+ FAR struct foc_model_state_b16_s *state);
+};
+
+/* FOC model handler */
+
+struct foc_model_b16_s
+{
+ FAR struct foc_model_ops_b16_s *ops;
+ FAR void *model;
+};
+
+/****************************************************************************
+ * Public Data
+ ****************************************************************************/
+
+#ifdef CONFIG_INDUSTRY_FOC_MODEL_PMSM
+/* PMSM model operations (fixed16) */
+
+extern struct foc_model_ops_b16_s g_foc_model_pmsm_ops_b16;
+#endif
+
+/****************************************************************************
+ * Public Function Prototypes
+ ****************************************************************************/
+
+/****************************************************************************
+ * Name: foc_model_init_b16
+ ****************************************************************************/
+
+int foc_model_init_b16(FAR foc_model_b16_t *h,
+ FAR struct foc_model_ops_b16_s *ops);
+
+/****************************************************************************
+ * Name: foc_model_deinit_b16
+ ****************************************************************************/
+
+int foc_model_deinit_b16(FAR foc_model_b16_t *h);
+
+/****************************************************************************
+ * Name: foc_model_cfg_b16
+ ****************************************************************************/
+
+int foc_model_cfg_b16(FAR foc_model_b16_t *h, FAR void *cfg);
+
+/****************************************************************************
+ * Name: foc_model_run_b16
+ ****************************************************************************/
+
+void foc_model_run_b16(FAR foc_model_b16_t *h,
+ b16_t load,
+ FAR ab_frame_b16_t *v_ab);
+
+/****************************************************************************
+ * Name: foc_model_state_b16
+ ****************************************************************************/
+
+void foc_model_state_b16(FAR foc_model_b16_t *h,
+ FAR struct foc_model_state_b16_s *state);
+
+#endif /* __INDUSTRY_FOC_FIXED16_FOC_MODEL_H */
diff --git a/include/industry/foc/fixed16/foc_ramp.h b/include/industry/foc/fixed16/foc_ramp.h
new file mode 100644
index 0000000..1c2a154
--- /dev/null
+++ b/include/industry/foc/fixed16/foc_ramp.h
@@ -0,0 +1,68 @@
+/****************************************************************************
+ * apps/include/industry/foc/fixed16/foc_ramp.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_RAMP_H
+#define __INDUSTRY_FOC_FIXED16_FOC_RAMP_H
+
+/****************************************************************************
+ * Included Files
+ ****************************************************************************/
+
+#include <nuttx/config.h>
+
+#include <dspb16.h>
+
+/****************************************************************************
+ * Public Type Definition
+ ****************************************************************************/
+
+/* Ramp data */
+
+struct foc_ramp_b16_s
+{
+ uint8_t ramp_mode; /* Ramp mode */
+ b16_t per; /* Controller period */
+ b16_t ramp_thr; /* Ramp threshold */
+ b16_t ramp_acc; /* Ramp acceleration */
+ b16_t ramp_dec; /* Ramp deceleration */
+ b16_t ramp_dec_per; /* dec * per */
+ b16_t ramp_acc_per; /* acc * per */
+ b16_t diff; /* Difference now */
+};
+
+/****************************************************************************
+ * Public Function Prototypes
+ ****************************************************************************/
+
+/****************************************************************************
+ * Name: foc_ramp_init_b16
+ ****************************************************************************/
+
+int foc_ramp_init_b16(FAR struct foc_ramp_b16_s *ramp, b16_t per,
+ b16_t thr, b16_t acc, b16_t dec);
+
+/****************************************************************************
+ * Name: foc_ramp_run_b16
+ ****************************************************************************/
+
+int foc_ramp_run_b16(FAR struct foc_ramp_b16_s *ramp, b16_t des,
+ b16_t now, FAR b16_t *set);
+
+#endif /* __INDUSTRY_FOC_FIXED16_FOC_RAMP_H */
diff --git a/include/industry/foc/fixed16/foc_velocity.h b/include/industry/foc/fixed16/foc_velocity.h
new file mode 100644
index 0000000..311cc23
--- /dev/null
+++ b/include/industry/foc/fixed16/foc_velocity.h
@@ -0,0 +1,121 @@
+/****************************************************************************
+ * apps/include/industry/foc/fixed16/foc_velocity.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_VELOCITY_H
+#define __INDUSTRY_FOC_FIXED16_FOC_VELOCITY_H
+
+/****************************************************************************
+ * Included Files
+ ****************************************************************************/
+
+#include <nuttx/config.h>
+
+#include <dspb16.h>
+
+#include "industry/foc/fixed16/foc_handler.h"
+
+/****************************************************************************
+ * Public Type Definition
+ ****************************************************************************/
+
+/* Input to velocity handler */
+
+struct foc_velocity_in_b16_s
+{
+ FAR struct foc_state_b16_s *state; /* FOC state */
+ b16_t angle; /* Last angle */
+ b16_t vel; /* Last velocity */
+ b16_t dir; /* Movement direction */
+};
+
+/* Output from velocity handler */
+
+struct foc_velocity_out_b16_s
+{
+ b16_t velocity;
+};
+
+/* Forward declaration */
+
+typedef struct foc_velocity_b16_s foc_velocity_b16_t;
+
+/* Velocity operations */
+
+struct foc_velocity_ops_b16_s
+{
+ /* Initialize */
+
+ CODE int (*init)(FAR foc_velocity_b16_t *h);
+
+ /* Deinitialize */
+
+ CODE void (*deinit)(FAR foc_velocity_b16_t *h);
+
+ /* Configure */
+
+ CODE int (*cfg)(FAR foc_velocity_b16_t *h, FAR void *cfg);
+
+ /* Run */
+
+ CODE void (*run)(FAR foc_velocity_b16_t *h,
+ FAR struct foc_velocity_in_b16_s *in,
+ FAR struct foc_velocity_out_b16_s *out);
+};
+
+/* Velocity handler - sensor or sensorless */
+
+struct foc_velocity_b16_s
+{
+ FAR struct foc_velocity_ops_b16_s *ops;
+ FAR void *data;
+};
+
+/****************************************************************************
+ * Public Function Prototypes
+ ****************************************************************************/
+
+/****************************************************************************
+ * Name: foc_velocity_init_b16
+ ****************************************************************************/
+
+int foc_velocity_init_b16(FAR foc_velocity_b16_t *h,
+ FAR struct foc_velocity_ops_b16_s *ops);
+
+/****************************************************************************
+ * Name: foc_velocity_deinit_b16
+ ****************************************************************************/
+
+int foc_velocity_deinit_b16(FAR foc_velocity_b16_t *h);
+
+/****************************************************************************
+ * Name: foc_velocity_cfg_b16
+ ****************************************************************************/
+
+int foc_velocity_cfg_b16(FAR foc_velocity_b16_t *h, FAR void *cfg);
+
+/****************************************************************************
+ * Name: foc_velocity_run_b16
+ ****************************************************************************/
+
+void foc_velocity_run_b16(FAR foc_velocity_b16_t *h,
+ FAR struct foc_velocity_in_b16_s *in,
+ FAR struct foc_velocity_out_b16_s *out);
+
+#endif /* __INDUSTRY_FOC_FIXED16_FOC_VELOCITY_H */
diff --git a/include/industry/foc/float/foc_angle.h b/include/industry/foc/float/foc_angle.h
new file mode 100644
index 0000000..2076dfa
--- /dev/null
+++ b/include/industry/foc/float/foc_angle.h
@@ -0,0 +1,140 @@
+/****************************************************************************
+ * apps/include/industry/foc/float/foc_angle.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_ANGLE_H
+#define __INDUSTRY_FOC_FLOAT_FOC_ANGLE_H
+
+/****************************************************************************
+ * Included Files
+ ****************************************************************************/
+
+#include <nuttx/config.h>
+
+#include <dsp.h>
+
+#include "industry/foc/float/foc_handler.h"
+
+/****************************************************************************
+ * Public Type Definition
+ ****************************************************************************/
+
+/* Input to angle handler */
+
+struct foc_angle_in_f32_s
+{
+ FAR struct foc_state_f32_s *state; /* FOC state */
+ float angle; /* Last angle */
+ float vel; /* Last velocity */
+ float dir; /* Movement direction */
+};
+
+/* Output from angle handler */
+
+struct foc_angle_out_f32_s
+{
+ float angle;
+};
+
+/* Forward declaration */
+
+typedef struct foc_angle_f32_s foc_angle_f32_t;
+
+/* Angle operations */
+
+struct foc_angle_ops_f32_s
+{
+ /* Initialize */
+
+ CODE int (*init)(FAR foc_angle_f32_t *h);
+
+ /* Deinitialize */
+
+ CODE void (*deinit)(FAR foc_angle_f32_t *h);
+
+ /* Configure */
+
+ CODE int (*cfg)(FAR foc_angle_f32_t *h, FAR void *cfg);
+
+ /* Run angle handler */
+
+ CODE void (*run)(FAR foc_angle_f32_t *h,
+ FAR struct foc_angle_in_f32_s *in,
+ FAR struct foc_angle_out_f32_s *out);
+};
+
+/* Angle handler - sensor or sensorless */
+
+struct foc_angle_f32_s
+{
+ FAR struct foc_angle_ops_f32_s *ops;
+ FAR void *data;
+};
+
+#ifdef CONFIG_INDUSTRY_FOC_ANGLE_OPENLOOP
+/* Open-loop configuration data */
+
+struct foc_openloop_cfg_f32_s
+{
+ float per; /* Controller period */
+};
+#endif /* CONFIG_INDUSTRY_FOC_ANGLE_OPENLOOP */
+
+/****************************************************************************
+ * Public Data
+ ****************************************************************************/
+
+#ifdef CONFIG_INDUSTRY_FOC_ANGLE_OPENLOOP
+/* Open-loop angle operations (float) */
+
+extern struct foc_angle_ops_f32_s g_foc_angle_ol_f32;
+#endif
+
+/****************************************************************************
+ * Public Function Prototypes
+ ****************************************************************************/
+
+/****************************************************************************
+ * Name: foc_angle_init_f32
+ ****************************************************************************/
+
+int foc_angle_init_f32(FAR foc_angle_f32_t *h,
+ FAR struct foc_angle_ops_f32_s *ops);
+
+/****************************************************************************
+ * Name: foc_angle_deinit_f32
+ ****************************************************************************/
+
+int foc_angle_deinit_f32(FAR foc_angle_f32_t *h);
+
+/****************************************************************************
+ * Name: foc_angle_cfg_f32
+ ****************************************************************************/
+
+int foc_angle_cfg_f32(FAR foc_angle_f32_t *h, FAR void *cfg);
+
+/****************************************************************************
+ * Name: foc_angle_run_f32
+ ****************************************************************************/
+
+void foc_angle_run_f32(FAR foc_angle_f32_t *h,
+ FAR struct foc_angle_in_f32_s *in,
+ FAR struct foc_angle_out_f32_s *out);
+
+#endif /* __INDUSTRY_FOC_FLOAT_FOC_ANGLE_H */
diff --git a/include/industry/foc/float/foc_cordic.h b/include/industry/foc/float/foc_cordic.h
new file mode 100644
index 0000000..28562bf
--- /dev/null
+++ b/include/industry/foc/float/foc_cordic.h
@@ -0,0 +1,48 @@
+/****************************************************************************
+ * apps/include/industry/foc/float/foc_cordic.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_CORDIC_H
+#define __INDUSTRY_FOC_FLOAT_FOC_CORDIC_H
+
+/****************************************************************************
+ * Included Files
+ ****************************************************************************/
+
+#include <nuttx/config.h>
+
+#include <dsp.h>
+
+/****************************************************************************
+ * Public Function Prototypes
+ ****************************************************************************/
+
+/****************************************************************************
+ * Name: foc_cordic_dqsat_f32
+ ****************************************************************************/
+
+int foc_cordic_dqsat_f32(int fd, FAR dq_frame_f32_t *dq_ref, float mag_max);
+
+/****************************************************************************
+ * Name: foc_cordic_angle_f32
+ ****************************************************************************/
+
+int foc_cordic_angle_f32(int fd, FAR phase_angle_f32_t *angle, float a);
+
+#endif /* __INDUSTRY_FOC_FLOAT_FOC_CORDIC_H */
diff --git a/include/industry/foc/float/foc_handler.h b/include/industry/foc/float/foc_handler.h
new file mode 100644
index 0000000..b3941fe
--- /dev/null
+++ b/include/industry/foc/float/foc_handler.h
@@ -0,0 +1,248 @@
+/****************************************************************************
+ * apps/include/industry/foc/float/foc_handler.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_HANDLER_H
+#define __INDUSTRY_FOC_FLOAT_FOC_HANDLER_H
+
+/****************************************************************************
+ * Included Files
+ ****************************************************************************/
+
+#include <nuttx/config.h>
+
+#include <dsp.h>
+
+#ifdef CONFIG_INDUSTRY_FOC_CORDIC
+# include "industry/foc/float/foc_cordic.h"
+#endif
+
+/****************************************************************************
+ * Public Type Definition
+ ****************************************************************************/
+
+/* Input to FOC controller */
+
+struct foc_handler_input_f32_s
+{
+ FAR float *current; /* Phase current samples */
+ FAR dq_frame_f32_t *dq_ref; /* DQ reference frame */
+ FAR dq_frame_f32_t *vdq_comp; /* DQ voltage compensation */
+ FAR float angle; /* Phase angle */
+ FAR float vbus; /* Bus voltage */
+ FAR int mode; /* Controller mode (enum foc_handler_mode_e) */
+};
+
+/* Output from FOC controller */
+
+struct foc_handler_output_f32_s
+{
+ float duty[CONFIG_MOTOR_FOC_PHASES]; /* New duty cycle for PWM */
+};
+
+/* Controller state */
+
+struct foc_state_f32_s
+{
+ float curr[CONFIG_MOTOR_FOC_PHASES];
+ float volt[CONFIG_MOTOR_FOC_PHASES];
+ ab_frame_f32_t iab;
+ ab_frame_f32_t vab;
+ dq_frame_f32_t vdq;
+ dq_frame_f32_t idq;
+};
+
+/* Forward declaration */
+
+typedef struct foc_handler_f32_s foc_handler_f32_t;
+
+/* Modulation operations */
+
+struct foc_modulation_ops_f32_s
+{
+ /* Initialize */
+
+ CODE int (*init)(FAR foc_handler_f32_t *h);
+
+ /* Deinitialzie */
+
+ CODE void (*deinit)(FAR foc_handler_f32_t *h);
+
+ /* Configure modulation */
+
+ CODE void (*cfg)(FAR foc_handler_f32_t *h, FAR void *cfg);
+
+ /* Modulation specific current correction */
+
+ CODE void (*current)(FAR foc_handler_f32_t *h, FAR float *curr);
+
+ /* Get the base voltage for a given modulation scheme */
+
+ CODE void (*vbase_get)(FAR foc_handler_f32_t *h,
+ float vbus,
+ FAR float *vbase);
+
+ /* Run modulation */
+
+ CODE void (*run)(FAR foc_handler_f32_t *h,
+ FAR ab_frame_f32_t *v_ab_mod,
+ FAR float *duty);
+};
+
+/* Current/voltage controller operations */
+
+struct foc_control_ops_f32_s
+{
+ /* Initialize */
+
+ CODE int (*init)(FAR foc_handler_f32_t *h);
+
+ /* Deinitialize */
+
+ CODE void (*deinit)(FAR foc_handler_f32_t *h);
+
+ /* Configure controller */
+
+ CODE void (*cfg)(FAR foc_handler_f32_t *h,
+ FAR void *cfg);
+
+ /* Feed controller with input data */
+
+ CODE void (*input_set)(FAR foc_handler_f32_t *h,
+ FAR float *current,
+ float vbase,
+ float angle);
+
+ /* Run voltage controller */
+
+ CODE void (*voltage_run)(FAR foc_handler_f32_t *h,
+ FAR dq_frame_f32_t *dq_ref,
+ FAR ab_frame_f32_t *v_ab_mod);
+
+ /* Run current controller */
+
+ CODE void (*current_run)(FAR foc_handler_f32_t *h,
+ FAR dq_frame_f32_t *dq_ref,
+ FAR dq_frame_f32_t *vdq_comp,
+ FAR ab_frame_f32_t *v_ab_mod);
+
+ /* Get controller state data */
+
+ CODE void (*state_get)(FAR foc_handler_f32_t *h,
+ FAR struct foc_state_f32_s *state);
+};
+
+/* FOC handler operations */
+
+struct foc_handler_ops_f32_s
+{
+ /* Current/voltage controller interface */
+
+ struct foc_control_ops_f32_s *ctrl;
+
+ /* Modulation interface */
+
+ struct foc_modulation_ops_f32_s *mod;
+};
+
+/* FOC handler data */
+
+struct foc_handler_f32_s
+{
+#ifdef CONFIG_INDUSTRY_FOC_CORDIC
+ int fd; /* CORDIC device */
+#endif
+ struct foc_handler_ops_f32_s ops; /* Handler operations */
+ FAR void *modulation; /* Modulation data */
+ FAR void *control; /* Controller data */
+};
+
+/* Modulation configuration */
+
+struct foc_mod_cfg_f32_s
+{
+ float pwm_duty_max; /* Maximum allowed PWM duty cycle */
+};
+
+/****************************************************************************
+ * Public Data
+ ****************************************************************************/
+
+#ifdef CONFIG_INDUSTRY_FOC_CONTROL_PI
+/* FOC PI controller (float) */
+
+extern struct foc_control_ops_f32_s g_foc_control_pi_f32;
+#endif
+
+#ifdef CONFIG_INDUSTRY_FOC_MODULATION_SVM3
+/* 3-phase space vector modulation Modulation handler (float) */
+
+extern struct foc_modulation_ops_f32_s g_foc_mod_svm3_f32;
+#endif
+
+/****************************************************************************
+ * Public Function Prototypes
+ ****************************************************************************/
+
+/****************************************************************************
+ * Name: foc_handler_init_f32
+ ****************************************************************************/
+
+int foc_handler_init_f32(FAR foc_handler_f32_t *h,
+ FAR struct foc_control_ops_f32_s *ctrl,
+ FAR struct foc_modulation_ops_f32_s *mod);
+
+/****************************************************************************
+ * Name: foc_handler_deinit_f32
+ ****************************************************************************/
+
+int foc_handler_deinit_f32(FAR foc_handler_f32_t *h);
+
+/****************************************************************************
+ * Name: foc_handler_run_f32
+ ****************************************************************************/
+
+int foc_handler_run_f32(FAR foc_handler_f32_t *h,
+ FAR struct foc_handler_input_f32_s *in,
+ FAR struct foc_handler_output_f32_s *out);
+
+/****************************************************************************
+ * Name: foc_handler_cfg_f32
+ ****************************************************************************/
+
+void foc_handler_cfg_f32(FAR foc_handler_f32_t *h,
+ FAR void *ctrl_cfg,
+ FAR void *mod_cfg);
+
+/****************************************************************************
+ * Name: foc_handler_state_f32
+ ****************************************************************************/
+
+void foc_handler_state_f32(FAR foc_handler_f32_t *h,
+ FAR struct foc_state_f32_s *state);
+
+#ifdef CONFIG_INDUSTRY_FOC_HANDLER_PRINT
+/****************************************************************************
+ * Name: foc_handler_state_print_f32
+ ****************************************************************************/
+
+void foc_handler_state_print_f32(FAR struct foc_state_f32_s *state);
+#endif
+
+#endif /* __INDUSTRY_FOC_FLOAT_FOC_HANDLER_H */
diff --git a/include/industry/foc/float/foc_model.h b/include/industry/foc/float/foc_model.h
new file mode 100644
index 0000000..da26eec
--- /dev/null
+++ b/include/industry/foc/float/foc_model.h
@@ -0,0 +1,160 @@
+/****************************************************************************
+ * apps/include/industry/foc/float/foc_model.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_MODEL_H
+#define __INDUSTRY_FOC_FLOAT_FOC_MODEL_H
+
+/****************************************************************************
+ * Included Files
+ ****************************************************************************/
+
+#include <nuttx/config.h>
+
+#include <dsp.h>
+
+/****************************************************************************
+ * Public Type Definition
+ ****************************************************************************/
+
+#ifdef CONFIG_INDUSTRY_FOC_MODEL_PMSM
+/* PMSM model configuration */
+
+struct foc_model_pmsm_cfg_f32_s
+{
+ uint8_t poles;
+ float res;
+ float ind;
+ float iner;
+ float flux_link;
+ float ind_d;
+ float ind_q;
+ float per;
+ float iphase_adc;
+};
+#endif /* CONFIG_INDUSTRY_FOC_MODEL_PMSM */
+
+/* FOC model state */
+
+struct foc_model_state_f32_s
+{
+ int32_t curr_raw[CONFIG_MOTOR_FOC_PHASES];
+ float curr[CONFIG_MOTOR_FOC_PHASES];
+ float volt[CONFIG_MOTOR_FOC_PHASES];
+ ab_frame_f32_t iab;
+ ab_frame_f32_t vab;
+ dq_frame_f32_t vdq;
+ dq_frame_f32_t idq;
+ float omega_e;
+ float omega_m;
+};
+
+/* Forward declaration */
+
+typedef struct foc_model_f32_s foc_model_f32_t;
+
+/* FOC model operations */
+
+struct foc_model_ops_f32_s
+{
+ /* Initialize */
+
+ CODE int (*init)(FAR foc_model_f32_t *h);
+
+ /* Deinitialize */
+
+ CODE void (*deinit)(FAR foc_model_f32_t *h);
+
+ /* Configure model */
+
+ CODE int (*cfg)(FAR foc_model_f32_t *h, FAR void *cfg);
+
+ /* Run electrical model */
+
+ CODE void (*ele_run)(FAR foc_model_f32_t *h,
+ FAR ab_frame_f32_t *v_ab);
+
+ /* Run mechanical model */
+
+ CODE void (*mech_run)(FAR foc_model_f32_t *h,
+ float load);
+
+ /* Get model state */
+
+ CODE void (*state)(FAR foc_model_f32_t *h,
+ FAR struct foc_model_state_f32_s *state);
+};
+
+/* FOC model handler */
+
+struct foc_model_f32_s
+{
+ FAR struct foc_model_ops_f32_s *ops;
+ FAR void *model;
+};
+
+/****************************************************************************
+ * Public Data
+ ****************************************************************************/
+
+#ifdef CONFIG_INDUSTRY_FOC_MODEL_PMSM
+/* PMSM model operations (float) */
+
+extern struct foc_model_ops_f32_s g_foc_model_pmsm_ops_f32;
+#endif
+
+/****************************************************************************
+ * Public Function Prototypes
+ ****************************************************************************/
+
+/****************************************************************************
+ * Name: foc_model_init_f32
+ ****************************************************************************/
+
+int foc_model_init_f32(FAR foc_model_f32_t *h,
+ FAR struct foc_model_ops_f32_s *ops);
+
+/****************************************************************************
+ * Name: foc_model_deinit_f32
+ ****************************************************************************/
+
+int foc_model_deinit_f32(FAR foc_model_f32_t *h);
+
+/****************************************************************************
+ * Name: foc_model_cfg_f32
+ ****************************************************************************/
+
+int foc_model_cfg_f32(FAR foc_model_f32_t *h, FAR void *cfg);
+
+/****************************************************************************
+ * Name: foc_model_run_f32
+ ****************************************************************************/
+
+void foc_model_run_f32(FAR foc_model_f32_t *h,
+ float load,
+ FAR ab_frame_f32_t *v_ab);
+
+/****************************************************************************
+ * Name: foc_model_state_f32
+ ****************************************************************************/
+
+void foc_model_state_f32(FAR foc_model_f32_t *h,
+ FAR struct foc_model_state_f32_s *state);
+
+#endif /* __INDUSTRY_FOC_FLOAT_FOC_MODEL_H */
diff --git a/include/industry/foc/float/foc_ramp.h b/include/industry/foc/float/foc_ramp.h
new file mode 100644
index 0000000..ea75700
--- /dev/null
+++ b/include/industry/foc/float/foc_ramp.h
@@ -0,0 +1,68 @@
+/****************************************************************************
+ * apps/include/industry/foc/float/foc_ramp.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_RAMP_H
+#define __INDUSTRY_FOC_FLOAT_FOC_RAMP_H
+
+/****************************************************************************
+ * Included Files
+ ****************************************************************************/
+
+#include <nuttx/config.h>
+
+#include <dsp.h>
+
+/****************************************************************************
+ * Public Type Definition
+ ****************************************************************************/
+
+/* Ramp data */
+
+struct foc_ramp_f32_s
+{
+ uint8_t ramp_mode; /* Ramp mode */
+ float per; /* Controller period */
+ float ramp_thr; /* Ramp threshold */
+ float ramp_acc; /* Ramp acceleration */
+ float ramp_dec; /* Ramp deceleration */
+ float ramp_dec_per; /* dec * per */
+ float ramp_acc_per; /* acc * per */
+ float diff; /* Difference now */
+};
+
+/****************************************************************************
+ * Public Function Prototypes
+ ****************************************************************************/
+
+/****************************************************************************
+ * Name: foc_ramp_init_f32
+ ****************************************************************************/
+
+int foc_ramp_init_f32(FAR struct foc_ramp_f32_s *ramp, float per,
+ float thr, float acc, float dec);
+
+/****************************************************************************
+ * Name: foc_ramp_run_f32
+ ****************************************************************************/
+
+int foc_ramp_run_f32(FAR struct foc_ramp_f32_s *ramp, float des,
+ float now, FAR float *set);
+
+#endif /* __INDUSTRY_FOC_FLOAT_FOC_RAMP_H */
diff --git a/include/industry/foc/float/foc_velocity.h b/include/industry/foc/float/foc_velocity.h
new file mode 100644
index 0000000..2ccaaac
--- /dev/null
+++ b/include/industry/foc/float/foc_velocity.h
@@ -0,0 +1,121 @@
+/****************************************************************************
+ * apps/include/industry/foc/float/foc_velocity.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_VELOCITY_H
+#define __INDUSTRY_FOC_FLOAT_FOC_VELOCITY_H
+
+/****************************************************************************
+ * Included Files
+ ****************************************************************************/
+
+#include <nuttx/config.h>
+
+#include <dsp.h>
+
+#include "industry/foc/float/foc_handler.h"
+
+/****************************************************************************
+ * Public Type Definition
+ ****************************************************************************/
+
+/* Input to velocity handler */
+
+struct foc_velocity_in_f32_s
+{
+ FAR struct foc_state_f32_s *state; /* FOC state */
+ float angle; /* Last angle */
+ float vel; /* Last velocity */
+ float dir; /* Movement direction */
+};
+
+/* Output from velocity handler */
+
+struct foc_velocity_out_f32_s
+{
+ float velocity;
+};
+
+/* Forward declaration */
+
+typedef struct foc_velocity_f32_s foc_velocity_f32_t;
+
+/* Velocity operations */
+
+struct foc_velocity_ops_f32_s
+{
+ /* Initialize */
+
+ CODE int (*init)(FAR foc_velocity_f32_t *h);
+
+ /* Deinitialize */
+
+ CODE void (*deinit)(FAR foc_velocity_f32_t *h);
+
+ /* Configure */
+
+ CODE int (*cfg)(FAR foc_velocity_f32_t *h, FAR void *cfg);
+
+ /* Run velocity handler */
+
+ CODE void (*run)(FAR foc_velocity_f32_t *h,
+ FAR struct foc_velocity_in_f32_s *in,
+ FAR struct foc_velocity_out_f32_s *out);
+};
+
+/* Velocity handler - sensor or sensorless */
+
+struct foc_velocity_f32_s
+{
+ FAR struct foc_velocity_ops_f32_s *ops;
+ FAR void *data;
+};
+
+/****************************************************************************
+ * Public Function Prototypes
+ ****************************************************************************/
+
+/****************************************************************************
+ * Name: foc_velocity_init_f32
+ ****************************************************************************/
+
+int foc_velocity_init_f32(FAR foc_velocity_f32_t *h,
+ FAR struct foc_velocity_ops_f32_s *ops);
+
+/****************************************************************************
+ * Name: foc_velocity_deinit_f32
+ ****************************************************************************/
+
+int foc_velocity_deinit_f32(FAR foc_velocity_f32_t *h);
+
+/****************************************************************************
+ * Name: foc_velocity_cfg_f32
+ ****************************************************************************/
+
+int foc_velocity_cfg_f32(FAR foc_velocity_f32_t *h, FAR void *cfg);
+
+/****************************************************************************
+ * Name: foc_velocity_run_f32
+ ****************************************************************************/
+
+void foc_velocity_run_f32(FAR foc_velocity_f32_t *h,
+ FAR struct foc_velocity_in_f32_s *in,
+ FAR struct foc_velocity_out_f32_s *out);
+
+#endif /* __INDUSTRY_FOC_FLOAT_FOC_VELOCITY_H */
diff --git a/include/industry/foc/foc_common.h b/include/industry/foc/foc_common.h
new file mode 100644
index 0000000..d474e9c
--- /dev/null
+++ b/include/industry/foc/foc_common.h
@@ -0,0 +1,78 @@
+/****************************************************************************
+ * apps/include/industry/foc/foc_common.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 __APPS_INCLUDE_INDUSTRY_FOC_FOC_COMMON_H
+#define __APPS_INCLUDE_INDUSTRY_FOC_FOC_COMMON_H
+
+/****************************************************************************
+ * Included Files
+ ****************************************************************************/
+
+#include <nuttx/config.h>
+
+#include <stdint.h>
+
+/****************************************************************************
+ * Pre-processor Definitions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Public Types
+ ****************************************************************************/
+
+/* FOC controller mode */
+
+enum foc_handler_mode_e
+{
+ FOC_HANDLER_MODE_INIT = 0, /* Initial state */
+ FOC_HANDLER_MODE_IDLE, /* Idle */
+ FOC_HANDLER_MODE_VOLTAGE, /* Voltage mode - control DQ voltage */
+ FOC_HANDLER_MODE_CURRENT /* Current mode - control DQ current */
+};
+
+/* FOC number type identifiers */
+
+enum foc_number_type_e
+{
+ FOC_NUMBER_TYPE_INVALID = 0,
+#ifdef CONFIG_INDUSTRY_FOC_FLOAT
+ FOC_NUMBER_TYPE_FLOAT = 1, /* float */
+#endif
+#ifdef CONFIG_INDUSTRY_FOC_FIXED16
+ FOC_NUMBER_TYPE_FIXED16 = 2, /* b16_t */
+#endif
+};
+
+/* Speed ramp mode */
+
+enum foc_ramp_mode_e
+{
+ RAMP_MODE_INVALID = 0, /* Reserved */
+ RAMP_MODE_SOFTSTART = 1, /* Soft start */
+ RAMP_MODE_SOFTSTOP = 2, /* Soft stop */
+ RAMP_MODE_NORMAL = 3, /* Normal operation */
+};
+
+/****************************************************************************
+ * Public Function Prototypes
+ ****************************************************************************/
+
+#endif /* __APPS_INCLUDE_INDUSTRY_FOC_FOC_COMMON_H */
diff --git a/include/industry/foc/foc_log.h b/include/industry/foc/foc_log.h
new file mode 100644
index 0000000..0634d49
--- /dev/null
+++ b/include/industry/foc/foc_log.h
@@ -0,0 +1,42 @@
+/****************************************************************************
+ * apps/include/industry/foc/foc_log.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 __APPS_INCLUDE_INDUSTRY_FOC_FOC_LOG_H
+#define __APPS_INCLUDE_INDUSTRY_FOC_FOC_LOG_H
+
+/****************************************************************************
+ * Included Files
+ ****************************************************************************/
+
+#include <nuttx/config.h>
+
+#include <stdio.h>
+#include <inttypes.h>
+
+/****************************************************************************
+ * Pre-processor Definitions
+ ****************************************************************************/
+
+#define FOCLIBLOG(format, ...) printf(format, ##__VA_ARGS__)
+#define FOCLIBERR(format, ...) printf(format, ##__VA_ARGS__)
+#define FOCLIBWARN(format, ...) printf(format, ##__VA_ARGS__)
+
+#endif /* __APPS_INCLUDE_INDUSTRY_FOC_FOC_LOG_H */
diff --git a/include/industry/foc/foc_utils.h b/include/industry/foc/foc_utils.h
new file mode 100644
index 0000000..65406ce
--- /dev/null
+++ b/include/industry/foc/foc_utils.h
@@ -0,0 +1,92 @@
+/****************************************************************************
+ * apps/include/industry/foc/foc_utils.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 __APPS_INCLUDE_INDUSTRY_FOC_FOC_UTILS_H
+#define __APPS_INCLUDE_INDUSTRY_FOC_FOC_UTILS_H
+
+/****************************************************************************
+ * Included Files
+ ****************************************************************************/
+
+#include <nuttx/config.h>
+
+#include <nuttx/motor/foc/foc.h>
+#include <nuttx/motor/motor_ioctl.h>
+
+/****************************************************************************
+ * Public Function Prototypes
+ ****************************************************************************/
+
+/****************************************************************************
+ * Name: foc_dev_stop
+ ****************************************************************************/
+
+int foc_dev_stop(int fd);
+
+/****************************************************************************
+ * Name: foc_dev_start
+ ****************************************************************************/
+
+int foc_dev_start(int fd);
+
+/****************************************************************************
+ * Name: foc_dev_clearfault
+ ****************************************************************************/
+
+int foc_dev_clearfault(int fd);
+
+/****************************************************************************
+ * Name: foc_dev_getstate
+ ****************************************************************************/
+
+int foc_dev_getstate(int fd, FAR struct foc_state_s *state);
+
+/****************************************************************************
+ * Name: foc_dev_setparams
+ ****************************************************************************/
+
+int foc_dev_setparams(int fd, FAR struct foc_params_s *params);
+
+/****************************************************************************
+ * Name: foc_dev_setcfg
+ ****************************************************************************/
+
+int foc_dev_setcfg(int fd, FAR struct foc_cfg_s *cfg);
+
+/****************************************************************************
+ * Name: foc_dev_getcfg
+ ****************************************************************************/
+
+int foc_dev_getcfg(int fd, FAR struct foc_cfg_s *cfg);
+
+/****************************************************************************
+ * Name: foc_dev_getinfo
+ ****************************************************************************/
+
+int foc_dev_getinfo(int fd, FAR struct foc_info_s *info);
+
+/****************************************************************************
+ * Name: foc_cfg_print
+ ****************************************************************************/
+
+void foc_cfg_print(FAR struct foc_cfg_s *cfg);
+
+#endif /* __APPS_INCLUDE_INDUSTRY_FOC_FOC_UTILS_H */
diff --git a/industry/foc/Kconfig b/industry/foc/Kconfig
new file mode 100644
index 0000000..1d18be9
--- /dev/null
+++ b/industry/foc/Kconfig
@@ -0,0 +1,69 @@
+#
+# For a description of the syntax of this configuration file,
+# see the file kconfig-language.txt in the NuttX tools repository.
+#
+
+config INDUSTRY_FOC
+ bool "FOC Library"
+ default n
+ select LIBDSP
+ ---help---
+ Enable or disable the FOC Library
+
+if INDUSTRY_FOC
+
+config INDUSTRY_FOC_CORDIC
+ bool "Enable CORDIC support"
+ depends on EXPERIMENTAL
+ default n
+ ---help---
+ Enable support for CORDIC hardware acceleration
+
+config INDUSTRY_FOC_FIXED16
+ bool "Enable support for fixed16"
+ default n
+ ---help---
+ Enable support for FOC fixed16 calculations
+
+config INDUSTRY_FOC_FLOAT
+ bool "Enable support for float"
+ default n
+ ---help---
+ Enable support for FOC float calculations
+
+config INDUSTRY_FOC_HANDLER_PRINT
+ bool "FOC handler state printer"
+ default n
+ ---help---
+ Enable support for FOC handler state printer
+
+config INDUSTRY_FOC_ANGLE_OPENLOOP
+ bool "FOC angle open-loop handler"
+ default y
+ ---help---
+ Enable support for open-loop angle handler
+
+config INDUSTRY_FOC_CONTROL_PI
+ bool "FOC PI controller"
+ default y
+ ---help---
+ Enable support for classic FOC PI controller
+
+config INDUSTRY_FOC_MODULATION_SVM3
+ bool "FOC SVM3 modulation"
+ default y
+ ---help---
+ Enable support for FOC 3-phase space vector modulation
+
+config INDUSTRY_FOC_MODEL_PMSM
+ bool "FOC PMSM model support"
+ select INDUSTRY_FOC_HAVE_MODEL
+ default n
+ ---help---
+ Enable support for PMSM model
+
+config INDUSTRY_FOC_HAVE_MODEL
+ bool
+ default n
+
+endif
diff --git a/industry/foc/Make.defs b/industry/foc/Make.defs
new file mode 100644
index 0000000..f5a84f0
--- /dev/null
+++ b/industry/foc/Make.defs
@@ -0,0 +1,24 @@
+############################################################################
+# apps/industry/foc/Make.defs
+# Adds selected applications to apps/ build
+#
+# 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.
+#
+############################################################################
+
+ifeq ($(CONFIG_INDUSTRY_FOC),y)
+CONFIGURED_APPS += $(APPDIR)/industry/foc
+endif
diff --git a/industry/foc/Makefile b/industry/foc/Makefile
new file mode 100644
index 0000000..29b66bd
--- /dev/null
+++ b/industry/foc/Makefile
@@ -0,0 +1,85 @@
+############################################################################
+# apps/industry/foc/Makefile
+#
+# 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.
+#
+############################################################################
+
+include $(APPDIR)/Make.defs
+
+# FOC Library
+
+CSRCS = foc_utils.c
+
+# float support
+
+ifeq ($(CONFIG_INDUSTRY_FOC_FLOAT),y)
+
+CSRCS += float/foc_ramp.c
+CSRCS += float/foc_handler.c
+CSRCS += float/foc_angle.c
+CSRCS += float/foc_velocity.c
+ifeq ($(CONFIG_INDUSTRY_FOC_ANGLE_OPENLOOP),y)
+CSRCS += float/foc_openloop.c
+endif
+ifeq ($(CONFIG_INDUSTRY_FOC_CORDIC),y)
+CSRCS += float/foc_cordic.c
+endif
+ifeq ($(CONFIG_INDUSTRY_FOC_CONTROL_PI),y)
+CSRCS += float/foc_picontrol.c
+endif
+ifeq ($(CONFIG_INDUSTRY_FOC_MODULATION_SVM3),y)
+CSRCS += float/foc_svm3.c
+endif
+ifeq ($(CONFIG_INDUSTRY_FOC_HAVE_MODEL),y)
+CSRCS += float/foc_model.c
+endif
+ifeq ($(CONFIG_INDUSTRY_FOC_MODEL_PMSM),y)
+CSRCS += float/foc_model_pmsm.c
+endif
+
+endif
+
+# fixed16 support
+
+ifeq ($(CONFIG_INDUSTRY_FOC_FIXED16),y)
+
+CSRCS += fixed16/foc_ramp.c
+CSRCS += fixed16/foc_handler.c
+CSRCS += fixed16/foc_angle.c
+CSRCS += fixed16/foc_velocity.c
+ifeq ($(CONFIG_INDUSTRY_FOC_ANGLE_OPENLOOP),y)
+CSRCS += fixed16/foc_openloop.c
+endif
+ifeq ($(CONFIG_INDUSTRY_FOC_CORDIC),y)
+CSRCS += fixed16/foc_cordic.c
+endif
+ifeq ($(CONFIG_INDUSTRY_FOC_CONTROL_PI),y)
+CSRCS += fixed16/foc_picontrol.c
+endif
+ifeq ($(CONFIG_INDUSTRY_FOC_MODULATION_SVM3),y)
+CSRCS += fixed16/foc_svm3.c
+endif
+ifeq ($(CONFIG_INDUSTRY_FOC_HAVE_MODEL),y)
+CSRCS += fixed16/foc_model.c
+endif
+ifeq ($(CONFIG_INDUSTRY_FOC_MODEL_PMSM),y)
+CSRCS += fixed16/foc_model_pmsm.c
+endif
+
+endif
+
+include $(APPDIR)/Application.mk
diff --git a/industry/foc/fixed16/foc_angle.c b/industry/foc/fixed16/foc_angle.c
new file mode 100644
index 0000000..2f255f1
--- /dev/null
+++ b/industry/foc/fixed16/foc_angle.c
@@ -0,0 +1,157 @@
+/****************************************************************************
+ * apps/industry/foc/fixed16/foc_angle.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 "industry/foc/foc_log.h"
+#include "industry/foc/fixed16/foc_angle.h"
+
+/****************************************************************************
+ * Public Functions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Name: foc_angle_init_b16
+ *
+ * Description:
+ * Initialize the FOC angle handler (fixed16)
+ *
+ * Input Parameter:
+ * h - pointer to FOC angle handler
+ * ops - pointer to FOC angle handler operations
+ *
+ ****************************************************************************/
+
+int foc_angle_init_b16(FAR foc_angle_b16_t *h,
+ FAR struct foc_angle_ops_b16_s *ops)
+{
+ int ret = OK;
+
+ DEBUGASSERT(h);
+ DEBUGASSERT(ops);
+
+ /* Angle ops */
+
+ DEBUGASSERT(ops->init);
+ DEBUGASSERT(ops->deinit);
+ DEBUGASSERT(ops->cfg);
+ DEBUGASSERT(ops->run);
+
+ /* Reset handler */
+
+ memset(h, 0, sizeof(foc_angle_b16_t));
+
+ /* Connect ops */
+
+ h->ops = ops;
+
+ /* Initialize angle */
+
+ ret = h->ops->init(h);
+ if (ret < 0)
+ {
+ FOCLIBERR("ERROR: ops->init failed %d\n", ret);
+ goto errout;
+ }
+
+errout:
+ return ret;
+}
+
+/****************************************************************************
+ * Name: foc_angle_deinit_b16
+ *
+ * Description:
+ * De-initialize the FOC angle handler (fixed16)
+ *
+ * Input Parameter:
+ * h - pointer to FOC angle handler
+ *
+ ****************************************************************************/
+
+int foc_angle_deinit_b16(FAR foc_angle_b16_t *h)
+{
+ int ret = OK;
+
+ DEBUGASSERT(h);
+
+ /* Deinitialize angle */
+
+ h->ops->deinit(h);
+
+ /* Reset data */
+
+ memset(h, 0, sizeof(foc_angle_b16_t));
+
+ return ret;
+}
+
+/****************************************************************************
+ * Name: foc_angle_cfg_b16
+ *
+ * Description:
+ * Configure the FOC angle handler (fixed16)
+ *
+ * Input Parameter:
+ * h - pointer to FOC angle handler
+ * cfg - pointer to angle handler configuration data
+ *
+ ****************************************************************************/
+
+int foc_angle_cfg_b16(FAR foc_angle_b16_t *h, FAR void *cfg)
+{
+ DEBUGASSERT(h);
+ DEBUGASSERT(cfg);
+
+ return h->ops->cfg(h, cfg);
+}
+
+/****************************************************************************
+ * Name: foc_angle_run_b16
+ *
+ * Description:
+ * Process the FOC angle handler 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
+ *
+ ****************************************************************************/
+
+void foc_angle_run_b16(FAR foc_angle_b16_t *h,
+ FAR struct foc_angle_in_b16_s *in,
+ FAR struct foc_angle_out_b16_s *out)
+{
+ DEBUGASSERT(h);
+ DEBUGASSERT(in);
+ DEBUGASSERT(out);
+
+ /* Run angle handler */
+
+ h->ops->run(h, in, out);
+}
diff --git a/industry/foc/fixed16/foc_cordic.c b/industry/foc/fixed16/foc_cordic.c
new file mode 100644
index 0000000..e7311ab
--- /dev/null
+++ b/industry/foc/fixed16/foc_cordic.c
@@ -0,0 +1,81 @@
+/****************************************************************************
+ * apps/industry/foc/fixed16/foc_cordic.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 "industry/foc/fixed16/foc_cordic.h"
+
+/****************************************************************************
+ * Public Functions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Name: foc_cordic_dqsat_b16
+ *
+ * Description:
+ * CORDIC DQ-frame saturation (fixed16)
+ *
+ * Input Parameter:
+ * fd - the file descriptor for CORDIC device
+ * dq_ref - DQ vector
+ * mag_max - vector magnitude max
+ *
+ ****************************************************************************/
+
+int foc_cordic_dqsat_b16(int fd, FAR dq_frame_b16_t *dq, b16_t mag_max)
+{
+ DEBUGASSERT(dq);
+
+ /* TODO: */
+
+ ASSERT(0);
+
+ return OK;
+}
+
+/****************************************************************************
+ * Name: foc_cordic_angle_b16
+ *
+ * Description:
+ * CORDIC angle update (fixed16)
+ *
+ * Input Parameter:
+ * fd - the file descriptor for CORDIC device
+ * angle - phase angle data
+ * a - phase angle in rad
+ *
+ ****************************************************************************/
+
+int foc_cordic_angle_b16(int fd, FAR phase_angle_b16_t *angle, b16_t a)
+{
+ DEBUGASSERT(angle);
+
+ /* TODO: */
+
+ ASSERT(0);
+
+ return OK;
+}
diff --git a/industry/foc/fixed16/foc_handler.c b/industry/foc/fixed16/foc_handler.c
new file mode 100644
index 0000000..9b1927b
--- /dev/null
+++ b/industry/foc/fixed16/foc_handler.c
@@ -0,0 +1,345 @@
+/****************************************************************************
+ * apps/industry/foc/fixed16/foc_handler.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 "industry/foc/foc_log.h"
+#include "industry/foc/foc_common.h"
+#include "industry/foc/fixed16/foc_handler.h"
+
+/****************************************************************************
+ * Public Functions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Name: foc_handler_init_b16
+ *
+ * Description:
+ * Initialize the FOC handler (fixed16)
+ *
+ * Input Parameter:
+ * h - pointer to FOC handler
+ * ctrl - pointer to controller operations
+ * mod - pointer to modulation operations
+ *
+ ****************************************************************************/
+
+int foc_handler_init_b16(FAR foc_handler_b16_t *h,
+ FAR struct foc_control_ops_b16_s *ctrl,
+ FAR struct foc_modulation_ops_b16_s *mod)
+{
+ int ret = OK;
+
+ DEBUGASSERT(h);
+ DEBUGASSERT(ctrl);
+ DEBUGASSERT(mod);
+
+ /* Controller ops */
+
+ DEBUGASSERT(ctrl->init);
+ DEBUGASSERT(ctrl->deinit);
+ DEBUGASSERT(ctrl->cfg);
+ DEBUGASSERT(ctrl->input_set);
+ DEBUGASSERT(ctrl->voltage_run);
+ DEBUGASSERT(ctrl->current_run);
+ DEBUGASSERT(ctrl->state_get);
+
+ /* Modulation ops */
+
+ DEBUGASSERT(mod->init);
+ DEBUGASSERT(mod->deinit);
+ DEBUGASSERT(mod->cfg);
+ DEBUGASSERT(mod->current);
+ DEBUGASSERT(mod->vbase_get);
+ DEBUGASSERT(mod->run);
+
+ /* Reset handler */
+
+ memset(h, 0, sizeof(foc_handler_b16_t));
+
+ /* Connect ops */
+
+ h->ops.ctrl = ctrl;
+ h->ops.mod = mod;
+
+ /* Initialize control handler */
+
+ ret = h->ops.mod->init(h);
+ if (ret < 0)
+ {
+ FOCLIBERR("ERROR: mod->init failed %d\n", ret);
+ goto errout;
+ }
+
+ /* Initialize modulation handler */
+
+ ret = h->ops.ctrl->init(h);
+ if (ret < 0)
+ {
+ FOCLIBERR("ERROR: ctrl->init failed %d\n", ret);
+ goto errout;
+ }
+
+errout:
+ return ret;
+}
+
+/****************************************************************************
+ * Name: foc_handler_deinit_b16
+ *
+ * Description:
+ * De-initialize the FOC handler (fixed16)
+ *
+ * Input Parameter:
+ * h - pointer to FOC handler
+ *
+ ****************************************************************************/
+
+int foc_handler_deinit_b16(FAR foc_handler_b16_t *h)
+{
+ int ret = OK;
+
+ DEBUGASSERT(h);
+
+ /* Deinitialize modulation handler */
+
+ h->ops.mod->deinit(h);
+
+ /* Deinitialize control handler */
+
+ h->ops.ctrl->deinit(h);
+
+ /* Reset data */
+
+ memset(h, 0, sizeof(foc_handler_b16_t));
+
+ return ret;
+}
+
+/****************************************************************************
+ * Name: foc_handler_cfg_b16
+ *
+ * Description:
+ * Configure the FOC handler (fixed16)
+ *
+ * Input Parameter:
+ * h - pointer to FOC handler
+ * ctrl_cfg - pointer to controller configuration data
+ * mod_cfg - pointer to modulation configuration data
+ *
+ ****************************************************************************/
+
+void foc_handler_cfg_b16(FAR foc_handler_b16_t *h,
+ FAR void *ctrl_cfg,
+ FAR void *mod_cfg)
+{
+ DEBUGASSERT(h);
+ DEBUGASSERT(ctrl_cfg);
+ DEBUGASSERT(mod_cfg);
+
+ /* Configure controller */
+
+ h->ops.ctrl->cfg(h, ctrl_cfg);
+
+ /* Configure modulation */
+
+ h->ops.mod->cfg(h, mod_cfg);
+}
+
+/****************************************************************************
+ * Name: foc_handler_run_b16
+ *
+ * Description:
+ * Run the FOC handler and process the given input data (fixed16)
+ *
+ * Input Parameter:
+ * h - pointer to FOC handler
+ * in - pointer to FOC handler input data
+ * out - pointer to FOC handler output data
+ *
+ ****************************************************************************/
+
+int foc_handler_run_b16(FAR foc_handler_b16_t *h,
+ FAR struct foc_handler_input_b16_s *in,
+ FAR struct foc_handler_output_b16_s *out)
+{
+ ab_frame_b16_t v_ab_mod;
+ b16_t vbase = 0;
+ int ret = OK;
+
+ DEBUGASSERT(h);
+ DEBUGASSERT(in);
+ DEBUGASSERT(out);
+
+ /* Do nothing if control mode not specified yet.
+ * This also protects against initial state when the controller is
+ * started but input data has not yet been provided.
+ */
+
+ if (in->mode <= FOC_HANDLER_MODE_INIT)
+ {
+ ret = -EINVAL;
+ goto errout;
+ }
+
+ /* Correct current samples according to modulation state */
+
+ h->ops.mod->current(h, in->current);
+
+ /* Get VBASE */
+
+ h->ops.mod->vbase_get(h, in->vbus, &vbase);
+
+ /* Feed controller with phase currents */
+
+ h->ops.ctrl->input_set(h, in->current, vbase, in->angle);
+
+ /* Call controller */
+
+ switch (in->mode)
+ {
+ /* IDLE */
+
+ case FOC_HANDLER_MODE_IDLE:
+ {
+ /* Do nothing and set duty to zeros */
+
+ ret = OK;
+ goto errout;
+ }
+
+ /* FOC current mode - control DQ-current */
+
+ case FOC_HANDLER_MODE_CURRENT:
+ {
+ /* Current controller */
+
+ h->ops.ctrl->current_run(h,
+ in->dq_ref,
+ in->vdq_comp,
+ &v_ab_mod);
+
+ break;
+ }
+
+ /* FOC voltage mode - control DQ-voltage */
+
+ case FOC_HANDLER_MODE_VOLTAGE:
+ {
+ /* Voltage controller */
+
+ h->ops.ctrl->voltage_run(h,
+ in->dq_ref,
+ &v_ab_mod);
+
+ break;
+ }
+
+ /* Otherwise - we should not be here */
+
+ default:
+ {
+ ret = -EINVAL;
+ goto errout;
+ }
+ }
+
+ /* Duty cycle modulation */
+
+ h->ops.mod->run(h, &v_ab_mod, out->duty);
+
+ return ret;
+
+errout:
+
+ /* Set duty to zeros */
+
+ memset(out->duty, 0, sizeof(b16_t) * CONFIG_MOTOR_FOC_PHASES);
+
+ return ret;
+}
+
+/****************************************************************************
+ * Name: foc_handler_state_b16
+ *
+ * Description:
+ * Get FOC handler state (fixed16)
+ *
+ * Input Parameter:
+ * h - pointer to FOC handler
+ * state - pointer to FOC state data
+ *
+ ****************************************************************************/
+
+void foc_handler_state_b16(FAR foc_handler_b16_t *h,
+ FAR struct foc_state_b16_s *state)
+{
+ DEBUGASSERT(h);
+ DEBUGASSERT(state);
+
+ h->ops.ctrl->state_get(h, state);
+}
+
+#ifdef CONFIG_INDUSTRY_FOC_HANDLER_PRINT
+/****************************************************************************
+ * Name: foc_handler_state_print_b16
+ *
+ * Description:
+ * Print FOC handler state (fixed16)
+ *
+ * Input Parameter:
+ * state - pointer to FOC state data
+ *
+ ****************************************************************************/
+
+void foc_handler_state_print_b16(FAR struct foc_state_b16_s *state)
+{
+ DEBUGASSERT(state);
+
+#if CONFIG_MOTOR_FOC_PHASES == 3
+ FOCLIBLOG("curr = [%.2f %.2f %.2f]\n",
+ b16tof(state->curr[0]),
+ b16tof(state->curr[1]),
+ b16tof(state->curr[2]));
+ FOCLIBLOG("volt = [%.2f %.2f %.2f]\n",
+ b16tof(state->volt[0]),
+ b16tof(state->volt[1]),
+ b16tof(state->volt[2]));
+#else
+# error TODO
+#endif
+
+ FOCLIBLOG("iab = [%.2f %.2f]\n",
+ b16tof(state->iab.a), b16tof(state->iab.b));
+ FOCLIBLOG("vab = [%.2f %.2f]\n",
+ b16tof(state->vab.a), b16tof(state->vab.b));
+ FOCLIBLOG("idq = [%.2f %.2f]\n",
+ b16tof(state->idq.d), b16tof(state->idq.q));
+ FOCLIBLOG("vdq = [%.2f %.2f]\n",
+ b16tof(state->vdq.d), b16tof(state->vdq.q));
+}
+#endif
diff --git a/industry/foc/fixed16/foc_model.c b/industry/foc/fixed16/foc_model.c
new file mode 100644
index 0000000..8540dee
--- /dev/null
+++ b/industry/foc/fixed16/foc_model.c
@@ -0,0 +1,188 @@
+/****************************************************************************
+ * apps/industry/foc/fixed16/foc_model.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 "industry/foc/foc_log.h"
+#include "industry/foc/foc_common.h"
+#include "industry/foc/fixed16/foc_model.h"
+
+/****************************************************************************
+ * Public Functions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Name: foc_model_init_b16
+ *
+ * Description:
+ * Initialize the FOC model (fixed16)
+ *
+ * Input Parameter:
+ * h - pointer to FOC model handler
+ * ops - pointer to FOC model operations
+ *
+ ****************************************************************************/
+
+int foc_model_init_b16(FAR foc_model_b16_t *h,
+ FAR struct foc_model_ops_b16_s *ops)
+{
+ int ret = OK;
+
+ DEBUGASSERT(h);
+ DEBUGASSERT(ops);
+
+ /* Model ops */
+
+ DEBUGASSERT(ops->init);
+ DEBUGASSERT(ops->deinit);
+ DEBUGASSERT(ops->cfg);
+ DEBUGASSERT(ops->ele_run);
+ DEBUGASSERT(ops->mech_run);
+ DEBUGASSERT(ops->state);
+
+ /* Reset handler */
+
+ memset(h, 0, sizeof(foc_model_b16_t));
+
+ /* Connect ops */
+
+ h->ops = ops;
+
+ /* Initialize model */
+
+ ret = h->ops->init(h);
+ if (ret < 0)
+ {
+ FOCLIBERR("ERROR: ops->init failed %d\n", ret);
+ goto errout;
+ }
+
+errout:
+ return ret;
+}
+
+/****************************************************************************
+ * Name: foc_model_deinit_b16
+ *
+ * Description:
+ * De-initialize the FOC model (fixed16)
+ *
+ * Input Parameter:
+ * h - pointer to FOC model handler
+ *
+ ****************************************************************************/
+
+int foc_model_deinit_b16(FAR foc_model_b16_t *h)
+{
+ int ret = OK;
+
+ DEBUGASSERT(h);
+
+ /* Deinitialize model */
+
+ h->ops->deinit(h);
+
+ /* Reset data */
+
+ memset(h, 0, sizeof(foc_model_b16_t));
+
+ return ret;
+}
+
+/****************************************************************************
+ * Name: foc_model_cfg_b16
+ *
+ * Description:
+ * Configure the FOC model (fixed16)
+ *
+ * Input Parameter:
+ * h - pointer to FOC model handler
+ * cfg - pointer to FOC model configuration data
+ *
+ ****************************************************************************/
+
+int foc_model_cfg_b16(FAR foc_model_b16_t *h, FAR void *cfg)
+{
+ DEBUGASSERT(h);
+ DEBUGASSERT(cfg);
+
+ return h->ops->cfg(h, cfg);
+}
+
+/****************************************************************************
+ * Name: foc_model_run_b16
+ *
+ * Description:
+ * Run the FOC model (fixed16)
+ *
+ * Input Parameter:
+ * h - pointer to FOC model handler
+ * load - applied load
+ * v_ab - applied voltage in alpha-beta frame
+ *
+ * REVISIT:
+ * It would be better if we feed model with duty cycle and VBUS, but it
+ * complicates the calculations, so we leave it like that for now
+ *
+ ****************************************************************************/
+
+void foc_model_run_b16(FAR foc_model_b16_t *h,
+ b16_t load,
+ FAR ab_frame_b16_t *v_ab)
+{
+ DEBUGASSERT(h);
+ DEBUGASSERT(v_ab);
+
+ /* Run electrical model */
+
+ h->ops->ele_run(h, v_ab);
+
+ /* Run mechanical model */
+
+ h->ops->mech_run(h, load);
+}
+
+/****************************************************************************
+ * Name: foc_model_state_b16
+ *
+ * Description:
+ * Get model state (fixed16)
+ *
+ * Input Parameter:
+ * h - pointer to FOC model handler
+ * state - pointer to FOC model state
+ *
+ ****************************************************************************/
+
+void foc_model_state_b16(FAR foc_model_b16_t *h,
+ FAR struct foc_model_state_b16_s *state)
+{
+ DEBUGASSERT(h);
+ DEBUGASSERT(state);
+
+ h->ops->state(h, state);
+}
diff --git a/industry/foc/fixed16/foc_model_pmsm.c b/industry/foc/fixed16/foc_model_pmsm.c
new file mode 100644
index 0000000..e3da0d9
--- /dev/null
+++ b/industry/foc/fixed16/foc_model_pmsm.c
@@ -0,0 +1,300 @@
+/****************************************************************************
+ * apps/industry/foc/fixed16/foc_model_pmsm.c
+ * This file implements PMSM model 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/fixed16/foc_model.h"
+
+/****************************************************************************
+ * Pre-processor Definitions
+ ****************************************************************************/
+
+#if CONFIG_MOTOR_FOC_PHASES != 3
+# error
+#endif
+
+/****************************************************************************
+ * Private Function Prototypes
+ ****************************************************************************/
+
+/* PMSM model data */
+
+struct foc_model_pmsm_b16_s
+{
+ b16_t one_by_iphadc;
+ struct foc_model_pmsm_cfg_b16_s cfg;
+ struct pmsm_model_b16_s model;
+};
+
+/****************************************************************************
+ * Private Function Prototypes
+ ****************************************************************************/
+
+static int foc_model_pmsm_init_b16(FAR foc_model_b16_t *h);
+static void foc_model_pmsm_deinit_b16(FAR foc_model_b16_t *h);
+static int foc_model_pmsm_cfg_b16(FAR foc_model_b16_t *h, FAR void *cfg);
+static void foc_model_pmsm_elerun_b16(FAR foc_model_b16_t *h,
+ FAR ab_frame_b16_t *v_ab);
+static void foc_model_pmsm_mechrun_b16(FAR foc_model_b16_t *h, b16_t load);
+static void foc_model_pmsm_state_b16(FAR foc_model_b16_t *h,
+ FAR struct foc_model_state_b16_s *state);
+
+/****************************************************************************
+ * Public Data
+ ****************************************************************************/
+
+/* FOC model fixed16 interface */
+
+struct foc_model_ops_b16_s g_foc_model_pmsm_ops_b16 =
+{
+ .init = foc_model_pmsm_init_b16,
+ .deinit = foc_model_pmsm_deinit_b16,
+ .cfg = foc_model_pmsm_cfg_b16,
+ .ele_run = foc_model_pmsm_elerun_b16,
+ .mech_run = foc_model_pmsm_mechrun_b16,
+ .state = foc_model_pmsm_state_b16,
+};
+
+/****************************************************************************
+ * Private Functions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Name: foc_model_pmsm_init_b16
+ *
+ * Description:
+ * Initialize PMSM model (fixed16)
+ *
+ * Input Parameter:
+ * h - pointer to FOC model handler
+ *
+ ****************************************************************************/
+
+static int foc_model_pmsm_init_b16(FAR foc_model_b16_t *h)
+{
+ int ret = OK;
+
+ DEBUGASSERT(h);
+
+ /* Connect model data */
+
+ h->model = zalloc(sizeof(struct foc_model_pmsm_b16_s));
+ if (h->model == NULL)
+ {
+ ret = -ENOMEM;
+ goto errout;
+ }
+
+errout:
+ return ret;
+}
+
+/****************************************************************************
+ * Name: foc_model_pmsm_deinit_b16
+ *
+ * Description:
+ * Deinitialize PMSM model (fixed16)
+ *
+ * Input Parameter:
+ * h - pointer to FOC model handler
+ *
+ ****************************************************************************/
+
+static void foc_model_pmsm_deinit_b16(FAR foc_model_b16_t *h)
+{
+ DEBUGASSERT(h);
+
+ /* Free model data */
+
+ if (h->model)
+ {
+ free (h->model);
+ }
+}
+
+/****************************************************************************
+ * Name: foc_model_pmsm_cfg_b16
+ *
+ * Description:
+ * Configure PMSM model (fixed16)
+ *
+ * Input Parameter:
+ * h - pointer to FOC model handler
+ * cfg - pointer to FOC model configuration (struct foc_model_pmsm_b16_s)
+ *
+ ****************************************************************************/
+
+static int foc_model_pmsm_cfg_b16(FAR foc_model_b16_t *h, FAR void *cfg)
+{
+ FAR struct foc_model_pmsm_b16_s *model = NULL;
+ struct pmsm_phy_params_b16_s phy;
+
+ DEBUGASSERT(h);
+ DEBUGASSERT(cfg);
+
+ /* Get model data */
+
+ DEBUGASSERT(h->model);
+ model = h->model;
+
+ /* Copy data */
+
+ memcpy(&model->cfg, cfg, sizeof(struct foc_model_pmsm_cfg_b16_s));
+
+ /* Initialize model */
+
+ pmsm_phy_params_init_b16(&phy,
+ model->cfg.poles,
+ model->cfg.res,
+ model->cfg.ind,
+ model->cfg.iner,
+ model->cfg.flux_link,
+ model->cfg.ind_q,
+ model->cfg.ind_d);
+
+ pmsm_model_initialize_b16(&model->model, &phy, model->cfg.per);
+
+ /* Get one by iphase_adc */
+
+ model->one_by_iphadc = b16divb16(b16ONE, model->cfg.iphase_adc);
+
+ return OK;
+}
+
+/****************************************************************************
+ * Name: foc_model_pmsm_elerun_b16
+ *
+ * Description:
+ * Run model electrical simulation (fixed16)
+ *
+ * Input Parameter:
+ * h - pointer to FOC model handler
+ * v_ab - applied voltage in alpha-beta frame
+ *
+ ****************************************************************************/
+
+static void foc_model_pmsm_elerun_b16(FAR foc_model_b16_t *h,
+ FAR ab_frame_b16_t *v_ab)
+{
+ FAR struct foc_model_pmsm_b16_s *model = NULL;
+
+ DEBUGASSERT(h);
+ DEBUGASSERT(v_ab);
+
+ /* Get model data */
+
+ DEBUGASSERT(h->model);
+ model = h->model;
+
+ /* Run electrical model */
+
+ pmsm_model_elec_b16(&model->model, v_ab);
+}
+
+/****************************************************************************
+ * Name: foc_model_pmsm_mechrun_b16
+ *
+ * Description:
+ * Run model mechanical simulation (fixed16)
+ *
+ * Input Parameter:
+ * h - pointer to FOC model handler
+ * load - applied load
+ *
+ ****************************************************************************/
+
+static void foc_model_pmsm_mechrun_b16(FAR foc_model_b16_t *h, b16_t load)
+{
+ FAR struct foc_model_pmsm_b16_s *model = NULL;
+
+ DEBUGASSERT(h);
+
+ /* Get model data */
+
+ DEBUGASSERT(h->model);
+ model = h->model;
+
+ /* Run mechanical model */
+
+ pmsm_model_mech_b16(&model->model, load);
+}
+
+/****************************************************************************
+ * Name: foc_model_pmsm_state_b16
+ *
+ * Description:
+ * Get the model state (fixed16)
+ *
+ * Input Parameter:
+ * h - pointer to FOC model handler
+ * state - pointer to FOC model state
+ *
+ ****************************************************************************/
+
+static void foc_model_pmsm_state_b16(FAR foc_model_b16_t *h,
+ FAR struct foc_model_state_b16_s *state)
+{
+ FAR struct foc_model_pmsm_b16_s *model = NULL;
+
+ DEBUGASSERT(h);
+ DEBUGASSERT(state);
+
+ /* Get model data */
+
+ DEBUGASSERT(h->model);
+ model = h->model;
+
+ /* Get model state */
+
+ state->volt[0] = model->model.state.v_abc.a;
+ state->volt[1] = model->model.state.v_abc.b;
+ state->volt[2] = model->model.state.v_abc.c;
+ state->vab.a = model->model.state.v_ab.a;
+ state->vab.b = model->model.state.v_ab.b;
+ state->vdq.d = model->model.state.v_dq.d;
+ state->vdq.q = model->model.state.v_dq.q;
+ state->curr[0] = model->model.state.i_abc.a;
+ state->curr[1] = model->model.state.i_abc.b;
+ state->curr[2] = model->model.state.i_abc.c;
+ state->iab.a = model->model.state.i_ab.a;
+ state->iab.b = model->model.state.i_ab.b;
+ state->idq.d = model->model.state.i_dq.d;
+ state->idq.q = model->model.state.i_dq.q;
+ state->omega_e = model->model.state.omega_e;
+ state->omega_m = model->model.state.omega_m;
+
+ /* Get RAW currents */
+
+ state->curr_raw[0] = b16toi(b16mulb16(state->curr[0],
+ model->one_by_iphadc));
+ state->curr_raw[1] = b16toi(b16mulb16(state->curr[1],
+ model->one_by_iphadc));
+ state->curr_raw[2] = b16toi(b16mulb16(state->curr[2],
+ model->one_by_iphadc));
+}
diff --git a/industry/foc/fixed16/foc_openloop.c b/industry/foc/fixed16/foc_openloop.c
new file mode 100644
index 0000000..1a272c9
--- /dev/null
+++ b/industry/foc/fixed16/foc_openloop.c
@@ -0,0 +1,195 @@
+/****************************************************************************
+ * apps/industry/foc/fixed16/foc_openloop.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 Function Prototypes
+ ****************************************************************************/
+
+/* Open-loop data */
+
+struct foc_openloop_b16_s
+{
+ struct foc_openloop_cfg_b16_s cfg;
+ struct openloop_data_b16_s data;
+};
+
+/****************************************************************************
+ * Private Function Prototypes
+ ****************************************************************************/
+
+static int foc_angle_ol_init_b16(FAR foc_angle_b16_t *h);
+static void foc_angle_ol_deinit_b16(FAR foc_angle_b16_t *h);
+static int foc_angle_ol_cfg_b16(FAR foc_angle_b16_t *h, FAR void *cfg);
+static void foc_angle_ol_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
+ ****************************************************************************/
+
+struct foc_angle_ops_b16_s g_foc_angle_ol_b16 =
+{
+ .init = foc_angle_ol_init_b16,
+ .deinit = foc_angle_ol_deinit_b16,
+ .cfg = foc_angle_ol_cfg_b16,
+ .run = foc_angle_ol_run_b16,
+};
+
+/****************************************************************************
+ * Private Functions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Name: foc_angle_ol_init_b16
+ *
+ * Description:
+ * Initialize the open-loop FOC angle handler (fixed16)
+ *
+ * Input Parameter:
+ * h - pointer to FOC angle handler
+ *
+ ****************************************************************************/
+
+static int foc_angle_ol_init_b16(FAR foc_angle_b16_t *h)
+{
+ int ret = OK;
+
+ DEBUGASSERT(h);
+
+ /* Connect angle data */
+
+ h->data = zalloc(sizeof(struct foc_openloop_b16_s));
+ if (h->data == NULL)
+ {
+ ret = -ENOMEM;
+ goto errout;
+ }
+
+errout:
+ return ret;
+}
+
+/****************************************************************************
+ * Name: foc_angle_ol_deinit_b16
+ *
+ * Description:
+ * De-initialize the open-loop FOC angle handler (fixed16)
+ *
+ * Input Parameter:
+ * h - pointer to FOC angle handler
+ *
+ ****************************************************************************/
+
+static void foc_angle_ol_deinit_b16(FAR foc_angle_b16_t *h)
+{
+ DEBUGASSERT(h);
+
+ /* Free angle data */
+
+ if (h->data)
+ {
+ free(h->data);
+ }
+}
+
+/****************************************************************************
+ * Name: foc_angle_ol_cfg_b16
+ *
+ * Description:
+ * Configure the open-loop FOC angle handler (fixed16)
+ *
+ * Input Parameter:
+ * h - pointer to FOC angle handler
+ * cfg - pointer to angle handler configuration data
+ * (struct foc_openloop_b16_s)
+ *
+ ****************************************************************************/
+
+static int foc_angle_ol_cfg_b16(FAR foc_angle_b16_t *h, FAR void *cfg)
+{
+ FAR struct foc_openloop_b16_s *ol = NULL;
+
+ DEBUGASSERT(h);
+
+ /* Get modulation data */
+
+ DEBUGASSERT(h->data);
+ ol = h->data;
+
+ /* Copy configuration */
+
+ memcpy(&ol->cfg, cfg, sizeof(struct foc_openloop_cfg_b16_s));
+
+ /* Initialize open-loop controller data */
+
+ DEBUGASSERT(ol->cfg.per > 0);
+ motor_openloop_init_b16(&ol->data, ol->cfg.per);
+
+ return OK;
+}
+
+/****************************************************************************
+ * Name: foc_angle_ol_run_b16
+ *
+ * Description:
+ * Process the FOC open-loop 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 void foc_angle_ol_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_openloop_b16_s *ol = NULL;
+
+ DEBUGASSERT(h);
+
+ /* Get modulation data */
+
+ DEBUGASSERT(h->data);
+ ol = h->data;
+
+ /* Update open-loop */
+
+ motor_openloop_b16(&ol->data, in->vel, in->dir);
+
+ /* Get open-loop angle */
+
+ out->angle = motor_openloop_angle_get_b16(&ol->data);
+}
diff --git a/industry/foc/fixed16/foc_picontrol.c b/industry/foc/fixed16/foc_picontrol.c
new file mode 100644
index 0000000..4906a47
--- /dev/null
+++ b/industry/foc/fixed16/foc_picontrol.c
@@ -0,0 +1,417 @@
+/****************************************************************************
+ * apps/industry/foc/fixed16/foc_picontrol.c
+ * This file implements classical FOC PI current controller 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/fixed16/foc_handler.h"
+
+/****************************************************************************
+ * Pre-processor Definitions
+ ****************************************************************************/
+
+#if CONFIG_MOTOR_FOC_PHASES != 3
+# error
+#endif
+
+/****************************************************************************
+ * Private Data Types
+ ****************************************************************************/
+
+/* FOC PI fixed16 controller data */
+
+struct foc_picontrol_b16_s
+{
+ b16_t vbase_last; /* Last VBASE sample */
+ phase_angle_b16_t angle; /* Phase angle */
+ struct foc_initdata_b16_s cfg; /* Controller configuration */
+ struct foc_data_b16_s data; /* Controller private data */
+};
+
+/****************************************************************************
+ * Private Function Prototypes
+ ****************************************************************************/
+
+static int foc_control_init_b16(FAR foc_handler_b16_t *h);
+static void foc_control_deinit_b16(FAR foc_handler_b16_t *h);
+static void foc_control_cfg_b16(FAR foc_handler_b16_t *h, FAR void *cfg);
+static void foc_control_input_set_b16(FAR foc_handler_b16_t *h,
+ FAR b16_t *current,
+ b16_t vbase,
+ b16_t angle);
+static void foc_control_voltage_run_b16(FAR foc_handler_b16_t *h,
+ FAR dq_frame_b16_t *dq_ref,
+ FAR ab_frame_b16_t *v_ab_mod);
+static void foc_control_current_run_b16(FAR foc_handler_b16_t *h,
+ FAR dq_frame_b16_t *dq_ref,
+ FAR dq_frame_b16_t *vdq_comp,
+ FAR ab_frame_b16_t *v_ab_mod);
+static void foc_control_state_get_b16(FAR foc_handler_b16_t *h,
+ FAR struct foc_state_b16_s *state);
+
+/****************************************************************************
+ * Public Data
+ ****************************************************************************/
+
+/* FOC control fixed16 interface */
+
+struct foc_control_ops_b16_s g_foc_control_pi_b16 =
+{
+ .init = foc_control_init_b16,
+ .deinit = foc_control_deinit_b16,
+ .cfg = foc_control_cfg_b16,
+ .input_set = foc_control_input_set_b16,
+ .voltage_run = foc_control_voltage_run_b16,
+ .current_run = foc_control_current_run_b16,
+ .state_get = foc_control_state_get_b16,
+};
+
+/****************************************************************************
+ * Private Functions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Name: foc_control_init_b16
+ *
+ * Description:
+ * Initialize the FOC PI controller (fixed16)
+ *
+ * Input Parameter:
+ * h - pointer to FOC handler
+ *
+ ****************************************************************************/
+
+static int foc_control_init_b16(FAR foc_handler_b16_t *h)
+{
+ int ret = OK;
+
+ DEBUGASSERT(h);
+
+ /* Connect controller data */
+
+ h->control = zalloc(sizeof(struct foc_picontrol_b16_s));
+ if (h->control == NULL)
+ {
+ ret = -ENOMEM;
+ goto errout;
+ }
+
+errout:
+ return ret;
+}
+
+/****************************************************************************
+ * Name: foc_control_deinit_b16
+ *
+ * Description:
+ * Deinitialize the FOC PI controller (fixed16)
+ *
+ * Input Parameter:
+ * h - pointer to FOC handler
+ *
+ ****************************************************************************/
+
+static void foc_control_deinit_b16(FAR foc_handler_b16_t *h)
+{
+ DEBUGASSERT(h);
+
+ /* Free controller data */
+
+ if (h->control)
+ {
+ free(h->control);
+ }
+}
+
+/****************************************************************************
+ * Name: foc_control_cfg_set_b16
+ *
+ * Description:
+ * Configure the FOC controller (fixed16)
+ *
+ * Input Parameter:
+ * h - pointer to FOC handler
+ * cfg - pointer to controller configuration data
+ * (struct foc_picontrol_b16_s)
+ *
+ ****************************************************************************/
+
+static void foc_control_cfg_b16(FAR foc_handler_b16_t *h, FAR void *cfg)
+{
+ FAR struct foc_picontrol_b16_s *foc = NULL;
+
+ DEBUGASSERT(h);
+ DEBUGASSERT(cfg);
+
+ /* Get controller data */
+
+ DEBUGASSERT(h->control);
+ foc = h->control;
+
+ /* Copy data */
+
+ memcpy(&foc->cfg, cfg, sizeof(struct foc_initdata_b16_s));
+
+ /* Initialize FOC controller data */
+
+ foc_init_b16(&foc->data, &foc->cfg);
+}
+
+/****************************************************************************
+ * Name: foc_control_input_set_b16
+ *
+ * Description:
+ * Update input for controller (fixed16)
+ *
+ * Input Parameter:
+ * h - pointer to FOC handler
+ * current - phase currents in amps
+ * vbase - base voltage for controller
+ * angle - phase angle in rad
+ *
+ ****************************************************************************/
+
+static void foc_control_input_set_b16(FAR foc_handler_b16_t *h,
+ FAR b16_t *current,
+ b16_t vbase,
+ b16_t angle)
+{
+ FAR struct foc_picontrol_b16_s *foc = NULL;
+ abc_frame_b16_t i_abc;
+
+ DEBUGASSERT(h);
+ DEBUGASSERT(current);
+
+ /* Get controller data */
+
+ DEBUGASSERT(h->control);
+ foc = h->control;
+
+ /* Get current in abc frame */
+
+ i_abc.a = current[0];
+ i_abc.b = current[1];
+ i_abc.c = current[2];
+
+ foc_iabc_update_b16(&foc->data, &i_abc);
+
+ /* Update base voltage only if changed */
+
+ if (foc->vbase_last != vbase)
+ {
+ /* Update FOC base voltage */
+
+ foc_vbase_update_b16(&foc->data, vbase);
+
+ /* Update last FOC base voltage */
+
+ foc->vbase_last = vbase;
+ }
+
+ /* Update phase angle */
+
+#ifndef CONFIG_INDUSTRY_FOC_CORDIC
+ phase_angle_update_b16(&foc->angle, angle);
+#else
+ foc_cordic_angle_b16(h->fd, &foc->angle, angle);
+#endif
+
+ /* Feed the controller with phase angle */
+
+ foc_angle_update_b16(&foc->data, &foc->angle);
+}
+
+/****************************************************************************
+ * Name: foc_control_voltage_b16
+ *
+ * Description:
+ * Handle the FOC voltage control (fixed16)
+ *
+ * Input Parameter:
+ * h - pointer to FOC handler
+ * dq_ref - DQ voltage reference frame
+ * v_ab_mod - (out) modulation alpha-veta voltage
+ *
+ ****************************************************************************/
+
+static void foc_control_voltage_run_b16(FAR foc_handler_b16_t *h,
+ FAR dq_frame_b16_t *dq_ref,
+ FAR ab_frame_b16_t *v_ab_mod)
+{
+ FAR struct foc_picontrol_b16_s *foc = NULL;
+ b16_t mag_max = 0;
+
+ DEBUGASSERT(h);
+ DEBUGASSERT(dq_ref);
+ DEBUGASSERT(v_ab_mod);
+
+ /* Get controller data */
+
+ DEBUGASSERT(h->control);
+ foc = h->control;
+
+ /* Get maximum possible voltage DQ vetor magnitude */
+
+ foc_vdq_mag_max_get_b16(&foc->data, &mag_max);
+
+ /* Saturate voltage DQ vector */
+
+#ifndef CONFIG_INDUSTRY_FOC_CORDIC
+ dq_saturate_b16(dq_ref, mag_max);
+#else
+ foc_cordic_dqsat_b16(h->fd, dq_ref, mag_max);
+#endif
+
+ /* Call FOC voltage controller */
+
+ foc_voltage_control_b16(&foc->data, dq_ref);
+
+ /* Get output v_ab_mod frame */
+
+ foc_vabmod_get_b16(&foc->data, v_ab_mod);
+}
+
+/****************************************************************************
+ * Name: foc_control_current_b16
+ *
+ * Description:
+ * Handle the FOC current control (fixed16)
+ *
+ * h - pointer to FOC handler
+ * dq_ref - DQ current reference frame
+ * vdq_comp - DQ voltage compensation
+ * v_ab_mod - (out) modulation alpha-veta voltage
+ *
+ ****************************************************************************/
+
+static void foc_control_current_run_b16(FAR foc_handler_b16_t *h,
+ FAR dq_frame_b16_t *dq_ref,
+ FAR dq_frame_b16_t *vdq_comp,
+ FAR ab_frame_b16_t *v_ab_mod)
+{
+ FAR struct foc_picontrol_b16_s *foc = NULL;
+ dq_frame_b16_t v_dq_ref;
+ b16_t mag_max = 0;
+
+ DEBUGASSERT(h);
+ DEBUGASSERT(dq_ref);
+ DEBUGASSERT(vdq_comp);
+ DEBUGASSERT(v_ab_mod);
+
+ /* Get controller data */
+
+ DEBUGASSERT(h->control);
+ foc = h->control;
+
+ /* Reset voltage reference */
+
+ v_dq_ref.d = 0;
+ v_dq_ref.q = 0;
+
+ /* Call FOC current controller */
+
+ foc_current_control_b16(&foc->data, dq_ref, vdq_comp, &v_dq_ref);
+
+ /* Get maximum possible voltage DQ vetor magnitude */
+
+ foc_vdq_mag_max_get_b16(&foc->data, &mag_max);
+
+ /* Saturate voltage DQ vector */
+
+#ifndef CONFIG_INDUSTRY_FOC_CORDIC
+ dq_saturate_b16(dq_ref, mag_max);
+#else
+ foc_cordic_dqsat_b16(h->fd, dq_ref, mag_max);
+#endif
+
+ /* Call FOC voltage control */
+
+ foc_voltage_control_b16(&foc->data, &v_dq_ref);
+
+ /* Get output v_ab_mod frame */
+
+ foc_vabmod_get_b16(&foc->data, v_ab_mod);
+}
+
+/****************************************************************************
+ * Name: foc_control_state_get_b16
+ *
+ * Description:
+ * Get the FOC controller state (fixed16)
+ *
+ * Input Parameter:
+ * h - pointer to FOC handler
+ * state - (out) pointer to FOC state data
+ *
+ ****************************************************************************/
+
+static void foc_control_state_get_b16(FAR foc_handler_b16_t *h,
+ FAR struct foc_state_b16_s *state)
+{
+ FAR struct foc_picontrol_b16_s *foc = NULL;
+
+ DEBUGASSERT(h);
+ DEBUGASSERT(state);
+
+ /* Get controller data */
+
+ DEBUGASSERT(h->control);
+ foc = h->control;
+
+ /* Copy DQ voltage */
+
+ state->vdq.q = foc->data.v_dq.q;
+ state->vdq.d = foc->data.v_dq.d;
+
+ /* Copy DQ current */
+
+ state->idq.q = foc->data.i_dq.q;
+ state->idq.d = foc->data.i_dq.d;
+
+ /* Copy alpha-beta current */
+
+ state->iab.a = foc->data.i_ab.a;
+ state->iab.b = foc->data.i_ab.b;
+
+ /* Copy alpha-beta voltage */
+
+ state->vab.a = foc->data.v_ab.a;
+ state->vab.b = foc->data.v_ab.b;
+
+ /* Copy phase current */
+
+ state->curr[0] = foc->data.i_abc.a;
+ state->curr[1] = foc->data.i_abc.b;
+ state->curr[2] = foc->data.i_abc.c;
+
+ /* Copy phase voltage */
+
+ state->volt[0] = foc->data.v_abc.a;
+ state->volt[1] = foc->data.v_abc.b;
+ state->volt[2] = foc->data.v_abc.c;
+}
diff --git a/industry/foc/fixed16/foc_ramp.c b/industry/foc/fixed16/foc_ramp.c
new file mode 100644
index 0000000..237ebd3
--- /dev/null
+++ b/industry/foc/fixed16/foc_ramp.c
@@ -0,0 +1,178 @@
+/****************************************************************************
+ * apps/industry/foc/fixed16/foc_ramp.c
+ * This file implements ramp 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 "industry/foc/foc_common.h"
+#include "industry/foc/fixed16/foc_ramp.h"
+
+/****************************************************************************
+ * Public Functions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Name: foc_ramp_init_b16
+ *
+ * Description:
+ * Initialize ramp (fixed16)
+ *
+ * Input Parameter:
+ * ramp - pointer to ramp handler
+ * per - ramp period
+ * thr - ramp threshold
+ * acc - ramp acceleration
+ * dec - ramp decceleration
+ *
+ ****************************************************************************/
+
+int foc_ramp_init_b16(FAR struct foc_ramp_b16_s *ramp, b16_t per,
+ b16_t thr, b16_t acc, b16_t dec)
+{
+ DEBUGASSERT(ramp);
+ DEBUGASSERT(per > 0);
+ DEBUGASSERT(thr > 0);
+ DEBUGASSERT(acc > 0);
+ DEBUGASSERT(dec > 0);
+
+ ramp->per = per;
+ ramp->ramp_thr = thr;
+ ramp->ramp_acc = acc;
+ ramp->ramp_dec = dec;
+ ramp->ramp_acc_per = b16mulb16(ramp->ramp_acc, ramp->per);
+ ramp->ramp_dec_per = b16mulb16(ramp->ramp_dec, ramp->per);
+
+ return OK;
+}
+
+/****************************************************************************
+ * Name: foc_ramp_run_b16
+ *
+ * Description:
+ * Handle ramp (fixed16)
+ *
+ * Input Parameter:
+ * ramp - pointer to ramp handler
+ * des - ramp parameter destination
+ * now - ramp parameter now
+ * set - (out) ramp parameter set
+ *
+ ****************************************************************************/
+
+int foc_ramp_run_b16(FAR struct foc_ramp_b16_s *ramp, b16_t des,
+ b16_t now, FAR b16_t *set)
+{
+ DEBUGASSERT(ramp);
+
+ /* Check if we require soft start/stop operation.
+ * Only if the user set point differs from the driver set point
+ */
+
+ if (des != *set)
+ {
+ ramp->diff = des - *set;
+
+ if (ramp->diff >= ramp->ramp_thr)
+ {
+ ramp->ramp_mode = RAMP_MODE_SOFTSTART;
+ }
+ else if (ramp->diff <= -ramp->ramp_thr)
+ {
+ ramp->ramp_mode = RAMP_MODE_SOFTSTOP;
+ }
+ else
+ {
+ /* Just set new setpoint */
+
+ *set = des;
+ ramp->ramp_mode = RAMP_MODE_NORMAL;
+ }
+ }
+ else
+ {
+ ramp->ramp_mode = RAMP_MODE_NORMAL;
+ }
+
+ /* Handle according to current motor state */
+
+ switch (ramp->ramp_mode)
+ {
+ case RAMP_MODE_NORMAL:
+ {
+ /* Nothing to do here ? */
+
+ break;
+ }
+
+ case RAMP_MODE_SOFTSTART:
+ {
+ if (des - *set >= ramp->ramp_thr)
+ {
+ /* Increase setpoint with ramp */
+
+ *set = now + ramp->ramp_acc_per;
+ }
+ else
+ {
+ /* Set final setpoint and exit soft start */
+
+ *set = des;
+ ramp->ramp_mode = RAMP_MODE_NORMAL;
+ }
+
+ break;
+ }
+
+ case RAMP_MODE_SOFTSTOP:
+ {
+ if (des - *set <= -ramp->ramp_thr)
+ {
+ /* Stop motor with ramp */
+
+ *set = now - ramp->ramp_dec_per;
+ }
+ else
+ {
+ /* Set final setpoint and exit soft stop */
+
+ *set = des;
+ ramp->ramp_mode = RAMP_MODE_NORMAL;
+ }
+
+ break;
+ }
+
+ default:
+ {
+ /* We should not be here */
+
+ DEBUGASSERT(0);
+ break;
+ }
+ }
+
+ return OK;
+}
diff --git a/industry/foc/fixed16/foc_svm3.c b/industry/foc/fixed16/foc_svm3.c
new file mode 100644
index 0000000..6d85c20
--- /dev/null
+++ b/industry/foc/fixed16/foc_svm3.c
@@ -0,0 +1,287 @@
+/****************************************************************************
+ * apps/industry/foc/fixed16/foc_svm3.c
+ * This file implements 3-phase space vector modulation 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/fixed16/foc_handler.h"
+
+/****************************************************************************
+ * Pre-processor Definitions
+ ****************************************************************************/
+
+#if CONFIG_MOTOR_FOC_PHASES != 3
+# error
+#endif
+
+/* Enable current samples correction if 3-shunts */
+
+#if CONFIG_MOTOR_FOC_SHUNTS == 3
+# define FOC_CORRECT_CURRENT_SAMPLES 1
+#endif
+
+/****************************************************************************
+ * Private Function Prototypes
+ ****************************************************************************/
+
+/* SVM3 data */
+
+struct foc_svm3mod_b16_s
+{
+ struct foc_mod_cfg_b16_s cfg;
+ struct svm3_state_b16_s state;
+};
+
+/****************************************************************************
+ * Private Function Prototypes
+ ****************************************************************************/
+
+static int foc_modulation_init_b16(FAR foc_handler_b16_t *h);
+static void foc_modulation_deinit_b16(FAR foc_handler_b16_t *h);
+static void foc_modulation_cfg_b16(FAR foc_handler_b16_t *h, FAR void *cfg);
+static void foc_modulation_current_b16(FAR foc_handler_b16_t *h,
+ FAR b16_t *curr);
+static void foc_modulation_vbase_get_b16(FAR foc_handler_b16_t *h,
+ b16_t vbus,
+ FAR b16_t *vbase);
+static void foc_modulation_run_b16(FAR foc_handler_b16_t *h,
+ FAR ab_frame_b16_t *v_ab_mod,
+ FAR b16_t *duty);
+
+/****************************************************************************
+ * Public Data
+ ****************************************************************************/
+
+/* FOC modulation fixed16 interface */
+
+struct foc_modulation_ops_b16_s g_foc_mod_svm3_b16 =
+{
+ .init = foc_modulation_init_b16,
+ .deinit = foc_modulation_deinit_b16,
+ .cfg = foc_modulation_cfg_b16,
+ .current = foc_modulation_current_b16,
+ .vbase_get = foc_modulation_vbase_get_b16,
+ .run = foc_modulation_run_b16,
+};
+
+/****************************************************************************
+ * Private Functions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Name: foc_modulation_init_b16
+ *
+ * Description:
+ * Initialize the SVM3 modulation (fixed16)
+ *
+ * Input Parameter:
+ * h - pointer to FOC handler
+ *
+ ****************************************************************************/
+
+static int foc_modulation_init_b16(FAR foc_handler_b16_t *h)
+{
+ int ret = OK;
+
+ DEBUGASSERT(h);
+
+ /* Connect modulation data */
+
+ h->modulation = zalloc(sizeof(struct foc_svm3mod_b16_s));
+ if (h->modulation == NULL)
+ {
+ ret = -ENOMEM;
+ goto errout;
+ }
+
+ errout:
+ return ret;
+}
+
+/****************************************************************************
+ * Name: foc_modulation_deinit_b16
+ *
+ * Description:
+ * Deinitialize the SVM3 modulation (fixed16)
+ *
+ * Input Parameter:
+ * h - pointer to FOC handler
+ *
+ ****************************************************************************/
+
+static void foc_modulation_deinit_b16(FAR foc_handler_b16_t *h)
+{
+ DEBUGASSERT(h);
+
+ /* Free modulation data */
+
+ if (h->modulation)
+ {
+ free(h->modulation);
+ }
+}
+
+/****************************************************************************
+ * Name: foc_modulation_cfg_b16
+ *
+ * Description:
+ * Configure the SVM3 modulation (fixed16)
+ *
+ * Input Parameter:
+ * h - pointer to FOC handler
+ * cfg - pointer to modulation configuration data
+ * (struct foc_svm3mod_b16_s)
+ *
+ ****************************************************************************/
+
+static void foc_modulation_cfg_b16(FAR foc_handler_b16_t *h, FAR void *cfg)
+{
+ FAR struct foc_svm3mod_b16_s *svm = NULL;
+
+ DEBUGASSERT(h);
+ DEBUGASSERT(cfg);
+
+ /* Get modulation data */
+
+ DEBUGASSERT(h->modulation);
+ svm = h->modulation;
+
+ /* Copy data */
+
+ memcpy(&svm->cfg, cfg, sizeof(struct foc_mod_cfg_b16_s));
+
+ /* Initialize SVM3 data */
+
+ svm3_init_b16(&svm->state);
+}
+
+/****************************************************************************
+ * Name: foc_modulation_vbase_get_b16
+ *
+ * Description:
+ * Get the modulation base voltage (fixed16)
+ *
+ * Input Parameter:
+ * h - pointer to FOC handler
+ * vbus - bus voltage
+ * vbase - (out) pointer to base voltage from modulation
+ *
+ ****************************************************************************/
+
+static void foc_modulation_vbase_get_b16(FAR foc_handler_b16_t *h,
+ b16_t vbus,
+ FAR b16_t *vbase)
+{
+ DEBUGASSERT(h);
+ DEBUGASSERT(vbus);
+ DEBUGASSERT(vbase);
+
+ /* Get maximum possible base voltage */
+
+ *vbase = SVM3_BASE_VOLTAGE_GET_B16(vbus);
+}
+
+/****************************************************************************
+ * Name: foc_modulation_current_b16
+ *
+ * Description:
+ * Correct current samples accrding to the SVM3 modulation state (fixed16)
+ *
+ * Input Parameter:
+ * h - pointer to FOC handler
+ * curr - (in/out) phase currents
+ *
+ ****************************************************************************/
+
+static void foc_modulation_current_b16(FAR foc_handler_b16_t *h,
+ FAR b16_t *curr)
+{
+#ifdef FOC_CORRECT_CURRENT_SAMPLES
+ FAR struct foc_svm3mod_b16_s *svm = NULL;
+
+ DEBUGASSERT(h);
+ DEBUGASSERT(curr);
+
+ /* Get modulation data */
+
+ DEBUGASSERT(h->modulation);
+ svm = h->modulation;
+
+ /* Correct ADC current samples */
+
+ svm3_current_correct_b16(&svm->state,
+ &curr[0],
+ &curr[1],
+ &curr[2]);
+#endif
+}
+
+/****************************************************************************
+ * Name: foc_modulation_b16
+ *
+ * Description:
+ * Handle the SVM3 modulation (fixed16)
+ *
+ * Input Parameter:
+ * h - pointer to FOC handler
+ * v_ab_mod - requested modulation voltage scaled to 0.0-1.0 range
+ * duty - (out) duty cycle for switches
+ *
+ ****************************************************************************/
+
+static void foc_modulation_run_b16(FAR foc_handler_b16_t *h,
+ FAR ab_frame_b16_t *v_ab_mod,
+ FAR b16_t *duty)
+{
+ FAR struct foc_svm3mod_b16_s *svm = NULL;
+
+ DEBUGASSERT(h);
+ DEBUGASSERT(v_ab_mod);
+ DEBUGASSERT(duty);
+
+ /* Get modulation data */
+
+ DEBUGASSERT(h->modulation);
+ svm = h->modulation;
+
+ /* Call 3-phase space vector modulation */
+
+ svm3_b16(&svm->state, v_ab_mod);
+
+ /* Copy duty cycle */
+
+ duty[0] = svm->state.d_u;
+ duty[1] = svm->state.d_v;
+ duty[2] = svm->state.d_w;
+
+ /* Saturate duty cycle */
+
+ f_saturate_b16(&duty[0], 0, svm->cfg.pwm_duty_max);
+ f_saturate_b16(&duty[1], 0, svm->cfg.pwm_duty_max);
+ f_saturate_b16(&duty[2], 0, svm->cfg.pwm_duty_max);
+}
diff --git a/industry/foc/fixed16/foc_velocity.c b/industry/foc/fixed16/foc_velocity.c
new file mode 100644
index 0000000..005f924
--- /dev/null
+++ b/industry/foc/fixed16/foc_velocity.c
@@ -0,0 +1,157 @@
+/****************************************************************************
+ * apps/industry/foc/fixed16/foc_velocity.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 <errno.h>
+#include <errno.h>
+
+#include "industry/foc/foc_log.h"
+#include "industry/foc/fixed16/foc_velocity.h"
+
+/****************************************************************************
+ * Public Functions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Name: foc_velocity_init_b16
+ *
+ * Description:
+ * Initialize the FOC velocity handler (fixed16)
+ *
+ * Input Parameter:
+ * h - pointer to FOC velocity handler
+ * ops - pointer to FOC velocity handler operations
+ *
+ ****************************************************************************/
+
+int foc_velocity_init_b16(FAR foc_velocity_b16_t *h,
+ FAR struct foc_velocity_ops_b16_s *ops)
+{
+ int ret = OK;
+
+ DEBUGASSERT(h);
+ DEBUGASSERT(ops);
+
+ /* Velocity ops */
+
+ DEBUGASSERT(ops->init);
+ DEBUGASSERT(ops->deinit);
+ DEBUGASSERT(ops->cfg);
+ DEBUGASSERT(ops->run);
+
+ /* Reset handler */
+
+ memset(h, 0, sizeof(foc_velocity_b16_t));
+
+ /* Connect ops */
+
+ h->ops = ops;
+
+ /* Initialize velocity */
+
+ ret = h->ops->init(h);
+ if (ret < 0)
+ {
+ FOCLIBERR("ERROR: ops->init failed %d\n", ret);
+ goto errout;
+ }
+
+errout:
+ return ret;
+}
+
+/****************************************************************************
+ * Name: foc_velocity_deinit_b16
+ *
+ * Description:
+ * De-initialize the FOC velocity handler (fixed16)
+ *
+ * Input Parameter:
+ * h - pointer to FOC velocity handler
+ *
+ ****************************************************************************/
+
+int foc_velocity_deinit_b16(FAR foc_velocity_b16_t *h)
+{
+ int ret = OK;
+
+ DEBUGASSERT(h);
+
+ /* Deinitialize velocity */
+
+ h->ops->deinit(h);
+
+ /* Reset data */
+
+ memset(h, 0, sizeof(foc_velocity_b16_t));
+
+ return ret;
+}
+
+/****************************************************************************
+ * Name: foc_velocity_cfg_b16
+ *
+ * Description:
+ * Configure the FOC velocity handler (fixed16)
+ *
+ * Input Parameter:
+ * h - pointer to FOC velocity handler
+ * cfg - pointer to velocity handler configuration data
+ *
+ ****************************************************************************/
+
+int foc_velocity_cfg_b16(FAR foc_velocity_b16_t *h, FAR void *cfg)
+{
+ DEBUGASSERT(h);
+ DEBUGASSERT(cfg);
+
+ return h->ops->cfg(h, cfg);
+}
+
+/****************************************************************************
+ * Name: foc_velocity_run_b16
+ *
+ * Description:
+ * Process the FOC velocity handler data (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
+ *
+ ****************************************************************************/
+
+void foc_velocity_run_b16(FAR foc_velocity_b16_t *h,
+ FAR struct foc_velocity_in_b16_s *in,
+ FAR struct foc_velocity_out_b16_s *out)
+{
+ DEBUGASSERT(h);
+ DEBUGASSERT(in);
+ DEBUGASSERT(out);
+
+ /* Run velocity handler */
+
+ h->ops->run(h, in, out);
+}
diff --git a/industry/foc/float/foc_angle.c b/industry/foc/float/foc_angle.c
new file mode 100644
index 0000000..8673e78
--- /dev/null
+++ b/industry/foc/float/foc_angle.c
@@ -0,0 +1,157 @@
+/****************************************************************************
+ * apps/industry/foc/float/foc_angle.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 "industry/foc/foc_log.h"
+#include "industry/foc/float/foc_angle.h"
+
+/****************************************************************************
+ * Public Functions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Name: foc_angle_init_f32
+ *
+ * Description:
+ * Initialize the FOC angle handler (float32)
+ *
+ * Input Parameter:
+ * h - pointer to FOC angle handler
+ * ops - pointer to FOC angle handler operations
+ *
+ ****************************************************************************/
+
+int foc_angle_init_f32(FAR foc_angle_f32_t *h,
+ FAR struct foc_angle_ops_f32_s *ops)
+{
+ int ret = OK;
+
+ DEBUGASSERT(h);
+ DEBUGASSERT(ops);
+
+ /* Angle ops */
+
+ DEBUGASSERT(ops->init);
+ DEBUGASSERT(ops->deinit);
+ DEBUGASSERT(ops->cfg);
+ DEBUGASSERT(ops->run);
+
+ /* Reset handler */
+
+ memset(h, 0, sizeof(foc_angle_f32_t));
+
+ /* Connect ops */
+
+ h->ops = ops;
+
+ /* Initialize angle */
+
+ ret = h->ops->init(h);
+ if (ret < 0)
+ {
+ FOCLIBERR("ERROR: ops->init failed %d\n", ret);
+ goto errout;
+ }
+
+errout:
+ return ret;
+}
+
+/****************************************************************************
+ * Name: foc_angle_deinit_f32
+ *
+ * Description:
+ * De-initialize the FOC angle handler (float32)
+ *
+ * Input Parameter:
+ * h - pointer to FOC angle handler
+ *
+ ****************************************************************************/
+
+int foc_angle_deinit_f32(FAR foc_angle_f32_t *h)
+{
+ int ret = OK;
+
+ DEBUGASSERT(h);
+
+ /* Deinitialize angle */
+
+ h->ops->deinit(h);
+
+ /* Reset data */
+
+ memset(h, 0, sizeof(foc_angle_f32_t));
+
+ return ret;
+}
+
+/****************************************************************************
+ * Name: foc_angle_cfg_f32
+ *
+ * Description:
+ * Configure the FOC angle handler (float32)
+ *
+ * Input Parameter:
+ * h - pointer to FOC angle handler
+ * cfg - pointer to angle handler configuration data
+ *
+ ****************************************************************************/
+
+int foc_angle_cfg_f32(FAR foc_angle_f32_t *h, FAR void *cfg)
+{
+ DEBUGASSERT(h);
+ DEBUGASSERT(cfg);
+
+ return h->ops->cfg(h, cfg);
+}
+
+/****************************************************************************
+ * Name: foc_angle_run_f32
+ *
+ * Description:
+ * Process the FOC angle handler data (float32)
+ *
+ * Input Parameter:
+ * h - pointer to FOC angle handler
+ * in - pointer to FOC angle handler input data
+ * out - pointer to FOC angle handler output data
+ *
+ ****************************************************************************/
+
+void foc_angle_run_f32(FAR foc_angle_f32_t *h,
+ FAR struct foc_angle_in_f32_s *in,
+ FAR struct foc_angle_out_f32_s *out)
+{
+ DEBUGASSERT(h);
+ DEBUGASSERT(in);
+ DEBUGASSERT(out);
+
+ /* Run angle handler */
+
+ h->ops->run(h, in, out);
+}
diff --git a/industry/foc/float/foc_cordic.c b/industry/foc/float/foc_cordic.c
new file mode 100644
index 0000000..271e7f2
--- /dev/null
+++ b/industry/foc/float/foc_cordic.c
@@ -0,0 +1,81 @@
+/****************************************************************************
+ * apps/industry/foc/float/foc_cordic.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 "industry/foc/float/foc_cordic.h"
+
+/****************************************************************************
+ * Public Functions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Name: foc_cordic_dqsat_f32
+ *
+ * Description:
+ * CORDIC DQ-frame saturation (float32)
+ *
+ * Input Parameter:
+ * fd - the file descriptor for CORDIC device
+ * dq_ref - DQ vector
+ * mag_max - vector magnitude max
+ *
+ ****************************************************************************/
+
+int foc_cordic_dqsat_f32(int fd, FAR dq_frame_f32_t *dq, float mag_max)
+{
+ DEBUGASSERT(dq);
+
+ /* TODO: */
+
+ ASSERT(0);
+
+ return OK;
+}
+
+/****************************************************************************
+ * Name: foc_cordic_angle_f32
+ *
+ * Description:
+ * CORDIC angle update (float32)
+ *
+ * Input Parameter:
+ * fd - the file descriptor for CORDIC device
+ * angle - phase angle data
+ * a - phase angle in rad
+ *
+ ****************************************************************************/
+
+int foc_cordic_angle_f32(int fd, FAR phase_angle_f32_t *angle, float a)
+{
+ DEBUGASSERT(angle);
+
+ /* TODO: */
+
+ ASSERT(0);
+
+ return OK;
+}
diff --git a/industry/foc/float/foc_handler.c b/industry/foc/float/foc_handler.c
new file mode 100644
index 0000000..c07c0b5
--- /dev/null
+++ b/industry/foc/float/foc_handler.c
@@ -0,0 +1,345 @@
+/****************************************************************************
+ * apps/industry/foc/float/foc_handler.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 "industry/foc/foc_log.h"
+#include "industry/foc/foc_common.h"
+#include "industry/foc/float/foc_handler.h"
+
+/****************************************************************************
+ * Public Functions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Name: foc_handler_init_f32
+ *
+ * Description:
+ * Initialize the FOC handler (float32)
+ *
+ * Input Parameter:
+ * h - pointer to FOC handler
+ * ctrl - pointer to controller operations
+ * mod - pointer to modulation operations
+ *
+ ****************************************************************************/
+
+int foc_handler_init_f32(FAR foc_handler_f32_t *h,
+ FAR struct foc_control_ops_f32_s *ctrl,
+ FAR struct foc_modulation_ops_f32_s *mod)
+{
+ int ret = OK;
+
+ DEBUGASSERT(h);
+ DEBUGASSERT(ctrl);
+ DEBUGASSERT(mod);
+
+ /* Controller ops */
+
+ DEBUGASSERT(ctrl->init);
+ DEBUGASSERT(ctrl->deinit);
+ DEBUGASSERT(ctrl->cfg);
+ DEBUGASSERT(ctrl->input_set);
+ DEBUGASSERT(ctrl->voltage_run);
+ DEBUGASSERT(ctrl->current_run);
+ DEBUGASSERT(ctrl->state_get);
+
+ /* Modulation ops */
+
+ DEBUGASSERT(mod->init);
+ DEBUGASSERT(mod->deinit);
+ DEBUGASSERT(mod->cfg);
+ DEBUGASSERT(mod->current);
+ DEBUGASSERT(mod->vbase_get);
+ DEBUGASSERT(mod->run);
+
+ /* Reset handler */
+
+ memset(h, 0, sizeof(foc_handler_f32_t));
+
+ /* Connect ops */
+
+ h->ops.ctrl = ctrl;
+ h->ops.mod = mod;
+
+ /* Initialize control handler */
+
+ ret = h->ops.mod->init(h);
+ if (ret < 0)
+ {
+ FOCLIBERR("ERROR: mod->init failed %d\n", ret);
+ goto errout;
+ }
+
+ /* Initialize modulation handler */
+
+ ret = h->ops.ctrl->init(h);
+ if (ret < 0)
+ {
+ FOCLIBERR("ERROR: ctrl->init failed %d\n", ret);
+ goto errout;
+ }
+
+errout:
+ return ret;
+}
+
+/****************************************************************************
+ * Name: foc_handler_deinit_f32
+ *
+ * Description:
+ * De-initialize the FOC handler (float32)
+ *
+ * Input Parameter:
+ * h - pointer to FOC handler
+ *
+ ****************************************************************************/
+
+int foc_handler_deinit_f32(FAR foc_handler_f32_t *h)
+{
+ int ret = OK;
+
+ DEBUGASSERT(h);
+
+ /* Deinitialize modulation handler */
+
+ h->ops.mod->deinit(h);
+
+ /* Deinitialize control handler */
+
+ h->ops.ctrl->deinit(h);
+
+ /* Reset data */
+
+ memset(h, 0, sizeof(foc_handler_f32_t));
+
+ return ret;
+}
+
+/****************************************************************************
+ * Name: foc_handler_cfg_f32
+ *
+ * Description:
+ * Configure the FOC handler (float32)
+ *
+ * Input Parameter:
+ * h - pointer to FOC handler
+ * ctrl_cfg - pointer to controller configuration data
+ * mod_cfg - pointer to modulation configuration data
+ *
+ ****************************************************************************/
+
+void foc_handler_cfg_f32(FAR foc_handler_f32_t *h,
+ FAR void *ctrl_cfg,
+ FAR void *mod_cfg)
+{
+ DEBUGASSERT(h);
+ DEBUGASSERT(ctrl_cfg);
+ DEBUGASSERT(mod_cfg);
+
+ /* Configure controller */
+
+ h->ops.ctrl->cfg(h, ctrl_cfg);
+
+ /* Configure modulation */
+
+ h->ops.mod->cfg(h, mod_cfg);
+}
+
+/****************************************************************************
+ * Name: foc_handler_run_f32
+ *
+ * Description:
+ * Run the FOC handler and process the given input data (float32)
+ *
+ * Input Parameter:
+ * h - pointer to FOC handler
+ * in - pointer to FOC handler input data
+ * out - pointer to FOC handler output data
+ *
+ ****************************************************************************/
+
+int foc_handler_run_f32(FAR foc_handler_f32_t *h,
+ FAR struct foc_handler_input_f32_s *in,
+ FAR struct foc_handler_output_f32_s *out)
+{
+ ab_frame_f32_t v_ab_mod;
+ float vbase = 0;
+ int ret = OK;
+
+ DEBUGASSERT(h);
+ DEBUGASSERT(in);
+ DEBUGASSERT(out);
+
+ /* Do nothing if control mode not specified yet.
+ * This also protects against initial state when the controller is
+ * started but input data has not yet been provided.
+ */
+
+ if (in->mode <= FOC_HANDLER_MODE_INIT)
+ {
+ ret = -EINVAL;
+ goto errout;
+ }
+
+ /* Correct current samples according to modulation state */
+
+ h->ops.mod->current(h, in->current);
+
+ /* Get VBASE */
+
+ h->ops.mod->vbase_get(h, in->vbus, &vbase);
+
+ /* Feed controller with phase currents */
+
+ h->ops.ctrl->input_set(h, in->current, vbase, in->angle);
+
+ /* Call controller */
+
+ switch (in->mode)
+ {
+ /* IDLE */
+
+ case FOC_HANDLER_MODE_IDLE:
+ {
+ /* Do nothing and set duty to zeros */
+
+ ret = OK;
+ goto errout;
+ }
+
+ /* FOC current mode - control DQ-current */
+
+ case FOC_HANDLER_MODE_CURRENT:
+ {
+ /* Current controller */
+
+ h->ops.ctrl->current_run(h,
+ in->dq_ref,
+ in->vdq_comp,
+ &v_ab_mod);
+
+ break;
+ }
+
+ /* FOC voltage mode - control DQ-voltage */
+
+ case FOC_HANDLER_MODE_VOLTAGE:
+ {
+ /* Voltage controller */
+
+ h->ops.ctrl->voltage_run(h,
+ in->dq_ref,
+ &v_ab_mod);
+
+ break;
+ }
+
+ /* Otherwise - we should not be here */
+
+ default:
+ {
+ ret = -EINVAL;
+ goto errout;
+ }
+ }
+
+ /* Duty cycle modulation */
+
+ h->ops.mod->run(h, &v_ab_mod, out->duty);
+
+ return ret;
+
+errout:
+
+ /* Set duty to zeros */
+
+ memset(out->duty, 0, sizeof(float) * CONFIG_MOTOR_FOC_PHASES);
+
+ return ret;
+}
+
+/****************************************************************************
+ * Name: foc_handler_state_f32
+ *
+ * Description:
+ * Get FOC handler state (float32)
+ *
+ * Input Parameter:
+ * h - pointer to FOC handler
+ * state - pointer to FOC state data
+ *
+ ****************************************************************************/
+
+void foc_handler_state_f32(FAR foc_handler_f32_t *h,
+ FAR struct foc_state_f32_s *state)
+{
+ DEBUGASSERT(h);
+ DEBUGASSERT(state);
+
+ h->ops.ctrl->state_get(h, state);
+}
+
+#ifdef CONFIG_INDUSTRY_FOC_HANDLER_PRINT
+/****************************************************************************
+ * Name: foc_handler_state_print_f32
+ *
+ * Description:
+ * Print FOC handler state (float32)
+ *
+ * Input Parameter:
+ * state - pointer to FOC state data
+ *
+ ****************************************************************************/
+
+void foc_handler_state_print_f32(FAR struct foc_state_f32_s *state)
+{
+ DEBUGASSERT(state);
+
+#if CONFIG_MOTOR_FOC_PHASES == 3
+ FOCLIBLOG("curr = [%.2f %.2f %.2f]\n",
+ state->curr[0],
+ state->curr[1],
+ state->curr[2]);
+ FOCLIBLOG("volt = [%.2f %.2f %.2f]\n",
+ state->volt[0],
+ state->volt[1],
+ state->volt[2]);
+#else
+# error TODO
+#endif
+
+ FOCLIBLOG("iab = [%.2f %.2f]\n", state->iab.a,
+ state->iab.b);
+ FOCLIBLOG("vab = [%.2f %.2f]\n", state->vab.a,
+ state->vab.b);
+ FOCLIBLOG("idq = [%.2f %.2f]\n", state->idq.d,
+ state->idq.q);
+ FOCLIBLOG("vdq = [%.2f %.2f]\n", state->vdq.d,
+ state->vdq.q);
+}
+#endif
diff --git a/industry/foc/float/foc_model.c b/industry/foc/float/foc_model.c
new file mode 100644
index 0000000..d46e0b4
--- /dev/null
+++ b/industry/foc/float/foc_model.c
@@ -0,0 +1,188 @@
+/****************************************************************************
+ * apps/industry/foc/float/foc_model.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 "industry/foc/foc_log.h"
+#include "industry/foc/foc_common.h"
+#include "industry/foc/float/foc_model.h"
+
+/****************************************************************************
+ * Public Functions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Name: foc_model_init_f32
+ *
+ * Description:
+ * Initialize the FOC model (float32)
+ *
+ * Input Parameter:
+ * h - pointer to FOC model handler
+ * ops - pointer to FOC model operations
+ *
+ ****************************************************************************/
+
+int foc_model_init_f32(FAR foc_model_f32_t *h,
+ FAR struct foc_model_ops_f32_s *ops)
+{
+ int ret = OK;
+
+ DEBUGASSERT(h);
+ DEBUGASSERT(ops);
+
+ /* Model ops */
+
+ DEBUGASSERT(ops->init);
+ DEBUGASSERT(ops->deinit);
+ DEBUGASSERT(ops->cfg);
+ DEBUGASSERT(ops->ele_run);
+ DEBUGASSERT(ops->mech_run);
+ DEBUGASSERT(ops->state);
+
+ /* Reset handler */
+
+ memset(h, 0, sizeof(foc_model_f32_t));
+
+ /* Connect ops */
+
+ h->ops = ops;
+
+ /* Initialize model */
+
+ ret = h->ops->init(h);
+ if (ret < 0)
+ {
+ FOCLIBERR("ERROR: ops->init failed %d\n", ret);
+ goto errout;
+ }
+
+errout:
+ return ret;
+}
+
+/****************************************************************************
+ * Name: foc_model_deinit_f32
+ *
+ * Description:
+ * De-initialize the FOC model (float32)
+ *
+ * Input Parameter:
+ * h - pointer to FOC model handler
+ *
+ ****************************************************************************/
+
+int foc_model_deinit_f32(FAR foc_model_f32_t *h)
+{
+ int ret = OK;
+
+ DEBUGASSERT(h);
+
+ /* Deinitialize model */
+
+ h->ops->deinit(h);
+
+ /* Reset data */
+
+ memset(h, 0, sizeof(foc_model_f32_t));
+
+ return ret;
+}
+
+/****************************************************************************
+ * Name: foc_model_cfg_f32
+ *
+ * Description:
+ * Configure the FOC model (float32)
+ *
+ * Input Parameter:
+ * h - pointer to FOC model handler
+ * cfg - pointer to FOC model configuration data
+ *
+ ****************************************************************************/
+
+int foc_model_cfg_f32(FAR foc_model_f32_t *h, FAR void *cfg)
+{
+ DEBUGASSERT(h);
+ DEBUGASSERT(cfg);
+
+ return h->ops->cfg(h, cfg);
+}
+
+/****************************************************************************
+ * Name: foc_model_run_f32
+ *
+ * Description:
+ * Run the FOC model (float32)
+ *
+ * Input Parameter:
+ * h - pointer to FOC model handler
+ * load - applied load
+ * v_ab - applied voltage in alpha-beta frame
+ *
+ * REVISIT:
+ * It would be better if we feed model with duty cycle and VBUS, but it
+ * complicates the calculations, so we leave it like that for now
+ *
+ ****************************************************************************/
+
+void foc_model_run_f32(FAR foc_model_f32_t *h,
+ float load,
+ FAR ab_frame_f32_t *v_ab)
+{
+ DEBUGASSERT(h);
+ DEBUGASSERT(v_ab);
+
+ /* Run electrical model */
+
+ h->ops->ele_run(h, v_ab);
+
+ /* Run mechanical model */
+
+ h->ops->mech_run(h, load);
+}
+
+/****************************************************************************
+ * Name: foc_model_state_f32
+ *
+ * Description:
+ * Get model state (float32)
+ *
+ * Input Parameter:
+ * h - pointer to FOC model handler
+ * state - pointer to FOC model state
+ *
+ ****************************************************************************/
+
+void foc_model_state_f32(FAR foc_model_f32_t *h,
+ FAR struct foc_model_state_f32_s *state)
+{
+ DEBUGASSERT(h);
+ DEBUGASSERT(state);
+
+ h->ops->state(h, state);
+}
diff --git a/industry/foc/float/foc_model_pmsm.c b/industry/foc/float/foc_model_pmsm.c
new file mode 100644
index 0000000..cca7b76
--- /dev/null
+++ b/industry/foc/float/foc_model_pmsm.c
@@ -0,0 +1,297 @@
+/****************************************************************************
+ * apps/industry/foc/float/foc_model_pmsm.c
+ * This file implements PMSM model 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/float/foc_model.h"
+
+/****************************************************************************
+ * Pre-processor Definitions
+ ****************************************************************************/
+
+#if CONFIG_MOTOR_FOC_PHASES != 3
+# error
+#endif
+
+/****************************************************************************
+ * Private Function Prototypes
+ ****************************************************************************/
+
+/* PMSM model data */
+
+struct foc_model_pmsm_f32_s
+{
+ float one_by_iphadc;
+ struct foc_model_pmsm_cfg_f32_s cfg;
+ struct pmsm_model_f32_s model;
+};
+
+/****************************************************************************
+ * Private Function Prototypes
+ ****************************************************************************/
+
+static int foc_model_pmsm_init_f32(FAR foc_model_f32_t *h);
+static void foc_model_pmsm_deinit_f32(FAR foc_model_f32_t *h);
+static int foc_model_pmsm_cfg_f32(FAR foc_model_f32_t *h, FAR void *cfg);
+static void foc_model_pmsm_elerun_f32(FAR foc_model_f32_t *h,
+ FAR ab_frame_f32_t *v_ab);
+static void foc_model_pmsm_mechrun_f32(FAR foc_model_f32_t *h, float load);
+static void foc_model_pmsm_state_f32(FAR foc_model_f32_t *h,
+ FAR struct foc_model_state_f32_s *state);
+
+/****************************************************************************
+ * Public Data
+ ****************************************************************************/
+
+/* FOC model float interface */
+
+struct foc_model_ops_f32_s g_foc_model_pmsm_ops_f32 =
+{
+ .init = foc_model_pmsm_init_f32,
+ .deinit = foc_model_pmsm_deinit_f32,
+ .cfg = foc_model_pmsm_cfg_f32,
+ .ele_run = foc_model_pmsm_elerun_f32,
+ .mech_run = foc_model_pmsm_mechrun_f32,
+ .state = foc_model_pmsm_state_f32,
+};
+
+/****************************************************************************
+ * Private Functions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Name: foc_model_pmsm_init_f32
+ *
+ * Description:
+ * Initialize PMSM model (float32)
+ *
+ * Input Parameter:
+ * h - pointer to FOC model handler
+ *
+ ****************************************************************************/
+
+static int foc_model_pmsm_init_f32(FAR foc_model_f32_t *h)
+{
+ int ret = OK;
+
+ DEBUGASSERT(h);
+
+ /* Connect model data */
+
+ h->model = zalloc(sizeof(struct foc_model_pmsm_f32_s));
+ if (h->model == NULL)
+ {
+ ret = -ENOMEM;
+ goto errout;
+ }
+
+errout:
+ return ret;
+}
+
+/****************************************************************************
+ * Name: foc_model_pmsm_deinit_f32
+ *
+ * Description:
+ * Deinitialize PMSM model (float32)
+ *
+ * Input Parameter:
+ * h - pointer to FOC model handler
+ *
+ ****************************************************************************/
+
+static void foc_model_pmsm_deinit_f32(FAR foc_model_f32_t *h)
+{
+ DEBUGASSERT(h);
+
+ /* Free model data */
+
+ if (h->model)
+ {
+ free (h->model);
+ }
+}
+
+/****************************************************************************
+ * Name: foc_model_pmsm_cfg_f32
+ *
+ * Description:
+ * Configure PMSM model (float32)
+ *
+ * Input Parameter:
+ * h - pointer to FOC model handler
+ * cfg - pointer to FOC model configuration (struct foc_model_pmsm_f32_s)
+ *
+ ****************************************************************************/
+
+static int foc_model_pmsm_cfg_f32(FAR foc_model_f32_t *h, FAR void *cfg)
+{
+ FAR struct foc_model_pmsm_f32_s *model = NULL;
+ struct pmsm_phy_params_f32_s phy;
+
+ DEBUGASSERT(h);
+ DEBUGASSERT(cfg);
+
+ /* Get model data */
+
+ DEBUGASSERT(h->model);
+ model = h->model;
+
+ /* Copy data */
+
+ memcpy(&model->cfg, cfg, sizeof(struct foc_model_pmsm_cfg_f32_s));
+
+ /* Initialize model */
+
+ pmsm_phy_params_init(&phy,
+ model->cfg.poles,
+ model->cfg.res,
+ model->cfg.ind,
+ model->cfg.iner,
+ model->cfg.flux_link,
+ model->cfg.ind_q,
+ model->cfg.ind_d);
+
+ pmsm_model_initialize(&model->model, &phy, model->cfg.per);
+
+ /* Get one by iphase_adc */
+
+ model->one_by_iphadc = (1.0f / model->cfg.iphase_adc);
+
+ return OK;
+}
+
+/****************************************************************************
+ * Name: foc_model_pmsm_elerun_f32
+ *
+ * Description:
+ * Run model electrical simulation (float32)
+ *
+ * Input Parameter:
+ * h - pointer to FOC model handler
+ * v_ab - applied voltage in alpha-beta frame
+ *
+ ****************************************************************************/
+
+static void foc_model_pmsm_elerun_f32(FAR foc_model_f32_t *h,
+ FAR ab_frame_f32_t *v_ab)
+{
+ FAR struct foc_model_pmsm_f32_s *model = NULL;
+
+ DEBUGASSERT(h);
+ DEBUGASSERT(v_ab);
+
+ /* Get model data */
+
+ DEBUGASSERT(h->model);
+ model = h->model;
+
+ /* Run electrical model */
+
+ pmsm_model_elec(&model->model, v_ab);
+}
+
+/****************************************************************************
+ * Name: foc_model_pmsm_mechrun_f32
+ *
+ * Description:
+ * Run model mechanical simulation (float32)
+ *
+ * Input Parameter:
+ * h - pointer to FOC model handler
+ * load - applied load
+ *
+ ****************************************************************************/
+
+static void foc_model_pmsm_mechrun_f32(FAR foc_model_f32_t *h, float load)
+{
+ FAR struct foc_model_pmsm_f32_s *model = NULL;
+
+ DEBUGASSERT(h);
+
+ /* Get model data */
+
+ DEBUGASSERT(h->model);
+ model = h->model;
+
+ /* Run mechanical model */
+
+ pmsm_model_mech(&model->model, load);
+}
+
+/****************************************************************************
+ * Name: foc_model_pmsm_state_f32
+ *
+ * Description:
+ * Get the model state (float32)
+ *
+ * Input Parameter:
+ * h - pointer to FOC model handler
+ * state - pointer to FOC model state
+ *
+ ****************************************************************************/
+
+static void foc_model_pmsm_state_f32(FAR foc_model_f32_t *h,
+ FAR struct foc_model_state_f32_s *state)
+{
+ FAR struct foc_model_pmsm_f32_s *model = NULL;
+
+ DEBUGASSERT(h);
+ DEBUGASSERT(state);
+
+ /* Get model data */
+
+ DEBUGASSERT(h->model);
+ model = h->model;
+
+ /* Get model state */
+
+ state->volt[0] = model->model.state.v_abc.a;
+ state->volt[1] = model->model.state.v_abc.b;
+ state->volt[2] = model->model.state.v_abc.c;
+ state->vab.a = model->model.state.v_ab.a;
+ state->vab.b = model->model.state.v_ab.b;
+ state->vdq.d = model->model.state.v_dq.d;
+ state->vdq.q = model->model.state.v_dq.q;
+ state->curr[0] = model->model.state.i_abc.a;
+ state->curr[1] = model->model.state.i_abc.b;
+ state->curr[2] = model->model.state.i_abc.c;
+ state->iab.a = model->model.state.i_ab.a;
+ state->iab.b = model->model.state.i_ab.b;
+ state->idq.d = model->model.state.i_dq.d;
+ state->idq.q = model->model.state.i_dq.q;
+ state->omega_e = model->model.state.omega_e;
+ state->omega_m = model->model.state.omega_m;
+
+ /* Get RAW currents */
+
+ state->curr_raw[0] = (int32_t)(state->curr[0] * model->one_by_iphadc);
+ state->curr_raw[1] = (int32_t)(state->curr[1] * model->one_by_iphadc);
+ state->curr_raw[2] = (int32_t)(state->curr[2] * model->one_by_iphadc);
+}
diff --git a/industry/foc/float/foc_openloop.c b/industry/foc/float/foc_openloop.c
new file mode 100644
index 0000000..365b0be
--- /dev/null
+++ b/industry/foc/float/foc_openloop.c
@@ -0,0 +1,197 @@
+/****************************************************************************
+ * apps/industry/foc/float/foc_openloop.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/float/foc_angle.h"
+
+/****************************************************************************
+ * Private Function Prototypes
+ ****************************************************************************/
+
+/* Open-loop data */
+
+struct foc_openloop_f32_s
+{
+ struct foc_openloop_cfg_f32_s cfg;
+ struct openloop_data_f32_s data;
+};
+
+/****************************************************************************
+ * Private Function Prototypes
+ ****************************************************************************/
+
+static int foc_angle_ol_init_f32(FAR foc_angle_f32_t *h);
+static void foc_angle_ol_deinit_f32(FAR foc_angle_f32_t *h);
+static int foc_angle_ol_cfg_f32(FAR foc_angle_f32_t *h, FAR void *cfg);
+static void foc_angle_ol_run_f32(FAR foc_angle_f32_t *h,
+ FAR struct foc_angle_in_f32_s *in,
+ FAR struct foc_angle_out_f32_s *out);
+
+/****************************************************************************
+ * Public Data
+ ****************************************************************************/
+
+/* FOC angle float interface */
+
+struct foc_angle_ops_f32_s g_foc_angle_ol_f32 =
+{
+ .init = foc_angle_ol_init_f32,
+ .deinit = foc_angle_ol_deinit_f32,
+ .cfg = foc_angle_ol_cfg_f32,
+ .run = foc_angle_ol_run_f32,
+};
+
+/****************************************************************************
+ * Private Functions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Name: foc_angle_ol_init_f32
+ *
+ * Description:
+ * Initialize the open-loop FOC angle handler (float32)
+ *
+ * Input Parameter:
+ * h - pointer to FOC angle handler
+ *
+ ****************************************************************************/
+
+static int foc_angle_ol_init_f32(FAR foc_angle_f32_t *h)
+{
+ int ret = OK;
+
+ DEBUGASSERT(h);
+
+ /* Connect angle data */
+
+ h->data = zalloc(sizeof(struct foc_openloop_f32_s));
+ if (h->data == NULL)
+ {
+ ret = -ENOMEM;
+ goto errout;
+ }
+
+errout:
+ return ret;
+}
+
+/****************************************************************************
+ * Name: foc_angle_ol_deinit_f32
+ *
+ * Description:
+ * De-initialize the open-loop FOC angle handler (float32)
+ *
+ * Input Parameter:
+ * h - pointer to FOC angle handler
+ *
+ ****************************************************************************/
+
+static void foc_angle_ol_deinit_f32(FAR foc_angle_f32_t *h)
+{
+ DEBUGASSERT(h);
+
+ /* Free angle data */
+
+ if (h->data)
+ {
+ free(h->data);
+ }
+}
+
+/****************************************************************************
+ * Name: foc_angle_ol_cfg_f32
+ *
+ * Description:
+ * Configure the open-loop FOC angle handler (float32)
+ *
+ * Input Parameter:
+ * h - pointer to FOC angle handler
+ * cfg - pointer to angle handler configuration data
+ * (struct foc_openloop_f32_s)
+ *
+ ****************************************************************************/
+
+static int foc_angle_ol_cfg_f32(FAR foc_angle_f32_t *h, FAR void *cfg)
+{
+ FAR struct foc_openloop_f32_s *ol = NULL;
+
+ DEBUGASSERT(h);
+
+ /* Get modulation data */
+
+ DEBUGASSERT(h->data);
+ ol = h->data;
+
+ /* Copy configuration */
+
+ memcpy(&ol->cfg, cfg, sizeof(struct foc_openloop_cfg_f32_s));
+
+ /* Initialize open-loop controller data */
+
+ DEBUGASSERT(ol->cfg.per > 0.0f);
+ motor_openloop_init(&ol->data, ol->cfg.per);
+
+ return OK;
+}
+
+/****************************************************************************
+ * Name: foc_angle_ol_run_f32
+ *
+ * Description:
+ * Process the FOC open-loop angle data (float32)
+ *
+ * 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 void foc_angle_ol_run_f32(FAR foc_angle_f32_t *h,
+ FAR struct foc_angle_in_f32_s *in,
+ FAR struct foc_angle_out_f32_s *out)
+{
+ FAR struct foc_openloop_f32_s *ol = NULL;
+
+ DEBUGASSERT(h);
+
+ /* Get modulation data */
+
+ DEBUGASSERT(h->data);
+ ol = h->data;
+
+ /* Update open-loop */
+
+ motor_openloop(&ol->data, in->vel, in->dir);
+
+ /* Get open-loop angle */
+
+ out->angle = motor_openloop_angle_get(&ol->data);
+}
diff --git a/industry/foc/float/foc_picontrol.c b/industry/foc/float/foc_picontrol.c
new file mode 100644
index 0000000..d51aaf4
--- /dev/null
+++ b/industry/foc/float/foc_picontrol.c
@@ -0,0 +1,418 @@
+/****************************************************************************
+ * apps/industry/foc/float/foc_picontrol.c
+ * This file implements classical FOC PI current controller 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/float/foc_handler.h"
+
+/****************************************************************************
+ * Pre-processor Definitions
+ ****************************************************************************/
+
+#if CONFIG_MOTOR_FOC_PHASES != 3
+# error
+#endif
+
+/****************************************************************************
+ * Private Data Types
+ ****************************************************************************/
+
+/* FOC PI float controller data */
+
+struct foc_picontrol_f32_s
+{
+ float vbase_last; /* Last VBASE sample */
+ phase_angle_f32_t angle; /* Phase angle */
+ struct foc_initdata_f32_s cfg; /* Controller configuration */
+ struct foc_data_f32_s data; /* Controller private data */
+};
+
+/****************************************************************************
+ * Private Function Prototypes
+ ****************************************************************************/
+
+static int foc_control_init_f32(FAR foc_handler_f32_t *h);
+static void foc_control_deinit_f32(FAR foc_handler_f32_t *h);
+static void foc_control_cfg_f32(FAR foc_handler_f32_t *h, FAR void *cfg);
+static void foc_control_input_set_f32(FAR foc_handler_f32_t *h,
+ FAR float *current,
+ float vbase,
+ float angle);
+static void foc_control_voltage_run_f32(FAR foc_handler_f32_t *h,
+ FAR dq_frame_f32_t *dq_ref,
+ FAR ab_frame_f32_t *v_ab_mod);
+static void foc_control_current_run_f32(FAR foc_handler_f32_t *h,
+ FAR dq_frame_f32_t *dq_ref,
+ FAR dq_frame_f32_t *vdq_comp,
+ FAR ab_frame_f32_t *v_ab_mod);
+static void foc_control_state_get_f32(FAR foc_handler_f32_t *h,
+ FAR struct foc_state_f32_s *state);
+
+/****************************************************************************
+ * Public Data
+ ****************************************************************************/
+
+/* FOC control float interface */
+
+struct foc_control_ops_f32_s g_foc_control_pi_f32 =
+{
+ .init = foc_control_init_f32,
+ .deinit = foc_control_deinit_f32,
+ .cfg = foc_control_cfg_f32,
+ .input_set = foc_control_input_set_f32,
+ .voltage_run = foc_control_voltage_run_f32,
+ .current_run = foc_control_current_run_f32,
+ .state_get = foc_control_state_get_f32,
+};
+
+/****************************************************************************
+ * Private Functions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Name: foc_control_init_f32
+ *
+ * Description:
+ * Initialize the FOC PI controller (float32)
+ *
+ * Input Parameter:
+ * h - pointer to FOC handler
+ *
+ ****************************************************************************/
+
+static int foc_control_init_f32(FAR foc_handler_f32_t *h)
+{
+ int ret = OK;
+
+ DEBUGASSERT(h);
+
+ /* Connect controller data */
+
+ h->control = zalloc(sizeof(struct foc_picontrol_f32_s));
+ if (h->control == NULL)
+ {
+ ret = -ENOMEM;
+ goto errout;
+ }
+
+errout:
+ return ret;
+}
+
+/****************************************************************************
+ * Name: foc_control_deinit_f32
+ *
+ * Description:
+ * Deinitialize the FOC PI controller (float32)
+ *
+ * Input Parameter:
+ * h - pointer to FOC handler
+ *
+ ****************************************************************************/
+
+static void foc_control_deinit_f32(FAR foc_handler_f32_t *h)
+{
+ DEBUGASSERT(h);
+
+ /* Free controller data */
+
+ if (h->control)
+ {
+ free(h->control);
+ }
+}
+
+/****************************************************************************
+ * Name: foc_control_cfg_set_f32
+ *
+ * Description:
+ * Configure the FOC controller (float32)
+ *
+ * Input Parameter:
+ * h - pointer to FOC handler
+ * cfg - pointer to controller configuration data
+ * (struct foc_picontrol_f32_s)
+ *
+ ****************************************************************************/
+
+static void foc_control_cfg_f32(FAR foc_handler_f32_t *h, FAR void *cfg)
+{
+ FAR struct foc_picontrol_f32_s *foc = NULL;
+
+ DEBUGASSERT(h);
+ DEBUGASSERT(cfg);
+
+ /* Get controller data */
+
+ DEBUGASSERT(h->control);
+ foc = h->control;
+
+ /* Copy data */
+
+ memcpy(&foc->cfg, cfg, sizeof(struct foc_initdata_f32_s));
+
+ /* Initialize FOC controller data */
+
+ foc_init(&foc->data, &foc->cfg);
+}
+
+/****************************************************************************
+ * Name: foc_control_input_set_f32
+ *
+ * Description:
+ * Update input for controller (float32)
+ *
+ * Input Parameter:
+ * h - pointer to FOC handler
+ * current - phase currents in amps
+ * vbase - base voltage for controller
+ * angle - phase angle in rad
+ *
+ ****************************************************************************/
+
+static void foc_control_input_set_f32(FAR foc_handler_f32_t *h,
+ FAR float *current,
+ float vbase,
+ float angle)
+{
+ FAR struct foc_picontrol_f32_s *foc = NULL;
+ abc_frame_f32_t i_abc;
+
+ DEBUGASSERT(h);
+ DEBUGASSERT(current);
+
+ /* Get controller data */
+
+ DEBUGASSERT(h->control);
+ foc = h->control;
+
+ /* Get current in abc frame */
+
+ i_abc.a = current[0];
+ i_abc.b = current[1];
+ i_abc.c = current[2];
+
+ foc_iabc_update(&foc->data, &i_abc);
+
+ /* Update base voltage only if changed */
+
+ if (foc->vbase_last != vbase)
+ {
+ /* Update FOC base voltage */
+
+ foc_vbase_update(&foc->data, vbase);
+
+ /* Update last FOC base voltage */
+
+ foc->vbase_last = vbase;
+ }
+
+ /* Update phase angle */
+
+#ifndef CONFIG_INDUSTRY_FOC_CORDIC
+ phase_angle_update(&foc->angle, angle);
+#else
+ foc_cordic_angle_f32(h->fd, &foc->angle, angle);
+#endif
+
+ /* Feed the controller with phase angle */
+
+ foc_angle_update(&foc->data, &foc->angle);
+}
+
+/****************************************************************************
+ * Name: foc_control_voltage_f32
+ *
+ * Description:
+ * Handle the FOC voltage control (float32)
+ *
+ * Input Parameter:
+ * h - pointer to FOC handler
+ * dq_ref - DQ voltage reference frame
+ * v_ab_mod - (out) modulation alpha-veta voltage
+ *
+ ****************************************************************************/
+
+static void foc_control_voltage_run_f32(FAR foc_handler_f32_t *h,
+ FAR dq_frame_f32_t *dq_ref,
+ FAR ab_frame_f32_t *v_ab_mod)
+{
+ FAR struct foc_picontrol_f32_s *foc = NULL;
+ float mag_max = 0;
+
+ DEBUGASSERT(h);
+ DEBUGASSERT(dq_ref);
+ DEBUGASSERT(v_ab_mod);
+
+ /* Get controller data */
+
+ DEBUGASSERT(h->control);
+ foc = h->control;
+
+ /* Get maximum possible voltage DQ vetor magnitude */
+
+ foc_vdq_mag_max_get(&foc->data, &mag_max);
+
+ /* Saturate voltage DQ vector */
+
+#ifndef CONFIG_INDUSTRY_FOC_CORDIC
+ dq_saturate(dq_ref, mag_max);
+#else
+ foc_cordic_dqsat_f32(h->fd, dq_ref, mag_max);
+#endif
+
+ /* Call FOC voltage controller */
+
+ foc_voltage_control(&foc->data, dq_ref);
+
+ /* Get output v_ab_mod frame */
+
+ foc_vabmod_get(&foc->data, v_ab_mod);
+}
+
+/****************************************************************************
+ * Name: foc_control_current_f32
+ *
+ * Description:
+ * Handle the FOC current control (float32)
+ *
+ * Input Parameter:
+ * h - pointer to FOC handler
+ * dq_ref - DQ current reference frame
+ * vdq_comp - DQ voltage compensation
+ * v_ab_mod - (out) modulation alpha-veta voltage
+ *
+ ****************************************************************************/
+
+static void foc_control_current_run_f32(FAR foc_handler_f32_t *h,
+ FAR dq_frame_f32_t *dq_ref,
+ FAR dq_frame_f32_t *vdq_comp,
+ FAR ab_frame_f32_t *v_ab_mod)
+{
+ FAR struct foc_picontrol_f32_s *foc = NULL;
+ float mag_max = 0;
+ dq_frame_f32_t v_dq_ref;
+
+ DEBUGASSERT(h);
+ DEBUGASSERT(dq_ref);
+ DEBUGASSERT(vdq_comp);
+ DEBUGASSERT(v_ab_mod);
+
+ /* Get controller data */
+
+ DEBUGASSERT(h->control);
+ foc = h->control;
+
+ /* Reset voltage reference */
+
+ v_dq_ref.d = 0.0f;
+ v_dq_ref.q = 0.0f;
+
+ /* Call FOC current controller */
+
+ foc_current_control(&foc->data, dq_ref, vdq_comp, &v_dq_ref);
+
+ /* Get maximum possible voltage DQ vetor magnitude */
+
+ foc_vdq_mag_max_get(&foc->data, &mag_max);
+
+ /* Saturate voltage DQ vector */
+
+#ifndef CONFIG_INDUSTRY_FOC_CORDIC
+ dq_saturate(dq_ref, mag_max);
+#else
+ foc_cordic_dqsat_f32(h->fd, dq_ref, mag_max);
+#endif
+
+ /* Call FOC voltage control */
+
+ foc_voltage_control(&foc->data, &v_dq_ref);
+
+ /* Get output v_ab_mod frame */
+
+ foc_vabmod_get(&foc->data, v_ab_mod);
+}
+
+/****************************************************************************
+ * Name: foc_control_state_get_f32
+ *
+ * Description:
+ * Get the FOC controller state (float32)
+ *
+ * Input Parameter:
+ * h - pointer to FOC handler
+ * state - (out) pointer to FOC state data
+ *
+ ****************************************************************************/
+
+static void foc_control_state_get_f32(FAR foc_handler_f32_t *h,
+ FAR struct foc_state_f32_s *state)
+{
+ FAR struct foc_picontrol_f32_s *foc = NULL;
+
+ DEBUGASSERT(h);
+ DEBUGASSERT(state);
+
+ /* Get controller data */
+
+ DEBUGASSERT(h->control);
+ foc = h->control;
+
+ /* Copy DQ voltage */
+
+ state->vdq.q = foc->data.v_dq.q;
+ state->vdq.d = foc->data.v_dq.d;
+
+ /* Copy DQ current */
+
+ state->idq.q = foc->data.i_dq.q;
+ state->idq.d = foc->data.i_dq.d;
+
+ /* Copy alpha-beta current */
+
+ state->iab.a = foc->data.i_ab.a;
+ state->iab.b = foc->data.i_ab.b;
+
+ /* Copy alpha-beta voltage */
+
+ state->vab.a = foc->data.v_ab.a;
+ state->vab.b = foc->data.v_ab.b;
+
+ /* Copy phase current */
+
+ state->curr[0] = foc->data.i_abc.a;
+ state->curr[1] = foc->data.i_abc.b;
+ state->curr[2] = foc->data.i_abc.c;
+
+ /* Copy phase voltage */
+
+ state->volt[0] = foc->data.v_abc.a;
+ state->volt[1] = foc->data.v_abc.b;
+ state->volt[2] = foc->data.v_abc.c;
+}
diff --git a/industry/foc/float/foc_ramp.c b/industry/foc/float/foc_ramp.c
new file mode 100644
index 0000000..566ee11
--- /dev/null
+++ b/industry/foc/float/foc_ramp.c
@@ -0,0 +1,178 @@
+/****************************************************************************
+ * apps/industry/foc/float/foc_ramp.c
+ * This file implements ramp 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 "industry/foc/foc_common.h"
+#include "industry/foc/float/foc_ramp.h"
+
+/****************************************************************************
+ * Public Functions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Name: foc_ramp_init_f32
+ *
+ * Description:
+ * Initialize ramp (float32)
+ *
+ * Input Parameter:
+ * ramp - pointer to ramp handler
+ * per - ramp period
+ * thr - ramp threshold
+ * acc - ramp acceleration
+ * dec - ramp decceleration
+ *
+ ****************************************************************************/
+
+int foc_ramp_init_f32(FAR struct foc_ramp_f32_s *ramp, float per,
+ float thr, float acc, float dec)
+{
+ DEBUGASSERT(ramp);
+ DEBUGASSERT(per > 0);
+ DEBUGASSERT(thr > 0);
+ DEBUGASSERT(acc > 0);
+ DEBUGASSERT(dec > 0);
+
+ ramp->per = per;
+ ramp->ramp_thr = thr;
+ ramp->ramp_acc = acc;
+ ramp->ramp_dec = dec;
+ ramp->ramp_acc_per = (ramp->ramp_acc * ramp->per);
+ ramp->ramp_dec_per = (ramp->ramp_dec * ramp->per);
+
+ return OK;
+}
+
+/****************************************************************************
+ * Name: foc_ramp_run_f32
+ *
+ * Description:
+ * Handle ramp (float32)
+ *
+ * Input Parameter:
+ * ramp - pointer to ramp handler
+ * des - ramp parameter destination
+ * now - ramp parameter now
+ * set - (out) ramp parameter set
+ *
+ ****************************************************************************/
+
+int foc_ramp_run_f32(FAR struct foc_ramp_f32_s *ramp, float des,
+ float now, FAR float *set)
+{
+ DEBUGASSERT(ramp);
+
+ /* Check if we require soft start/stop operation.
+ * Only if the user set point differs from the driver set point
+ */
+
+ if (des != *set)
+ {
+ ramp->diff = des - *set;
+
+ if (ramp->diff >= ramp->ramp_thr)
+ {
+ ramp->ramp_mode = RAMP_MODE_SOFTSTART;
+ }
+ else if (ramp->diff <= -ramp->ramp_thr)
+ {
+ ramp->ramp_mode = RAMP_MODE_SOFTSTOP;
+ }
+ else
+ {
+ /* Just set new setpoint */
+
+ *set = des;
+ ramp->ramp_mode = RAMP_MODE_NORMAL;
+ }
+ }
+ else
+ {
+ ramp->ramp_mode = RAMP_MODE_NORMAL;
+ }
+
+ /* Handle according to current motor state */
+
+ switch (ramp->ramp_mode)
+ {
+ case RAMP_MODE_NORMAL:
+ {
+ /* Nothing to do here ? */
+
+ break;
+ }
+
+ case RAMP_MODE_SOFTSTART:
+ {
+ if (des - *set >= ramp->ramp_thr)
+ {
+ /* Increase setpoin with ramp */
+
+ *set = now + ramp->ramp_acc_per;
+ }
+ else
+ {
+ /* Set final setpoint and exit soft start */
+
+ *set = des;
+ ramp->ramp_mode = RAMP_MODE_NORMAL;
+ }
+
+ break;
+ }
+
+ case RAMP_MODE_SOFTSTOP:
+ {
+ if (des - *set <= -ramp->ramp_thr)
+ {
+ /* Stop motor with ramp */
+
+ *set = now - ramp->ramp_dec_per;
+ }
+ else
+ {
+ /* Set final setpint and exit soft stop */
+
+ *set = des;
+ ramp->ramp_mode = RAMP_MODE_NORMAL;
+ }
+
+ break;
+ }
+
+ default:
+ {
+ /* We should not be here */
+
+ DEBUGASSERT(0);
+ break;
+ }
+ }
+
+ return OK;
+}
diff --git a/industry/foc/float/foc_svm3.c b/industry/foc/float/foc_svm3.c
new file mode 100644
index 0000000..da5e9d1
--- /dev/null
+++ b/industry/foc/float/foc_svm3.c
@@ -0,0 +1,287 @@
+/****************************************************************************
+ * apps/industry/foc/float/foc_svm3.c
+ * This file implements 3-phase space vector modulation 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/float/foc_handler.h"
+
+/****************************************************************************
+ * Pre-processor Definitions
+ ****************************************************************************/
+
+#if CONFIG_MOTOR_FOC_PHASES != 3
+# error
+#endif
+
+/* Enable current samples correction if 3-shunts */
+
+#if CONFIG_MOTOR_FOC_SHUNTS == 3
+# define FOC_CORRECT_CURRENT_SAMPLES 1
+#endif
+
+/****************************************************************************
+ * Private Data Types
+ ****************************************************************************/
+
+/* SVM3 data */
+
+struct foc_svm3mod_f32_s
+{
+ struct foc_mod_cfg_f32_s cfg;
+ struct svm3_state_f32_s state;
+};
+
+/****************************************************************************
+ * Private Function Prototypes
+ ****************************************************************************/
+
+static int foc_modulation_init_f32(FAR foc_handler_f32_t *h);
+static void foc_modulation_deinit_f32(FAR foc_handler_f32_t *h);
+static void foc_modulation_cfg_f32(FAR foc_handler_f32_t *h, FAR void *cfg);
+static void foc_modulation_current_f32(FAR foc_handler_f32_t *h,
+ FAR float *curr);
+static void foc_modulation_vbase_get_f32(FAR foc_handler_f32_t *h,
+ float vbus,
+ FAR float *vbase);
+static void foc_modulation_run_f32(FAR foc_handler_f32_t *h,
+ FAR ab_frame_f32_t *v_ab_mod,
+ FAR float *duty);
+
+/****************************************************************************
+ * Public Data
+ ****************************************************************************/
+
+/* FOC modulation float32 interface */
+
+struct foc_modulation_ops_f32_s g_foc_mod_svm3_f32 =
+{
+ .init = foc_modulation_init_f32,
+ .deinit = foc_modulation_deinit_f32,
+ .cfg = foc_modulation_cfg_f32,
+ .current = foc_modulation_current_f32,
+ .vbase_get = foc_modulation_vbase_get_f32,
+ .run = foc_modulation_run_f32,
+};
+
+/****************************************************************************
+ * Private Functions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Name: foc_modulation_init_f32
+ *
+ * Description:
+ * Initialize the SVM3 modulation (float32)
+ *
+ * Input Parameter:
+ * h - pointer to FOC handler
+ *
+ ****************************************************************************/
+
+static int foc_modulation_init_f32(FAR foc_handler_f32_t *h)
+{
+ int ret = OK;
+
+ DEBUGASSERT(h);
+
+ /* Connect modulation data */
+
+ h->modulation = zalloc(sizeof(struct foc_svm3mod_f32_s));
+ if (h->modulation == NULL)
+ {
+ ret = -ENOMEM;
+ goto errout;
+ }
+
+errout:
+ return ret;
+}
+
+/****************************************************************************
+ * Name: foc_modulation_deinit_f32
+ *
+ * Description:
+ * Deinitialize the SVM3 modulation (float32)
+ *
+ * Input Parameter:
+ * h - pointer to FOC handler
+ *
+ ****************************************************************************/
+
+static void foc_modulation_deinit_f32(FAR foc_handler_f32_t *h)
+{
+ DEBUGASSERT(h);
+
+ /* Free modulation data */
+
+ if (h->modulation)
+ {
+ free(h->modulation);
+ }
+}
+
+/****************************************************************************
+ * Name: foc_modulation_cfg_f32
+ *
+ * Description:
+ * Configure the SVM3 modulation (float32)
+ *
+ * Input Parameter:
+ * h - pointer to FOC handler
+ * cfg - pointer to modulation configuration data
+ * (struct foc_svm3mod_f32_s)
+ *
+ ****************************************************************************/
+
+static void foc_modulation_cfg_f32(FAR foc_handler_f32_t *h, FAR void *cfg)
+{
+ FAR struct foc_svm3mod_f32_s *svm = NULL;
+
+ DEBUGASSERT(h);
+ DEBUGASSERT(cfg);
+
+ /* Get modulation data */
+
+ DEBUGASSERT(h->modulation);
+ svm = h->modulation;
+
+ /* Copy data */
+
+ memcpy(&svm->cfg, cfg, sizeof(struct foc_mod_cfg_f32_s));
+
+ /* Initialize SVM3 data */
+
+ svm3_init(&svm->state);
+}
+
+/****************************************************************************
+ * Name: foc_modulation_vbase_get_f32
+ *
+ * Description:
+ * Get the modulation base voltage (float32)
+ *
+ * Input Parameter:
+ * h - pointer to FOC handler
+ * vbus - bus voltage
+ * vbase - (out) pointer to base voltage from modulation
+ *
+ ****************************************************************************/
+
+static void foc_modulation_vbase_get_f32(FAR foc_handler_f32_t *h,
+ float vbus,
+ FAR float *vbase)
+{
+ DEBUGASSERT(h);
+ DEBUGASSERT(vbus);
+ DEBUGASSERT(vbase);
+
+ /* Get maximum possible base voltage */
+
+ *vbase = SVM3_BASE_VOLTAGE_GET(vbus);
+}
+
+/****************************************************************************
+ * Name: foc_modulation_current_f32
+ *
+ * Description:
+ * Correct current samples accrding to the SVM3 modulation state (float32)
+ *
+ * Input Parameter:
+ * h - pointer to FOC handler
+ * curr - (in/out) phase currents
+ *
+ ****************************************************************************/
+
+static void foc_modulation_current_f32(FAR foc_handler_f32_t *h,
+ FAR float *curr)
+{
+#ifdef FOC_CORRECT_CURRENT_SAMPLES
+ FAR struct foc_svm3mod_f32_s *svm = NULL;
+
+ DEBUGASSERT(h);
+ DEBUGASSERT(curr);
+
+ /* Get modulation data */
+
+ DEBUGASSERT(h->modulation);
+ svm = h->modulation;
+
+ /* Correct ADC current samples */
+
+ svm3_current_correct(&svm->state,
+ &curr[0],
+ &curr[1],
+ &curr[2]);
+#endif
+}
+
+/****************************************************************************
+ * Name: foc_modulation_f32
+ *
+ * Description:
+ * Handle the SVM3 modulation (float32)
+ *
+ * Input Parameter:
+ * h - pointer to FOC handler
+ * v_ab_mod - requested modulation voltage scaled to 0.0-1.0 range
+ * duty - (out) duty cycle for switches
+ *
+ ****************************************************************************/
+
+static void foc_modulation_run_f32(FAR foc_handler_f32_t *h,
+ FAR ab_frame_f32_t *v_ab_mod,
+ FAR float *duty)
+{
+ FAR struct foc_svm3mod_f32_s *svm = NULL;
+
+ DEBUGASSERT(h);
+ DEBUGASSERT(v_ab_mod);
+ DEBUGASSERT(duty);
+
+ /* Get modulation data */
+
+ DEBUGASSERT(h->modulation);
+ svm = h->modulation;
+
+ /* Call 3-phase space vector modulation */
+
+ svm3(&svm->state, v_ab_mod);
+
+ /* Copy duty cycle */
+
+ duty[0] = svm->state.d_u;
+ duty[1] = svm->state.d_v;
+ duty[2] = svm->state.d_w;
+
+ /* Saturate duty cycle */
+
+ f_saturate(&duty[0], 0.0f, svm->cfg.pwm_duty_max);
+ f_saturate(&duty[1], 0.0f, svm->cfg.pwm_duty_max);
+ f_saturate(&duty[2], 0.0f, svm->cfg.pwm_duty_max);
+}
diff --git a/industry/foc/float/foc_velocity.c b/industry/foc/float/foc_velocity.c
new file mode 100644
index 0000000..2765682
--- /dev/null
+++ b/industry/foc/float/foc_velocity.c
@@ -0,0 +1,157 @@
+/****************************************************************************
+ * apps/industry/foc/float/foc_velocity.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 <debug.h>
+#include <errno.h>
+
+#include "industry/foc/foc_log.h"
+#include "industry/foc/float/foc_velocity.h"
+
+/****************************************************************************
+ * Public Functions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Name: foc_velocity_init_f32
+ *
+ * Description:
+ * Initialize the FOC velocity handler (float32)
+ *
+ * Input Parameter:
+ * h - pointer to FOC velocity handler
+ * ops - pointer to FOC velocity handler operations
+ *
+ ****************************************************************************/
+
+int foc_velocity_init_f32(FAR foc_velocity_f32_t *h,
+ FAR struct foc_velocity_ops_f32_s *ops)
+{
+ int ret = OK;
+
+ DEBUGASSERT(h);
+ DEBUGASSERT(ops);
+
+ /* Velocity ops */
+
+ DEBUGASSERT(ops->init);
+ DEBUGASSERT(ops->deinit);
+ DEBUGASSERT(ops->cfg);
+ DEBUGASSERT(ops->run);
+
+ /* Reset handler */
+
+ memset(h, 0, sizeof(foc_velocity_f32_t));
+
+ /* Connect ops */
+
+ h->ops = ops;
+
+ /* Initialize velocity */
+
+ ret = h->ops->init(h);
+ if (ret < 0)
+ {
+ FOCLIBERR("ERROR: ops->init failed %d\n", ret);
+ goto errout;
+ }
+
+errout:
+ return ret;
+}
+
+/****************************************************************************
+ * Name: foc_velocity_deinit_f32
+ *
+ * Description:
+ * De-initialize the FOC velocity handler (float32)
+ *
+ * Input Parameter:
+ * h - pointer to FOC velocity handler
+ *
+ ****************************************************************************/
+
+int foc_velocity_deinit_f32(FAR foc_velocity_f32_t *h)
+{
+ int ret = OK;
+
+ DEBUGASSERT(h);
+
+ /* Deinitialize velocity */
+
+ h->ops->deinit(h);
+
+ /* Reset data */
+
+ memset(h, 0, sizeof(foc_velocity_f32_t));
+
+ return ret;
+}
+
+/****************************************************************************
+ * Name: foc_velocity_cfg_f32
+ *
+ * Description:
+ * Configure the FOC velocity handler (float32)
+ *
+ * Input Parameter:
+ * h - pointer to FOC velocity handler
+ * cfg - pointer to velocity handler configuration data
+ *
+ ****************************************************************************/
+
+int foc_velocity_cfg_f32(FAR foc_velocity_f32_t *h, FAR void *cfg)
+{
+ DEBUGASSERT(h);
+ DEBUGASSERT(cfg);
+
+ return h->ops->cfg(h, cfg);
+}
+
+/****************************************************************************
+ * Name: foc_velocity_run_f32
+ *
+ * Description:
+ * Process the FOC velocity handler data (float32)
+ *
+ * Input Parameter:
+ * h - pointer to FOC velocity handler
+ * in - pointer to FOC velocity handler input data
+ * out - pointer to FOC velocity handler output data
+ *
+ ****************************************************************************/
+
+void foc_velocity_run_f32(FAR foc_velocity_f32_t *h,
+ FAR struct foc_velocity_in_f32_s *in,
+ FAR struct foc_velocity_out_f32_s *out)
+{
+ DEBUGASSERT(h);
+ DEBUGASSERT(in);
+ DEBUGASSERT(out);
+
+ /* Run velocity handler */
+
+ h->ops->run(h, in, out);
+}
diff --git a/industry/foc/foc_utils.c b/industry/foc/foc_utils.c
new file mode 100644
index 0000000..27838f8
--- /dev/null
+++ b/industry/foc/foc_utils.c
@@ -0,0 +1,175 @@
+/****************************************************************************
+ * apps/industry/foc/foc_utils.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 <sys/ioctl.h>
+#include <errno.h>
+
+#include "industry/foc/foc_log.h"
+#include "industry/foc/foc_utils.h"
+
+/****************************************************************************
+ * Public Functions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Name: foc_dev_stop
+ ****************************************************************************/
+
+int foc_dev_stop(int fd)
+{
+ int ret = OK;
+
+ ret = ioctl(fd, MTRIOC_STOP, 0);
+ if (ret != OK)
+ {
+ FOCLIBERR("ERROR: MTRIOC_STOP failed %d!\n", errno);
+ ret = -errno;
+ }
+
+ return ret;
+}
+
+/****************************************************************************
+ * Name: foc_dev_start
+ ****************************************************************************/
+
+int foc_dev_start(int fd)
+{
+ int ret = OK;
+
+ ret = ioctl(fd, MTRIOC_START, 0);
+ if (ret != OK)
+ {
+ FOCLIBERR("ERROR: MTRIOC_START failed %d!\n", errno);
+ ret = -errno;
+ }
+
+ return ret;
+}
+
+/****************************************************************************
+ * Name: foc_dev_clearfault
+ ****************************************************************************/
+
+int foc_dev_clearfault(int fd)
+{
+ int ret = OK;
+
+ /* Clear fault state */
+
+ ret = ioctl(fd, MTRIOC_CLEAR_FAULT, (unsigned long)0);
+ if (ret != OK)
+ {
+ FOCLIBERR("ERROR: MTRIOC_CLEAR_FAULT failed %d!\n", errno);
+ ret = -errno;
+ }
+
+ return ret;
+}
+
+/****************************************************************************
+ * Name: foc_dev_getstate
+ ****************************************************************************/
+
+int foc_dev_getstate(int fd, FAR struct foc_state_s *state)
+{
+ int ret = OK;
+
+ /* Blocking operation */
+
+ ret = ioctl(fd, MTRIOC_GET_STATE, (unsigned long)state);
+ if (ret != OK)
+ {
+ FOCLIBERR("ERROR: MTRIOC_GET_STATE failed %d!\n", errno);
+ ret = -errno;
+ }
+
+ return ret;
+}
+
+/****************************************************************************
+ * Name: foc_dev_setparams
+ ****************************************************************************/
+
+int foc_dev_setparams(int fd, FAR struct foc_params_s *params)
+{
+ int ret = OK;
+
+ ret = ioctl(fd, MTRIOC_SET_PARAMS, (unsigned long)params);
+ if (ret != OK)
+ {
+ FOCLIBERR("ERROR: MTRIOC_SET_PARAMS failed %d!\n", errno);
+ ret = -errno;
+ }
+
+ return ret;
+}
+
+/****************************************************************************
+ * Name: foc_dev_setcfg
+ ****************************************************************************/
+
+int foc_dev_setcfg(int fd, FAR struct foc_cfg_s *cfg)
+{
+ int ret = OK;
+
+ ret = ioctl(fd, MTRIOC_SET_CONFIG, (unsigned long)cfg);
+ if (ret != OK)
+ {
+ FOCLIBERR("ERROR: MTRIOC_SET_CONFIG failed %d!\n", errno);
+ ret = -errno;
+ }
+
+ return ret;
+}
+
+/****************************************************************************
+ * Name: foc_dev_getinfo
+ ****************************************************************************/
+
+int foc_dev_getinfo(int fd, FAR struct foc_info_s *info)
+{
+ int ret = OK;
+
+ ret = ioctl(fd, MTRIOC_GET_INFO, (unsigned long)info);
+ if (ret != OK)
+ {
+ FOCLIBERR("ERROR: MTRIOC_GET_INFO failed %d!\n", errno);
+ ret = -errno;
+ }
+
+ return ret;
+}
+
+/****************************************************************************
+ * Name: foc_cfg_print
+ ****************************************************************************/
+
+void foc_cfg_print(FAR struct foc_cfg_s *cfg)
+{
+ FOCLIBLOG("PWM freq=%" PRIu32 "\n", cfg->pwm_freq);
+ FOCLIBLOG("Notifier freq=%" PRIu32 "\n", cfg->notifier_freq);
+}