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/nuttx-apps.git
The following commit(s) were added to refs/heads/master by this push:
new 6cf6a73fc industry/foc: add support for PMSM feedforward compensation
6cf6a73fc is described below
commit 6cf6a73fc091b0e3bb28ee3dbf6f768625c45766
Author: raiden00pl <raide...@railab.me>
AuthorDate: Thu Oct 12 12:40:14 2023 +0200
industry/foc: add support for PMSM feedforward compensation
reference:
https://electronics.stackexchange.com/questions/386246/field-oriented-control-feed-forward-term
---
include/industry/foc/fixed16/foc_feedforward.h | 45 +++++++++++++++++
include/industry/foc/float/foc_feedforward.h | 45 +++++++++++++++++
industry/foc/CMakeLists.txt | 16 +++++-
industry/foc/Kconfig | 6 +++
industry/foc/Makefile | 6 +++
industry/foc/fixed16/foc_feedforward.c | 68 ++++++++++++++++++++++++++
industry/foc/float/foc_feedforward.c | 66 +++++++++++++++++++++++++
7 files changed, 250 insertions(+), 2 deletions(-)
diff --git a/include/industry/foc/fixed16/foc_feedforward.h
b/include/industry/foc/fixed16/foc_feedforward.h
new file mode 100644
index 000000000..742537a18
--- /dev/null
+++ b/include/industry/foc/fixed16/foc_feedforward.h
@@ -0,0 +1,45 @@
+/****************************************************************************
+ * apps/include/industry/foc/fixed16/foc_feedforward.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_FEEDFORWARD_H
+#define __INDUSTRY_FOC_FIXED16_FOC_FEEDFORWARD_H
+
+/****************************************************************************
+ * Included Files
+ ****************************************************************************/
+
+#include <nuttx/config.h>
+
+#include <dspb16.h>
+
+/****************************************************************************
+ * Public Type Definition
+ ****************************************************************************/
+
+/****************************************************************************
+ * Public Data
+ ****************************************************************************/
+
+int foc_feedforward_pmsm_b16(FAR struct motor_phy_params_b16_s *phy,
+ FAR dq_frame_b16_t *idq,
+ b16_t vel_now,
+ FAR dq_frame_b16_t *vdq_comp);
+
+#endif /* __INDUSTRY_FOC_FIXED16_FOC_FEEDFORWARD_H */
diff --git a/include/industry/foc/float/foc_feedforward.h
b/include/industry/foc/float/foc_feedforward.h
new file mode 100644
index 000000000..6be81336c
--- /dev/null
+++ b/include/industry/foc/float/foc_feedforward.h
@@ -0,0 +1,45 @@
+/****************************************************************************
+ * apps/include/industry/foc/float/foc_feedforward.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_FEEDFORWARD_H
+#define __INDUSTRY_FOC_FLOAT_FOC_FEEDFORWARD_H
+
+/****************************************************************************
+ * Included Files
+ ****************************************************************************/
+
+#include <nuttx/config.h>
+
+#include <dsp.h>
+
+/****************************************************************************
+ * Public Type Definition
+ ****************************************************************************/
+
+/****************************************************************************
+ * Public Data
+ ****************************************************************************/
+
+int foc_feedforward_pmsm_f32(FAR struct motor_phy_params_f32_s *phy,
+ FAR dq_frame_f32_t *idq,
+ float vel_now,
+ FAR dq_frame_f32_t *vdq_comp);
+
+#endif /* __INDUSTRY_FOC_FLOAT_FOC_FEEDFORWARD_H */
diff --git a/industry/foc/CMakeLists.txt b/industry/foc/CMakeLists.txt
index 3d5402dca..e0f1d2592 100644
--- a/industry/foc/CMakeLists.txt
+++ b/industry/foc/CMakeLists.txt
@@ -23,7 +23,9 @@ if(CONFIG_INDUSTRY_FOC)
set(CSRCS foc_utils.c)
if(CONFIG_INDUSTRY_FOC_FLOAT)
- list(APPEND CSRCS
+ list(
+ APPEND
+ CSRCS
float/foc_ramp.c
float/foc_handler.c
float/foc_angle.c
@@ -85,10 +87,16 @@ if(CONFIG_INDUSTRY_FOC)
if(CONFIG_INDUSTRY_FOC_VELOCITY_OPLL)
list(APPEND CSRCS float/foc_vel_opll.c)
endif()
+
+ if(CONFIG_INDUSTRY_FOC_FEEDFORWARD)
+ list(APPEND CSRCS float/foc_feedforward.c)
+ endif()
endif()
if(CONFIG_INDUSTRY_FOC_FIXED16)
- list(APPEND CSRCS
+ list(
+ APPEND
+ CSRCS
fixed16/foc_ramp.c
fixed16/foc_handler.c
fixed16/foc_angle.c
@@ -150,6 +158,10 @@ if(CONFIG_INDUSTRY_FOC)
if(CONFIG_INDUSTRY_FOC_VELOCITY_OPLL)
list(APPEND CSRCS fixed16/foc_vel_opll.c)
endif()
+
+ if(CONFIG_INDUSTRY_FOC_FEEDFORWARD)
+ list(APPEND CSRCS fixed16/foc_feedforward.c)
+ endif()
endif()
target_sources(apps PRIVATE ${CSRCS})
diff --git a/industry/foc/Kconfig b/industry/foc/Kconfig
index 983bfe262..c22356cec 100644
--- a/industry/foc/Kconfig
+++ b/industry/foc/Kconfig
@@ -116,6 +116,12 @@ config INDUSTRY_FOC_MODULATION_SVM3
---help---
Enable support for FOC 3-phase space vector modulation
+config INDUSTRY_FOC_FEEDFORWARD
+ bool "FOC current controller feedforward compensation"
+ default n
+ ---help---
+ Enable support for current controller feed forward compensation
+
config INDUSTRY_FOC_MODEL_PMSM
bool "FOC PMSM model support"
select INDUSTRY_FOC_HAVE_MODEL
diff --git a/industry/foc/Makefile b/industry/foc/Makefile
index 4923de19c..bdc0efaf8 100644
--- a/industry/foc/Makefile
+++ b/industry/foc/Makefile
@@ -75,6 +75,9 @@ endif
ifeq ($(CONFIG_INDUSTRY_FOC_VELOCITY_OPLL),y)
CSRCS += float/foc_vel_opll.c
endif
+ifeq ($(CONFIG_INDUSTRY_FOC_FEEDFORWARD),y)
+CSRCS += float/foc_feedforward.c
+endif
endif
@@ -129,6 +132,9 @@ endif
ifeq ($(CONFIG_INDUSTRY_FOC_VELOCITY_OPLL),y)
CSRCS += fixed16/foc_vel_opll.c
endif
+ifeq ($(CONFIG_INDUSTRY_FOC_FEEDFORWARD),y)
+CSRCS += fixed16/foc_feedforward.c
+endif
endif
diff --git a/industry/foc/fixed16/foc_feedforward.c
b/industry/foc/fixed16/foc_feedforward.c
new file mode 100644
index 000000000..b504f0374
--- /dev/null
+++ b/industry/foc/fixed16/foc_feedforward.c
@@ -0,0 +1,68 @@
+/****************************************************************************
+ * apps/industry/foc/fixed16/foc_feedforward.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_feedforward.h"
+
+/****************************************************************************
+ * Public Functions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Name: foc_feedforward_pmsm_b16
+ *
+ * Description:
+ * Feed forward compensation for PMSM (fixed16)
+ *
+ * Input Parameter:
+ * phy - motor physical parameters
+ * idq - iqd frame
+ * vel_now - electrical velocity
+ * vdq_comp - compensation vdq frame
+ *
+ ****************************************************************************/
+
+int foc_feedforward_pmsm_b16(FAR struct motor_phy_params_b16_s *phy,
+ FAR dq_frame_b16_t *idq,
+ fixed16 vel_now,
+ FAR dq_frame_b16_t *vdq_comp)
+{
+ DEBUGASSERT(phy);
+ DEBUGASSERT(idq);
+ DEBUGASSERT(vdq_comp);
+
+ /* NOTE: vdq_comp is substracted from vdq_ref in foc_current_control()
+ * so vq compensation must be negative here.
+ */
+
+ vdq_comp->q = -b16mulb16(vel_now,
+ (phy->flux_link + b16mulb16(phy->ind,
+ idq->q)));
+ vdq_comp->d = b16mulb16(b16mulb16(vel_now, phy->ind), idq->q)U;
+
+ return OK;
+}
diff --git a/industry/foc/float/foc_feedforward.c
b/industry/foc/float/foc_feedforward.c
new file mode 100644
index 000000000..d6da4c53b
--- /dev/null
+++ b/industry/foc/float/foc_feedforward.c
@@ -0,0 +1,66 @@
+/****************************************************************************
+ * apps/industry/foc/float/foc_feedforward.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_feedforward.h"
+
+/****************************************************************************
+ * Public Functions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Name: foc_feedforward_pmsm_f32
+ *
+ * Description:
+ * Feed forward compensation for PMSM (float32)
+ *
+ * Input Parameter:
+ * phy - motor physical parameters
+ * idq - iqd frame
+ * vel_now - electrical velocity
+ * vdq_comp - compensation vdq frame
+ *
+ ****************************************************************************/
+
+int foc_feedforward_pmsm_f32(FAR struct motor_phy_params_f32_s *phy,
+ FAR dq_frame_f32_t *idq,
+ float vel_now,
+ FAR dq_frame_f32_t *vdq_comp)
+{
+ DEBUGASSERT(phy);
+ DEBUGASSERT(idq);
+ DEBUGASSERT(vdq_comp);
+
+ /* NOTE: vdq_comp is substracted from vdq_ref in foc_current_control()
+ * so vq compensation must be negative here.
+ */
+
+ vdq_comp->q = -vel_now * (phy->flux_link + phy->ind * idq->q);
+ vdq_comp->d = vel_now * phy->ind * idq->q;
+
+ return OK;
+}
