From d75d6b7af0d4d79dbf79980605ffeca0277ca38c Mon Sep 17 00:00:00 2001 From: raiden00pl Date: Sat, 20 Mar 2021 21:06:54 +0100 Subject: [PATCH] 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(+) create mode 100644 include/industry/foc/fixed16/foc_angle.h create mode 100644 include/industry/foc/fixed16/foc_cordic.h create mode 100644 include/industry/foc/fixed16/foc_handler.h create mode 100644 include/industry/foc/fixed16/foc_model.h create mode 100644 include/industry/foc/fixed16/foc_ramp.h create mode 100644 include/industry/foc/fixed16/foc_velocity.h create mode 100644 include/industry/foc/float/foc_angle.h create mode 100644 include/industry/foc/float/foc_cordic.h create mode 100644 include/industry/foc/float/foc_handler.h create mode 100644 include/industry/foc/float/foc_model.h create mode 100644 include/industry/foc/float/foc_ramp.h create mode 100644 include/industry/foc/float/foc_velocity.h create mode 100644 include/industry/foc/foc_common.h create mode 100644 include/industry/foc/foc_log.h create mode 100644 include/industry/foc/foc_utils.h create mode 100644 industry/foc/Kconfig create mode 100644 industry/foc/Make.defs create mode 100644 industry/foc/Makefile create mode 100644 industry/foc/fixed16/foc_angle.c create mode 100644 industry/foc/fixed16/foc_cordic.c create mode 100644 industry/foc/fixed16/foc_handler.c create mode 100644 industry/foc/fixed16/foc_model.c create mode 100644 industry/foc/fixed16/foc_model_pmsm.c create mode 100644 industry/foc/fixed16/foc_openloop.c create mode 100644 industry/foc/fixed16/foc_picontrol.c create mode 100644 industry/foc/fixed16/foc_ramp.c create mode 100644 industry/foc/fixed16/foc_svm3.c create mode 100644 industry/foc/fixed16/foc_velocity.c create mode 100644 industry/foc/float/foc_angle.c create mode 100644 industry/foc/float/foc_cordic.c create mode 100644 industry/foc/float/foc_handler.c create mode 100644 industry/foc/float/foc_model.c create mode 100644 industry/foc/float/foc_model_pmsm.c create mode 100644 industry/foc/float/foc_openloop.c create mode 100644 industry/foc/float/foc_picontrol.c create mode 100644 industry/foc/float/foc_ramp.c create mode 100644 industry/foc/float/foc_svm3.c create mode 100644 industry/foc/float/foc_velocity.c create mode 100644 industry/foc/foc_utils.c diff --git a/include/industry/foc/fixed16/foc_angle.h b/include/industry/foc/fixed16/foc_angle.h new file mode 100644 index 000000000..02abb0d32 --- /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 + +#include + +#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 000000000..07900199d --- /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 + +#include + +/**************************************************************************** + * 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 000000000..3c445c0f3 --- /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 + +#include + +#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 000000000..ce0c8c8e4 --- /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 + +#include + +/**************************************************************************** + * 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 000000000..1c2a1547a --- /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 + +#include + +/**************************************************************************** + * 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 000000000..311cc23aa --- /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 + +#include + +#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 000000000..2076dfa04 --- /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 + +#include + +#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 000000000..28562bf83 --- /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 + +#include + +/**************************************************************************** + * 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 000000000..b3941fe95 --- /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 + +#include + +#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 000000000..da26eec0c --- /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 + +#include + +/**************************************************************************** + * 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 000000000..ea75700b0 --- /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 + +#include + +/**************************************************************************** + * 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 000000000..2ccaaacb1 --- /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 + +#include + +#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 000000000..d474e9cb0 --- /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 + +#include + +/**************************************************************************** + * 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 000000000..0634d4924 --- /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 + +#include +#include + +/**************************************************************************** + * 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 000000000..65406ce7d --- /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 + +#include +#include + +/**************************************************************************** + * 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 000000000..1d18be91c --- /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 000000000..f5a84f083 --- /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 000000000..29b66bd6c --- /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 000000000..2f255f10a --- /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 + +#include +#include + +#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 000000000..e7311ab6d --- /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 + +#include + +#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 000000000..9b1927b8c --- /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 + +#include +#include + +#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 000000000..8540dee6a --- /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 + +#include +#include + +#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 000000000..e3da0d999 --- /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 + +#include +#include +#include + +#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 000000000..1a272c948 --- /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 + +#include +#include +#include + +#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 000000000..4906a4733 --- /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 + +#include +#include +#include + +#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 000000000..237ebd3b8 --- /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 + +#include + +#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 000000000..6d85c20b2 --- /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 + +#include +#include +#include + +#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 000000000..005f9249d --- /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 + +#include +#include + +#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 000000000..8673e789f --- /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 + +#include +#include + +#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 000000000..271e7f225 --- /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 + +#include + +#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 000000000..c07c0b5ba --- /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 + +#include +#include + +#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 000000000..d46e0b474 --- /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 + +#include +#include + +#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 000000000..cca7b760d --- /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 + +#include +#include +#include + +#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 000000000..365b0be4c --- /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 + +#include +#include +#include + +#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 000000000..d51aaf405 --- /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 + +#include +#include +#include + +#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 000000000..566ee1144 --- /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 + +#include + +#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 000000000..da5e9d174 --- /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 + +#include +#include +#include + +#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 000000000..2765682c8 --- /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 + +#include +#include + +#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 000000000..27838f8e6 --- /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 + +#include +#include + +#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); +}