d75d6b7af0
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
301 lines
8.8 KiB
C
301 lines
8.8 KiB
C
/****************************************************************************
|
|
* 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 <nuttx/config.h>
|
|
|
|
#include <assert.h>
|
|
#include <errno.h>
|
|
#include <stdlib.h>
|
|
|
|
#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));
|
|
}
|