From 6cf6a73fc091b0e3bb28ee3dbf6f768625c45766 Mon Sep 17 00:00:00 2001 From: raiden00pl Date: Thu, 12 Oct 2023 12:40:14 +0200 Subject: [PATCH] industry/foc: add support for PMSM feedforward compensation reference: https://electronics.stackexchange.com/questions/386246/field-oriented-control-feed-forward-term --- .../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(-) create mode 100644 include/industry/foc/fixed16/foc_feedforward.h create mode 100644 include/industry/foc/float/foc_feedforward.h create mode 100644 industry/foc/fixed16/foc_feedforward.c create mode 100644 industry/foc/float/foc_feedforward.c 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 + +#include + +/**************************************************************************** + * 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 + +#include + +/**************************************************************************** + * 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 + +#include + +#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 + +#include + +#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; +}