From fbb09ff12884a63e3c60e11234b88fa57d1e8a73 Mon Sep 17 00:00:00 2001 From: raiden00pl Date: Sun, 20 Feb 2022 12:54:51 +0100 Subject: [PATCH] foclib/fixed16: add support for observers --- include/industry/foc/fixed16/foc_angle.h | 32 ++ include/industry/foc/fixed16/foc_handler.h | 1 + include/industry/foc/fixed16/foc_velocity.h | 38 +++ industry/foc/Makefile | 12 + industry/foc/fixed16/foc_ang_onfo.c | 321 ++++++++++++++++++++ industry/foc/fixed16/foc_ang_osmo.c | 283 +++++++++++++++++ industry/foc/fixed16/foc_picontrol.c | 4 + industry/foc/fixed16/foc_vel_odiv.c | 286 +++++++++++++++++ industry/foc/fixed16/foc_vel_opll.c | 284 +++++++++++++++++ 9 files changed, 1261 insertions(+) create mode 100644 industry/foc/fixed16/foc_ang_onfo.c create mode 100644 industry/foc/fixed16/foc_ang_osmo.c create mode 100644 industry/foc/fixed16/foc_vel_odiv.c create mode 100644 industry/foc/fixed16/foc_vel_opll.c diff --git a/include/industry/foc/fixed16/foc_angle.h b/include/industry/foc/fixed16/foc_angle.h index 55e2a3ae1..ba6cd22ff 100644 --- a/include/industry/foc/fixed16/foc_angle.h +++ b/include/industry/foc/fixed16/foc_angle.h @@ -106,6 +106,26 @@ struct foc_openloop_cfg_b16_s }; #endif +#ifdef CONFIG_INDUSTRY_FOC_ANGLE_ONFO +struct foc_angle_onfo_cfg_b16_s +{ + b16_t per; /* Controller period */ + b16_t gain; + b16_t gain_slow; + struct motor_phy_params_b16_s phy; +}; +#endif + +#ifdef CONFIG_INDUSTRY_FOC_ANGLE_OSMO +struct foc_angle_osmo_cfg_b16_s +{ + b16_t per; /* Controller period */ + b16_t k_slide; /* Bang-bang controller gain */ + b16_t err_max; /* Linear mode threshold */ + struct motor_phy_params_b16_s phy; +}; +#endif + #ifdef CONFIG_INDUSTRY_FOC_ANGLE_QENCO /* Qencoder configuration data */ @@ -136,6 +156,18 @@ struct foc_hall_cfg_b16_s extern struct foc_angle_ops_b16_s g_foc_angle_ol_b16; #endif +#ifdef CONFIG_INDUSTRY_FOC_ANGLE_ONFO +/* NFO oberver angle operations (fixed16) */ + +extern struct foc_angle_ops_b16_s g_foc_angle_onfo_b16; +#endif + +#ifdef CONFIG_INDUSTRY_FOC_ANGLE_OSMO +/* SMO oberver angle operations (fixed16) */ + +extern struct foc_angle_ops_b16_s g_foc_angle_osmo_b16; +#endif + #ifdef CONFIG_INDUSTRY_FOC_ANGLE_QENCO /* Qencoder angle operations (fixed16) */ diff --git a/include/industry/foc/fixed16/foc_handler.h b/include/industry/foc/fixed16/foc_handler.h index f618f2fbe..793aeafa8 100644 --- a/include/industry/foc/fixed16/foc_handler.h +++ b/include/industry/foc/fixed16/foc_handler.h @@ -66,6 +66,7 @@ struct foc_state_b16_s ab_frame_b16_t vab; dq_frame_b16_t vdq; dq_frame_b16_t idq; + b16_t mod_scale; }; /* Forward declaration */ diff --git a/include/industry/foc/fixed16/foc_velocity.h b/include/industry/foc/fixed16/foc_velocity.h index fdab3f13f..14119e968 100644 --- a/include/industry/foc/fixed16/foc_velocity.h +++ b/include/industry/foc/fixed16/foc_velocity.h @@ -95,6 +95,44 @@ struct foc_velocity_b16_s FAR void *data; }; +#ifdef CONFIG_INDUSTRY_FOC_VELOCITY_ODIV +/* Velocity DIV observer */ + +struct foc_vel_div_b16_cfg_s +{ + uint8_t samples; + b16_t filter; + b16_t per; +}; +#endif + +#ifdef CONFIG_INDUSTRY_FOC_VELOCITY_OPLL +/* Velocity PLL observer */ + +struct foc_vel_pll_b16_cfg_s +{ + b16_t kp; + b16_t ki; + b16_t per; +}; +#endif + +/**************************************************************************** + * Public Data + ****************************************************************************/ + +#ifdef CONFIG_INDUSTRY_FOC_VELOCITY_ODIV +/* Velocity DIV observer (fixed16) */ + +extern struct foc_velocity_ops_b16_s g_foc_velocity_odiv_b16; +#endif + +#ifdef CONFIG_INDUSTRY_FOC_VELOCITY_OPLL +/* Velocity PLL observer (fixed16) */ + +extern struct foc_velocity_ops_b16_s g_foc_velocity_opll_b16; +#endif + /**************************************************************************** * Public Function Prototypes ****************************************************************************/ diff --git a/industry/foc/Makefile b/industry/foc/Makefile index e1797f1ef..4923de19c 100644 --- a/industry/foc/Makefile +++ b/industry/foc/Makefile @@ -90,6 +90,12 @@ CSRCS += fixed16/foc_routine.c ifeq ($(CONFIG_INDUSTRY_FOC_ANGLE_OPENLOOP),y) CSRCS += fixed16/foc_ang_openloop.c endif +ifeq ($(CONFIG_INDUSTRY_FOC_ANGLE_ONFO),y) +CSRCS += fixed16/foc_ang_onfo.c +endif +ifeq ($(CONFIG_INDUSTRY_FOC_ANGLE_OSMO),y) +CSRCS += fixed16/foc_ang_osmo.c +endif ifeq ($(CONFIG_INDUSTRY_FOC_ANGLE_QENCO),y) CSRCS += fixed16/foc_ang_qenco.c endif @@ -117,6 +123,12 @@ endif ifeq ($(CONFIG_INDUSTRY_FOC_IDENT),y) CSRCS += fixed16/foc_ident.c endif +ifeq ($(CONFIG_INDUSTRY_FOC_VELOCITY_ODIV),y) +CSRCS += fixed16/foc_vel_odiv.c +endif +ifeq ($(CONFIG_INDUSTRY_FOC_VELOCITY_OPLL),y) +CSRCS += fixed16/foc_vel_opll.c +endif endif diff --git a/industry/foc/fixed16/foc_ang_onfo.c b/industry/foc/fixed16/foc_ang_onfo.c new file mode 100644 index 000000000..35fbe63f1 --- /dev/null +++ b/industry/foc/fixed16/foc_ang_onfo.c @@ -0,0 +1,321 @@ +/**************************************************************************** + * apps/industry/foc/fixed16/foc_ang_onfo.c + * + * Licensed to the Apache Software Foundation (ASF) under one or more + * contributor license agreements. See the NOTICE file distributed with + * this work for additional information regarding copyright ownership. The + * ASF licenses this file to you under the Apache License, Version 2.0 (the + * "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include + +#include +#include +#include + +#include "industry/foc/foc_log.h" +#include "industry/foc/fixed16/foc_angle.h" + +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ + +#define SIGN(x) ((x > 0) ? b16ONE : -b16ONE) + +#define LINEAR_MAP(x, in_min, in_max, out_min, out_max) \ + (b16divb16(b16mulb16((x - in_min), (out_max - out_min)), \ + (in_max - in_min)) + out_min) + +#ifndef ABS +# define ABS(a) (a < 0 ? -a : a) +#endif + +/**************************************************************************** + * Private Data Types + ****************************************************************************/ + +/* sensorless observer data */ + +struct foc_ang_onfo_b16_s +{ + struct foc_angle_onfo_cfg_b16_s cfg; + struct motor_aobserver_nfo_b16_s data; + struct motor_aobserver_b16_s o; + b16_t sensor_dir; +}; + +/**************************************************************************** + * Private Function Prototypes + ****************************************************************************/ + +static int foc_angle_onfo_init_b16(FAR foc_angle_b16_t *h); +static void foc_angle_onfo_deinit_b16(FAR foc_angle_b16_t *h); +static int foc_angle_onfo_cfg_b16(FAR foc_angle_b16_t *h, FAR void *cfg); +static int foc_angle_onfo_zero_b16(FAR foc_angle_b16_t *h); +static int foc_angle_onfo_dir_b16(FAR foc_angle_b16_t *h, b16_t dir); +static int foc_angle_onfo_run_b16(FAR foc_angle_b16_t *h, + FAR struct foc_angle_in_b16_s *in, + FAR struct foc_angle_out_b16_s *out); + +/**************************************************************************** + * Public Data + ****************************************************************************/ + +/* FOC angle b16_t interface */ + +struct foc_angle_ops_b16_s g_foc_angle_onfo_b16 = +{ + .init = foc_angle_onfo_init_b16, + .deinit = foc_angle_onfo_deinit_b16, + .cfg = foc_angle_onfo_cfg_b16, + .zero = foc_angle_onfo_zero_b16, + .dir = foc_angle_onfo_dir_b16, + .run = foc_angle_onfo_run_b16, +}; + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: foc_angle_onfo_init_b16 + * + * Description: + * Initialize the sensorless observer FOC angle handler (fixed16) + * + * Input Parameter: + * h - pointer to FOC angle handler + * + ****************************************************************************/ + +static int foc_angle_onfo_init_b16(FAR foc_angle_b16_t *h) +{ + int ret = OK; + + DEBUGASSERT(h); + + /* Connect angle data */ + + h->data = zalloc(sizeof(struct foc_ang_onfo_b16_s)); + if (h->data == NULL) + { + ret = -ENOMEM; + goto errout; + } + +errout: + return ret; +} + +/**************************************************************************** + * Name: foc_angle_onfo_deinit_b16 + * + * Description: + * De-initialize the sensorless observer FOC angle handler (fixed16) + * + * Input Parameter: + * h - pointer to FOC angle handler + * + ****************************************************************************/ + +static void foc_angle_onfo_deinit_b16(FAR foc_angle_b16_t *h) +{ + DEBUGASSERT(h); + + if (h->data) + { + /* Free angle data */ + + free(h->data); + } +} + +/**************************************************************************** + * Name: foc_angle_onfo_cfg_b16 + * + * Description: + * Configure the sensorless observer FOC angle handler (fixed16) + * + * Input Parameter: + * h - pointer to FOC angle handler + * cfg - pointer to angle handler configuration data + * (struct foc_ang_onfo_b16_s) + * + ****************************************************************************/ + +static int foc_angle_onfo_cfg_b16(FAR foc_angle_b16_t *h, FAR void *cfg) +{ + FAR struct foc_ang_onfo_b16_s *ob = NULL; + int ret = OK; + + DEBUGASSERT(h); + + /* Get sensorless observer data */ + + DEBUGASSERT(h->data); + ob = h->data; + + /* Copy configuration */ + + memcpy(&ob->cfg, cfg, sizeof(struct foc_angle_onfo_cfg_b16_s)); + + /* Initialize sensorless observer controller data */ + + DEBUGASSERT(ob->cfg.per > 0); + + /* Initialize nolinear fluxlink angle observer */ + + motor_aobserver_nfo_init_b16(&ob->data); + + /* Initialize sensorless observer */ + + motor_aobserver_init_b16(&ob->o, &ob->data, ob->cfg.per); + + /* Initialize with CW direction */ + + ob->sensor_dir = DIR_CW_B16; + + return ret; +} + +/**************************************************************************** + * Name: foc_angle_onfo_zero_b16 + * + * Description: + * Zero the sensorless observer FOC angle handler (fixed16) + * + * Input Parameter: + * h - pointer to FOC angle handler + * + ****************************************************************************/ + +static int foc_angle_onfo_zero_b16(FAR foc_angle_b16_t *h) +{ + FAR struct foc_ang_onfo_b16_s *ob = NULL; + + DEBUGASSERT(h); + + /* Get sensorless observer data */ + + DEBUGASSERT(h->data); + ob = h->data; + + /* Reinitialize observer data */ + + motor_aobserver_nfo_init_b16(&ob->data); + + /* Reset angle */ + + ob->o.angle = 0; + + return OK; +} + +/**************************************************************************** + * Name: foc_angle_onfo_dir_b16 + * + * Description: + * Set the sensorless observer FOC angle handler direction (fixed16) + * + * Input Parameter: + * h - pointer to FOC angle handler + * dir - sensor direction (1 if normal -1 if inverted) + * + ****************************************************************************/ + +static int foc_angle_onfo_dir_b16(FAR foc_angle_b16_t *h, b16_t dir) +{ + FAR struct foc_ang_onfo_b16_s *ob = NULL; + + DEBUGASSERT(h); + + /* Get sensorless observer data */ + + DEBUGASSERT(h->data); + ob = h->data; + + /* Configure direction */ + + ob->sensor_dir = dir; + + return OK; +} + +/**************************************************************************** + * Name: foc_angle_onfo_run_b16 + * + * Description: + * Process the FOC sensorless observer angle data (fixed16) + * + * Input Parameter: + * h - pointer to FOC angle handler + * in - pointer to FOC angle handler input data + * out - pointer to FOC angle handler output data + * + ****************************************************************************/ + +static int foc_angle_onfo_run_b16(FAR foc_angle_b16_t *h, + FAR struct foc_angle_in_b16_s *in, + FAR struct foc_angle_out_b16_s *out) +{ + FAR struct foc_ang_onfo_b16_s *ob = NULL; + FAR dq_frame_b16_t v_dq_mod; + b16_t duty_now; + b16_t dyn_gain; + + DEBUGASSERT(h); + + /* Get sensorless observer data */ + + DEBUGASSERT(h->data); + ob = h->data; + + /* Normalize the d-q voltage to get the d-q modulation + * voltage + */ + + v_dq_mod.d = b16mulb16(in->state->vdq.d, in->state->mod_scale); + v_dq_mod.q = b16mulb16(in->state->vdq.q, in->state->mod_scale); + + /* Update duty cycle now */ + + duty_now = b16mulb16(SIGN(in->state->vdq.q), + vector2d_mag_b16(v_dq_mod.d, v_dq_mod.q)); + + /* Update and the observer gain. */ + + dyn_gain = b16mulb16(LINEAR_MAP(ABS(duty_now), + 0, + b16ONE, + b16mulb16(ob->cfg.gain, ob->cfg.gain_slow), + ob->cfg.gain), + b16HALF); + + /* Update observer */ + + motor_aobserver_nfo_b16(&ob->o, &in->state->iab, &in->state->vab, + &ob->cfg.phy, dyn_gain); + + /* Copy data */ + + out->type = FOC_ANGLE_TYPE_ELE; + out->angle = b16mulb16(ob->sensor_dir, + motor_aobserver_angle_get_b16(&ob->o)); + + return OK; +} diff --git a/industry/foc/fixed16/foc_ang_osmo.c b/industry/foc/fixed16/foc_ang_osmo.c new file mode 100644 index 000000000..90acd2a86 --- /dev/null +++ b/industry/foc/fixed16/foc_ang_osmo.c @@ -0,0 +1,283 @@ +/**************************************************************************** + * apps/industry/foc/fixed16/foc_ang_osmo.c + * + * Licensed to the Apache Software Foundation (ASF) under one or more + * contributor license agreements. See the NOTICE file distributed with + * this work for additional information regarding copyright ownership. The + * ASF licenses this file to you under the Apache License, Version 2.0 (the + * "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include + +#include +#include +#include + +#include "industry/foc/foc_log.h" +#include "industry/foc/fixed16/foc_angle.h" + +/**************************************************************************** + * Private Data Types + ****************************************************************************/ + +/* sensorless observer data */ + +struct foc_ang_osmo_b16_s +{ + struct foc_angle_osmo_cfg_b16_s cfg; + struct motor_aobserver_smo_b16_s data; + struct motor_aobserver_b16_s o; + b16_t sensor_dir; +}; + +/**************************************************************************** + * Private Function Prototypes + ****************************************************************************/ + +static int foc_angle_osmo_init_b16(FAR foc_angle_b16_t *h); +static void foc_angle_osmo_deinit_b16(FAR foc_angle_b16_t *h); +static int foc_angle_osmo_cfg_b16(FAR foc_angle_b16_t *h, FAR void *cfg); +static int foc_angle_osmo_zero_b16(FAR foc_angle_b16_t *h); +static int foc_angle_osmo_dir_b16(FAR foc_angle_b16_t *h, b16_t dir); +static int foc_angle_osmo_run_b16(FAR foc_angle_b16_t *h, + FAR struct foc_angle_in_b16_s *in, + FAR struct foc_angle_out_b16_s *out); + +/**************************************************************************** + * Public Data + ****************************************************************************/ + +/* FOC angle b16_t interface */ + +struct foc_angle_ops_b16_s g_foc_angle_osmo_b16 = +{ + .init = foc_angle_osmo_init_b16, + .deinit = foc_angle_osmo_deinit_b16, + .cfg = foc_angle_osmo_cfg_b16, + .zero = foc_angle_osmo_zero_b16, + .dir = foc_angle_osmo_dir_b16, + .run = foc_angle_osmo_run_b16, +}; + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: foc_angle_osmo_init_b16 + * + * Description: + * Initialize the sensorless observer FOC angle handler (fixed16) + * + * Input Parameter: + * h - pointer to FOC angle handler + * + ****************************************************************************/ + +static int foc_angle_osmo_init_b16(FAR foc_angle_b16_t *h) +{ + int ret = OK; + + DEBUGASSERT(h); + + /* Connect angle data */ + + h->data = zalloc(sizeof(struct foc_ang_osmo_b16_s)); + if (h->data == NULL) + { + ret = -ENOMEM; + goto errout; + } + +errout: + return ret; +} + +/**************************************************************************** + * Name: foc_angle_osmo_deinit_b16 + * + * Description: + * De-initialize the sensorless observer FOC angle handler (fixed16) + * + * Input Parameter: + * h - pointer to FOC angle handler + * + ****************************************************************************/ + +static void foc_angle_osmo_deinit_b16(FAR foc_angle_b16_t *h) +{ + DEBUGASSERT(h); + + if (h->data) + { + /* Free angle data */ + + free(h->data); + } +} + +/**************************************************************************** + * Name: foc_angle_osmo_cfg_b16 + * + * Description: + * Configure the sensorless observer FOC angle handler (fixed16) + * + * Input Parameter: + * h - pointer to FOC angle handler + * cfg - pointer to angle handler configuration data + * (struct foc_ang_osmo_b16_s) + * + ****************************************************************************/ + +static int foc_angle_osmo_cfg_b16(FAR foc_angle_b16_t *h, FAR void *cfg) +{ + FAR struct foc_ang_osmo_b16_s *ob = NULL; + int ret = OK; + + DEBUGASSERT(h); + + /* Get sensorless observer data */ + + DEBUGASSERT(h->data); + ob = h->data; + + /* Copy configuration */ + + memcpy(&ob->cfg, cfg, sizeof(struct foc_angle_osmo_cfg_b16_s)); + + /* Initialize sensorless observer controller data */ + + DEBUGASSERT(ob->cfg.per > 0); + + /* Initialize SMO angle observer */ + + motor_aobserver_smo_init_b16(&ob->data, ob->cfg.k_slide, ob->cfg.err_max); + + /* Initialize sensorless observer */ + + motor_aobserver_init_b16(&ob->o, &ob->data, ob->cfg.per); + + /* Initialize with CW direction */ + + ob->sensor_dir = DIR_CW_B16; + + return ret; +} + +/**************************************************************************** + * Name: foc_angle_osmo_zero_b16 + * + * Description: + * Zero the sensorless observer FOC angle handler (fixed16) + * + * Input Parameter: + * h - pointer to FOC angle handler + * + ****************************************************************************/ + +static int foc_angle_osmo_zero_b16(FAR foc_angle_b16_t *h) +{ + FAR struct foc_ang_osmo_b16_s *ob = NULL; + + DEBUGASSERT(h); + + /* Get sensorless observer data */ + + DEBUGASSERT(h->data); + ob = h->data; + + /* Reinitialize observer data */ + + motor_aobserver_smo_init_b16(&ob->data, ob->cfg.k_slide, ob->cfg.err_max); + + /* Reset angle */ + + ob->o.angle = 0; + + return OK; +} + +/**************************************************************************** + * Name: foc_angle_osmo_dir_b16 + * + * Description: + * Set the sensorless observer FOC angle handler direction (fixed16) + * + * Input Parameter: + * h - pointer to FOC angle handler + * dir - sensor direction (1 if normal -1 if inverted) + * + ****************************************************************************/ + +static int foc_angle_osmo_dir_b16(FAR foc_angle_b16_t *h, b16_t dir) +{ + FAR struct foc_ang_osmo_b16_s *ob = NULL; + + DEBUGASSERT(h); + + /* Get sensorless observer data */ + + DEBUGASSERT(h->data); + ob = h->data; + + /* Configure direction */ + + ob->sensor_dir = dir; + + return OK; +} + +/**************************************************************************** + * Name: foc_angle_osmo_run_b16 + * + * Description: + * Process the FOC sensorless observer angle data (fixed16) + * + * Input Parameter: + * h - pointer to FOC angle handler + * in - pointer to FOC angle handler input data + * out - pointer to FOC angle handler output data + * + ****************************************************************************/ + +static int foc_angle_osmo_run_b16(FAR foc_angle_b16_t *h, + FAR struct foc_angle_in_b16_s *in, + FAR struct foc_angle_out_b16_s *out) +{ + FAR struct foc_ang_osmo_b16_s *ob = NULL; + + DEBUGASSERT(h); + + /* Get sensorless observer data */ + + DEBUGASSERT(h->data); + ob = h->data; + + /* Update observer */ + + motor_aobserver_smo_b16(&ob->o, &in->state->iab, &in->state->vab, + &ob->cfg.phy, in->dir, in->vel); + + /* Copy data */ + + out->type = FOC_ANGLE_TYPE_ELE; + out->angle = b16mulb16(ob->sensor_dir, + motor_aobserver_angle_get_b16(&ob->o)); + + return OK; +} diff --git a/industry/foc/fixed16/foc_picontrol.c b/industry/foc/fixed16/foc_picontrol.c index 2540aa4ab..66ee26524 100644 --- a/industry/foc/fixed16/foc_picontrol.c +++ b/industry/foc/fixed16/foc_picontrol.c @@ -415,4 +415,8 @@ static void foc_control_state_get_b16(FAR foc_handler_b16_t *h, state->volt[0] = foc->data.v_abc.a; state->volt[1] = foc->data.v_abc.b; state->volt[2] = foc->data.v_abc.c; + + /* Copy modulation scale */ + + state->mod_scale = foc->data.vab_mod_scale; } diff --git a/industry/foc/fixed16/foc_vel_odiv.c b/industry/foc/fixed16/foc_vel_odiv.c new file mode 100644 index 000000000..5df851893 --- /dev/null +++ b/industry/foc/fixed16/foc_vel_odiv.c @@ -0,0 +1,286 @@ +/**************************************************************************** + * apps/industry/foc/fixed16/foc_vel_odiv.c + * + * Licensed to the Apache Software Foundation (ASF) under one or more + * contributor license agreements. See the NOTICE file distributed with + * this work for additional information regarding copyright ownership. The + * ASF licenses this file to you under the Apache License, Version 2.0 (the + * "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include + +#include +#include +#include + +#include + +#include "industry/foc/foc_common.h" +#include "industry/foc/foc_log.h" +#include "industry/foc/fixed16/foc_velocity.h" + +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ + +/**************************************************************************** + * Private Data Types + ****************************************************************************/ + +/* Div private data */ + +struct foc_div_b16_s +{ + struct foc_vel_div_b16_cfg_s cfg; + struct motor_sobserver_div_b16_s data; + struct motor_sobserver_b16_s o; + b16_t sensor_dir; +}; + +/**************************************************************************** + * Private Function Prototypes + ****************************************************************************/ + +static int foc_velocity_div_init_b16(FAR foc_velocity_b16_t *h); +static void foc_velocity_div_deinit_b16(FAR foc_velocity_b16_t *h); +static int foc_velocity_div_cfg_b16(FAR foc_velocity_b16_t *h, + FAR void *cfg); +static int foc_velocity_div_zero_b16(FAR foc_velocity_b16_t *h); +static int foc_velocity_div_dir_b16(FAR foc_velocity_b16_t *h, b16_t dir); +static int foc_velocity_div_run_b16(FAR foc_velocity_b16_t *h, + FAR struct foc_velocity_in_b16_s *in, + FAR struct foc_velocity_out_b16_s *out); + +/**************************************************************************** + * Public Data + ****************************************************************************/ + +/* FOC velocity b16_t interface */ + +struct foc_velocity_ops_b16_s g_foc_velocity_odiv_b16 = +{ + .init = foc_velocity_div_init_b16, + .deinit = foc_velocity_div_deinit_b16, + .cfg = foc_velocity_div_cfg_b16, + .zero = foc_velocity_div_zero_b16, + .dir = foc_velocity_div_dir_b16, + .run = foc_velocity_div_run_b16, +}; + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: foc_velocity_div_init_b16 + * + * Description: + * Initialize the DIV velocity observer (fixed16) + * + * Input Parameter: + * h - pointer to FOC velocity handler + * + ****************************************************************************/ + +static int foc_velocity_div_init_b16(FAR foc_velocity_b16_t *h) +{ + int ret = OK; + + DEBUGASSERT(h); + + /* Connect velocity data */ + + h->data = zalloc(sizeof(struct foc_div_b16_s)); + if (h->data == NULL) + { + ret = -ENOMEM; + goto errout; + } + +errout: + return ret; +} + +/**************************************************************************** + * Name: foc_velocity_div_deinit_b16 + * + * Description: + * De-initialize the DIV velocity observer (fixed16) + * + * Input Parameter: + * h - pointer to FOC velocity handler + * + ****************************************************************************/ + +static void foc_velocity_div_deinit_b16(FAR foc_velocity_b16_t *h) +{ + DEBUGASSERT(h); + + if (h->data) + { + /* Free velocity data */ + + free(h->data); + } +} + +/**************************************************************************** + * Name: foc_velocity_div_cfg_b16 + * + * Description: + * Configure the DIV velocity observer (fixed16) + * + * Input Parameter: + * h - pointer to FOC velocity handler + * cfg - pointer to velocity handler configuration data + * (struct foc_div_b16_s) + * + ****************************************************************************/ + +static int foc_velocity_div_cfg_b16(FAR foc_velocity_b16_t *h, FAR void *cfg) +{ + FAR struct foc_div_b16_s *div = NULL; + int ret = OK; + + DEBUGASSERT(h); + + /* Get div data */ + + DEBUGASSERT(h->data); + div = h->data; + + /* Copy configuration */ + + memcpy(&div->cfg, cfg, sizeof(struct foc_vel_div_b16_cfg_s)); + + /* Configure observer */ + + motor_sobserver_div_init_b16(&div->data, + div->cfg.samples, + div->cfg.filter, + div->cfg.per); + + motor_sobserver_init_b16(&div->o, &div->data, div->cfg.per); + + /* Initialize with CW direction */ + + div->sensor_dir = DIR_CW_B16; + + return ret; +} + +/**************************************************************************** + * Name: foc_velocity_div_zero_b16 + * + * Description: + * Zero the DIV velocity observer (fixed16) + * + * Input Parameter: + * h - pointer to FOC velocity handler + * + ****************************************************************************/ + +static int foc_velocity_div_zero_b16(FAR foc_velocity_b16_t *h) +{ + FAR struct foc_div_b16_s *div = NULL; + int ret = OK; + + DEBUGASSERT(h); + + /* Get div data */ + + DEBUGASSERT(h->data); + div = h->data; + + /* Reinitialize observer */ + + motor_sobserver_div_init_b16(&div->data, + div->cfg.samples, + div->cfg.filter, + div->cfg.per); + + return ret; +} + +/**************************************************************************** + * Name: foc_velocity_div_dir_b16 + * + * Description: + * Set the DIV velocity observer direction (fixed16) + * + * Input Parameter: + * h - pointer to FOC velocity handler + * dir - sensor direction (1 if normal -1 if inverted) + * + ****************************************************************************/ + +static int foc_velocity_div_dir_b16(FAR foc_velocity_b16_t *h, b16_t dir) +{ + FAR struct foc_div_b16_s *div = NULL; + + DEBUGASSERT(h); + + /* Get div data */ + + DEBUGASSERT(h->data); + div = h->data; + + /* Set direction */ + + div->sensor_dir = dir; + + return OK; +} + +/**************************************************************************** + * Name: foc_velocity_div_run_b16 + * + * Description: + * Process the DIV velocity observer (fixed16) + * + * Input Parameter: + * h - pointer to FOC velocity handler + * in - pointer to FOC velocity handler input data + * out - pointer to FOC velocity handler output data + * + ****************************************************************************/ + +static int foc_velocity_div_run_b16(FAR foc_velocity_b16_t *h, + FAR struct foc_velocity_in_b16_s *in, + FAR struct foc_velocity_out_b16_s *out) +{ + FAR struct foc_div_b16_s *div = NULL; + + DEBUGASSERT(h); + + /* Get div data */ + + DEBUGASSERT(h->data); + div = h->data; + + /* Run observer */ + + motor_sobserver_div_b16(&div->o, in->angle); + + /* Copy data */ + + out->velocity = b16mulb16(div->sensor_dir, + motor_sobserver_speed_get_b16(&div->o)); + + return OK; +} diff --git a/industry/foc/fixed16/foc_vel_opll.c b/industry/foc/fixed16/foc_vel_opll.c new file mode 100644 index 000000000..b16cbee1d --- /dev/null +++ b/industry/foc/fixed16/foc_vel_opll.c @@ -0,0 +1,284 @@ +/**************************************************************************** + * apps/industry/foc/fixed16/foc_vel_opll.c + * + * Licensed to the Apache Software Foundation (ASF) under one or more + * contributor license agreements. See the NOTICE file distributed with + * this work for additional information regarding copyright ownership. The + * ASF licenses this file to you under the Apache License, Version 2.0 (the + * "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include + +#include +#include +#include + +#include + +#include "industry/foc/foc_common.h" +#include "industry/foc/foc_log.h" +#include "industry/foc/fixed16/foc_velocity.h" + +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ + +/**************************************************************************** + * Private Data Types + ****************************************************************************/ + +/* PLL observer private data */ + +struct foc_pll_b16_s +{ + struct foc_vel_pll_b16_cfg_s cfg; + struct motor_sobserver_pll_b16_s data; + struct motor_sobserver_b16_s o; + b16_t sensor_dir; +}; + +/**************************************************************************** + * Private Function Prototypes + ****************************************************************************/ + +static int foc_velocity_pll_init_b16(FAR foc_velocity_b16_t *h); +static void foc_velocity_pll_deinit_b16(FAR foc_velocity_b16_t *h); +static int foc_velocity_pll_cfg_b16(FAR foc_velocity_b16_t *h, + FAR void *cfg); +static int foc_velocity_pll_zero_b16(FAR foc_velocity_b16_t *h); +static int foc_velocity_pll_dir_b16(FAR foc_velocity_b16_t *h, b16_t dir); +static int foc_velocity_pll_run_b16(FAR foc_velocity_b16_t *h, + FAR struct foc_velocity_in_b16_s *in, + FAR struct foc_velocity_out_b16_s *out); + +/**************************************************************************** + * Public Data + ****************************************************************************/ + +/* FOC velocity b16_t interface */ + +struct foc_velocity_ops_b16_s g_foc_velocity_opll_b16 = +{ + .init = foc_velocity_pll_init_b16, + .deinit = foc_velocity_pll_deinit_b16, + .cfg = foc_velocity_pll_cfg_b16, + .zero = foc_velocity_pll_zero_b16, + .dir = foc_velocity_pll_dir_b16, + .run = foc_velocity_pll_run_b16, +}; + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: foc_velocity_pll_init_b16 + * + * Description: + * Initialize the PLL velocity observer (fixed16) + * + * Input Parameter: + * h - pointer to FOC velocity handler + * + ****************************************************************************/ + +static int foc_velocity_pll_init_b16(FAR foc_velocity_b16_t *h) +{ + int ret = OK; + + DEBUGASSERT(h); + + /* Connect velocity data */ + + h->data = zalloc(sizeof(struct foc_pll_b16_s)); + if (h->data == NULL) + { + ret = -ENOMEM; + goto errout; + } + +errout: + return ret; +} + +/**************************************************************************** + * Name: foc_velocity_pll_deinit_b16 + * + * Description: + * De-initialize the PLL velocity observer (fixed16) + * + * Input Parameter: + * h - pointer to FOC velocity handler + * + ****************************************************************************/ + +static void foc_velocity_pll_deinit_b16(FAR foc_velocity_b16_t *h) +{ + DEBUGASSERT(h); + + if (h->data) + { + /* Free velocity data */ + + free(h->data); + } +} + +/**************************************************************************** + * Name: foc_velocity_pll_cfg_b16 + * + * Description: + * Configure the PLL velocity observer (fixed16) + * + * Input Parameter: + * h - pointer to FOC velocity handler + * cfg - pointer to velocity handler configuration data + * (struct foc_pll_b16_s) + * + ****************************************************************************/ + +static int foc_velocity_pll_cfg_b16(FAR foc_velocity_b16_t *h, FAR void *cfg) +{ + FAR struct foc_pll_b16_s *pll = NULL; + int ret = OK; + + DEBUGASSERT(h); + + /* Get pll data */ + + DEBUGASSERT(h->data); + pll = h->data; + + /* Copy configuration */ + + memcpy(&pll->cfg, cfg, sizeof(struct foc_vel_pll_b16_cfg_s)); + + /* Configure observer */ + + motor_sobserver_pll_init_b16(&pll->data, + pll->cfg.kp, + pll->cfg.ki); + + motor_sobserver_init_b16(&pll->o, &pll->data, pll->cfg.per); + + /* Initialize with CW direction */ + + pll->sensor_dir = DIR_CW_B16; + + return ret; +} + +/**************************************************************************** + * Name: foc_velocity_pll_zero_b16 + * + * Description: + * Zero the DIV velocity observer (fixed16) + * + * Input Parameter: + * h - pointer to FOC velocity handler + * + ****************************************************************************/ + +static int foc_velocity_pll_zero_b16(FAR foc_velocity_b16_t *h) +{ + FAR struct foc_pll_b16_s *pll = NULL; + int ret = OK; + + DEBUGASSERT(h); + + /* Get pll data */ + + DEBUGASSERT(h->data); + pll = h->data; + + /* Reinitialize observer */ + + motor_sobserver_pll_init_b16(&pll->data, + pll->cfg.kp, + pll->cfg.ki); + + return ret; +} + +/**************************************************************************** + * Name: foc_velocity_pll_dir_b16 + * + * Description: + * Set the PLL velocity observer direction (fixed16) + * + * Input Parameter: + * h - pointer to FOC velocity handler + * dir - sensor direction (1 if normal -1 if inverted) + * + ****************************************************************************/ + +static int foc_velocity_pll_dir_b16(FAR foc_velocity_b16_t *h, b16_t dir) +{ + FAR struct foc_pll_b16_s *pll = NULL; + + DEBUGASSERT(h); + + /* Get pll data */ + + DEBUGASSERT(h->data); + pll = h->data; + + /* Set direction */ + + pll->sensor_dir = dir; + + return OK; +} + +/**************************************************************************** + * Name: foc_velocity_pll_run_b16 + * + * Description: + * Process the PLL velocity observer (fixed16) + * + * Input Parameter: + * h - pointer to FOC velocity handler + * in - pointer to FOC velocity handler input data + * out - pointer to FOC velocity handler output data + * + ****************************************************************************/ + +static int foc_velocity_pll_run_b16(FAR foc_velocity_b16_t *h, + FAR struct foc_velocity_in_b16_s *in, + FAR struct foc_velocity_out_b16_s *out) +{ + FAR struct foc_pll_b16_s *pll = NULL; + + DEBUGASSERT(h); + + /* Get pll data */ + + DEBUGASSERT(h->data); + pll = h->data; + + /* Run observer */ + + motor_sobserver_pll_b16(&pll->o, in->angle); + + /* Copy data */ + + out->velocity = b16mulb16(pll->sensor_dir, + motor_sobserver_speed_get_b16(&pll->o)); + + return OK; +}