420 lines
12 KiB
C
420 lines
12 KiB
C
/****************************************************************************
|
|
* 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 <nuttx/config.h>
|
|
|
|
#include <assert.h>
|
|
#include <errno.h>
|
|
#include <stdlib.h>
|
|
#include <string.h>
|
|
|
|
#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_ANGLE
|
|
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_DQSAT
|
|
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_DQSAT
|
|
dq_saturate(&v_dq_ref, mag_max);
|
|
#else
|
|
foc_cordic_dqsat_f32(h->fd, &v_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;
|
|
}
|