sergiotarxz/nuttx-apps
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

foclib/fixed16: add support for observers

Browse Source
This commit is contained in:
raiden00pl 2022-02-20 12:54:51 +01:00 committed by Xiang Xiao
parent 52beb5f717
commit fbb09ff128
9 changed files with 1261 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

32
include/industry/foc/fixed16/foc_angle.h
View File

@ -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) */

1
include/industry/foc/fixed16/foc_handler.h
View File

@ -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 */

38
include/industry/foc/fixed16/foc_velocity.h
View File

@ -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
****************************************************************************/

12
industry/foc/Makefile
View File

@ -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

321
industry/foc/fixed16/foc_ang_onfo.c Normal file
View File

@ -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 <nuttx/config.h>
#include <assert.h>
#include <errno.h>
#include <stdlib.h>
#include "industry/foc/foc_log.h"
#include "industry/foc/fixed16/foc_angle.h"
/****************************************************************************
* 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;
}

283
industry/foc/fixed16/foc_ang_osmo.c Normal file
View File

@ -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 <nuttx/config.h>
#include <assert.h>
#include <errno.h>
#include <stdlib.h>
#include "industry/foc/foc_log.h"
#include "industry/foc/fixed16/foc_angle.h"
/****************************************************************************
* Private 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;
}

4
industry/foc/fixed16/foc_picontrol.c
View File

@ -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;
}

286
industry/foc/fixed16/foc_vel_odiv.c Normal file
View File

@ -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 <nuttx/config.h>
#include <assert.h>
#include <errno.h>
#include <stdlib.h>
#include <dspb16.h>
#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;
}

284
industry/foc/fixed16/foc_vel_opll.c Normal file
View File

@ -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 <nuttx/config.h>
#include <assert.h>
#include <errno.h>
#include <stdlib.h>
#include <dspb16.h>
#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;
}