sergiotarxz/nuttx-apps
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity
144bf0bd1f
nuttx-apps/examples/foc/foc_motor_f32.c

2084 lines
50 KiB
C
Raw Blame History

/****************************************************************************
* apps/examples/foc/foc_motor_f32.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 <string.h>
#include <dsp.h>
#include "foc_cfg.h"
#include "foc_debug.h"
#include "foc_motor_f32.h"
#ifdef CONFIG_EXAMPLES_FOC_FEEDFORWARD
# include "industry/foc/float/foc_feedforward.h"
#endif
/****************************************************************************
* Pre-processor Definitions
****************************************************************************/
#define FOC_FLOAT_IDENT_RES_MIN (1e-6)
#define FOC_FLOAT_IDENT_RES_MAX (2.0f)
#define FOC_FLOAT_IDENT_IND_MIN (1e-9)
#define FOC_FLOAT_IDENT_IND_MAX (2.0f)
/****************************************************************************
* Private Type Definition
****************************************************************************/
/****************************************************************************
* Private Functions
****************************************************************************/
#ifdef CONFIG_EXAMPLES_FOC_HAVE_ALIGN
/****************************************************************************
* Name: foc_align_dir_cb
****************************************************************************/
static int foc_align_zero_cb(FAR void *priv, float offset)
{
FAR foc_angle_f32_t *angle = (FAR foc_angle_f32_t *)priv;
DEBUGASSERT(angle);
UNUSED(offset);
return foc_angle_zero_f32(angle);
}
/****************************************************************************
* Name: foc_align_dir_cb
****************************************************************************/
static int foc_align_dir_cb(FAR void *priv, float dir)
{
FAR foc_angle_f32_t *angle = (FAR foc_angle_f32_t *)priv;
DEBUGASSERT(angle);
return foc_angle_dir_f32(angle, dir);
}
/****************************************************************************
* Name: foc_motor_align
****************************************************************************/
static int foc_motor_align(FAR struct foc_motor_f32_s *motor, FAR bool *done)
{
struct foc_routine_in_f32_s in;
struct foc_routine_out_f32_s out;
struct foc_routine_aling_final_f32_s final;
int ret = OK;
/* Get input */
in.foc_state = &motor->foc_state;
in.angle = motor->angle_now;
in.angle_m = motor->angle_m;
#ifdef CONFIG_EXAMPLES_FOC_HAVE_VEL
in.vel = motor->vel.now;
#endif
in.vbus = motor->vbus;
/* Run align procedure */
ret = foc_routine_run_f32(&motor->align, &in, &out);
if (ret < 0)
{
PRINTFV("ERROR: foc_routine_run_f32 failed %d!\n", ret);
goto errout;
}
if (ret == FOC_ROUTINE_RUN_DONE)
{
ret = foc_routine_final_f32(&motor->align, &final);
if (ret < 0)
{
PRINTFV("ERROR: foc_routine_final_f32 failed %d!\n", ret);
goto errout;
}
PRINTF("Aling results:\n");
#ifdef CONFIG_INDUSTRY_FOC_ALIGN_DIR
PRINTF(" dir = %.2f\n", final.dir);
#endif
PRINTF(" offset = %.2f\n", final.offset);
*done = true;
}
/* Copy output */
motor->dq_ref.d = out.dq_ref.d;
motor->dq_ref.q = out.dq_ref.q;
motor->vdq_comp.d = out.vdq_comp.d;
motor->vdq_comp.q = out.vdq_comp.q;
motor->angle_now = out.angle;
motor->foc_mode = out.foc_mode;
errout:
return ret;
}
#endif
#ifdef CONFIG_EXAMPLES_FOC_HAVE_IDENT
/****************************************************************************
* Name: foc_motor_ident
****************************************************************************/
static int foc_motor_ident(FAR struct foc_motor_f32_s *motor, FAR bool *done)
{
struct foc_routine_in_f32_s in;
struct foc_routine_out_f32_s out;
struct foc_routine_ident_final_f32_s final;
int ret = OK;
/* Get input */
in.foc_state = &motor->foc_state;
in.angle = motor->angle_now;
#ifdef CONFIG_EXAMPLES_FOC_HAVE_VEL
in.vel = motor->vel.now;
#endif
in.vbus = motor->vbus;
/* Run ident procedure */
ret = foc_routine_run_f32(&motor->ident, &in, &out);
if (ret < 0)
{
PRINTFV("ERROR: foc_routine_run_f32 failed %d!\n", ret);
goto errout;
}
if (ret == FOC_ROUTINE_RUN_DONE)
{
ret = foc_routine_final_f32(&motor->ident, &final);
if (ret < 0)
{
PRINTFV("ERROR: foc_routine_final_f32 failed %d!\n", ret);
goto errout;
}
PRINTF("Ident results:\n");
PRINTF(" res = %.4f\n", final.res);
PRINTF(" ind = %.8f\n", final.ind);
if (final.res < FOC_FLOAT_IDENT_RES_MIN ||
final.res > FOC_FLOAT_IDENT_RES_MAX)
{
PRINTF("ERROR: Motor resistance out of valid range res=%.4f!\n",
final.res);
ret = -EINVAL;
goto errout;
}
if (final.ind < FOC_FLOAT_IDENT_IND_MIN ||
final.ind > FOC_FLOAT_IDENT_IND_MAX)
{
PRINTF("ERROR: Motor inductance out of valid range ind=%.8f!\n",
final.ind);
ret = -EINVAL;
goto errout;
}
/* Store results */
motor->phy_ident.res = final.res;
motor->phy_ident.ind = final.ind;
*done = true;
}
/* Copy output */
motor->dq_ref.d = out.dq_ref.d;
motor->dq_ref.q = out.dq_ref.q;
motor->vdq_comp.d = out.vdq_comp.d;
motor->vdq_comp.q = out.vdq_comp.q;
motor->angle_now = out.angle;
motor->foc_mode = out.foc_mode;
errout:
return ret;
}
#endif
#ifdef CONFIG_EXAMPLES_FOC_HAVE_RUN
/****************************************************************************
* Name: foc_runmode_init
****************************************************************************/
static int foc_runmode_init(FAR struct foc_motor_f32_s *motor)
{
int ret = OK;
switch (motor->envp->cfg->fmode)
{
case FOC_FMODE_IDLE:
{
motor->foc_mode_run = FOC_HANDLER_MODE_IDLE;
break;
}
case FOC_FMODE_VOLTAGE:
{
motor->foc_mode_run = FOC_HANDLER_MODE_VOLTAGE;
break;
}
case FOC_FMODE_CURRENT:
{
motor->foc_mode_run = FOC_HANDLER_MODE_CURRENT;
break;
}
default:
{
PRINTF("ERROR: unsupported op mode %d\n", motor->envp->cfg->fmode);
ret = -EINVAL;
goto errout;
}
}
/* Force open-loop if sensorless */
#ifdef CONFIG_EXAMPLES_FOC_SENSORLESS
# ifdef CONFIG_EXAMPLES_FOC_HAVE_OPENLOOP
motor->openloop_now = FOC_OPENLOOP_ENABLED;
# else
# error
# endif
#endif
errout:
return ret;
}
#endif
/****************************************************************************
* Name: foc_motor_configure
****************************************************************************/
static int foc_motor_configure(FAR struct foc_motor_f32_s *motor)
{
FAR struct foc_control_ops_f32_s *foc_ctrl = NULL;
FAR struct foc_modulation_ops_f32_s *foc_mod = NULL;
#ifdef CONFIG_EXAMPLES_FOC_CONTROL_PI
struct foc_initdata_f32_s ctrl_cfg;
#endif
#ifdef CONFIG_INDUSTRY_FOC_MODULATION_SVM3
struct foc_mod_cfg_f32_s mod_cfg;
#endif
#ifdef CONFIG_EXAMPLES_FOC_STATE_USE_MODEL_PMSM
struct foc_model_pmsm_cfg_f32_s pmsm_cfg;
#endif
int ret = OK;
DEBUGASSERT(motor);
#ifdef CONFIG_EXAMPLES_FOC_HAVE_VEL
/* Initialize velocity ramp */
ret = foc_ramp_init_f32(&motor->ramp,
motor->per,
RAMP_CFG_THR,
(motor->envp->cfg->acc / 1.0f),
(motor->envp->cfg->dec / 1.0f));
if (ret < 0)
{
PRINTF("ERROR: foc_ramp_init failed %d\n", ret);
goto errout;
}
#endif
/* Get FOC controller */
#ifdef CONFIG_EXAMPLES_FOC_CONTROL_PI
foc_ctrl = &g_foc_control_pi_f32;
#else
# error FOC controller not selected
#endif
/* Get FOC modulation */
#ifdef CONFIG_INDUSTRY_FOC_MODULATION_SVM3
foc_mod = &g_foc_mod_svm3_f32;
#else
# error FOC modulation not selected
#endif
/* Initialize FOC handler */
DEBUGASSERT(foc_ctrl != NULL);
DEBUGASSERT(foc_mod != NULL);
ret = foc_handler_init_f32(&motor->handler,
foc_ctrl,
foc_mod);
if (ret < 0)
{
PRINTF("ERROR: foc_handler_init failed %d\n", ret);
goto errout;
}
#ifdef CONFIG_EXAMPLES_FOC_CONTROL_PI
/* Get PI controller configuration */
ctrl_cfg.id_kp = (motor->envp->cfg->foc_pi_kp / 1000.0f);
ctrl_cfg.id_ki = (motor->envp->cfg->foc_pi_ki / 1000.0f);
ctrl_cfg.iq_kp = (motor->envp->cfg->foc_pi_kp / 1000.0f);
ctrl_cfg.iq_ki = (motor->envp->cfg->foc_pi_ki / 1000.0f);
#endif
#ifdef CONFIG_INDUSTRY_FOC_MODULATION_SVM3
/* Get SVM3 modulation configuration */
mod_cfg.pwm_duty_max = motor->pwm_duty_max;
#endif
/* Configure FOC handler */
foc_handler_cfg_f32(&motor->handler, &ctrl_cfg, &mod_cfg);
/* Configure motor phy */
motor_phy_params_init(&motor->phy,
CONFIG_EXAMPLES_FOC_MOTOR_POLES,
CONFIG_EXAMPLES_FOC_MOTOR_RES / 1000000.0f,
CONFIG_EXAMPLES_FOC_MOTOR_IND / 1000000.0f,
CONFIG_EXAMPLES_FOC_MOTOR_FLUXLINK / 1000000.0f);
#ifdef CONFIG_EXAMPLES_FOC_STATE_USE_MODEL_PMSM
/* Initialize PMSM model */
ret = foc_model_init_f32(&motor->model,
&g_foc_model_pmsm_ops_f32);
if (ret < 0)
{
PRINTF("ERROR: foc_model_init failed %d\n", ret);
goto errout;
}
/* Get PMSM model configuration */
pmsm_cfg.poles = FOC_MODEL_POLES;
pmsm_cfg.res = FOC_MODEL_RES;
pmsm_cfg.ind = FOC_MODEL_IND;
pmsm_cfg.iner = FOC_MODEL_INER;
pmsm_cfg.flux_link = FOC_MODEL_FLUX;
pmsm_cfg.ind_d = FOC_MODEL_INDD;
pmsm_cfg.ind_q = FOC_MODEL_INDQ;
pmsm_cfg.per = motor->per;
pmsm_cfg.iphase_adc = motor->iphase_adc;
/* Configure PMSM model */
foc_model_cfg_f32(&motor->model, &pmsm_cfg);
#endif
errout:
return ret;
}
/****************************************************************************
* Name: foc_motor_vbus
****************************************************************************/
static int foc_motor_vbus(FAR struct foc_motor_f32_s *motor, uint32_t vbus)
{
DEBUGASSERT(motor);
/* Update motor VBUS */
motor->vbus = (vbus * VBUS_ADC_SCALE);
return OK;
}
#ifdef CONFIG_EXAMPLES_FOC_HAVE_TORQ
/****************************************************************************
* Name: foc_motor_torq
****************************************************************************/
static int foc_motor_torq(FAR struct foc_motor_f32_s *motor, uint32_t torq)
{
DEBUGASSERT(motor);
/* Update motor torque destination */
motor->torq.des = (motor->dir * torq * SETPOINT_INTF_SCALE *
motor->envp->cfg->torqmax / 1000.0f);
return OK;
}
#endif
#ifdef CONFIG_EXAMPLES_FOC_HAVE_VEL
/****************************************************************************
* Name: foc_motor_vel
****************************************************************************/
static int foc_motor_vel(FAR struct foc_motor_f32_s *motor, uint32_t vel)
{
DEBUGASSERT(motor);
/* Update motor velocity destination */
motor->vel.des = (motor->dir * vel * SETPOINT_INTF_SCALE *
motor->envp->cfg->velmax / 1000.0f);
return OK;
}
#endif
#ifdef CONFIG_EXAMPLES_FOC_HAVE_POS
/****************************************************************************
* Name: foc_motor_pos
****************************************************************************/
static int foc_motor_pos(FAR struct foc_motor_f32_s *motor, uint32_t pos)
{
DEBUGASSERT(motor);
/* Update motor position destination */
motor->pos.des = (motor->dir * pos * SETPOINT_INTF_SCALE *
motor->envp->cfg->posmax / 1000.0f);
return OK;
}
#endif
/****************************************************************************
* Name: foc_motor_setpoint
****************************************************************************/
static int foc_motor_setpoint(FAR struct foc_motor_f32_s *motor, uint32_t sp)
{
int ret = OK;
switch (motor->envp->cfg->mmode)
{
#ifdef CONFIG_EXAMPLES_FOC_HAVE_TORQ
case FOC_MMODE_TORQ:
{
/* Update motor torque destination */
ret = foc_motor_torq(motor, sp);
if (ret < 0)
{
PRINTF("ERROR: foc_motor_torq failed %d!\n", ret);
goto errout;
}
break;
}
#endif
#ifdef CONFIG_EXAMPLES_FOC_HAVE_VEL
case FOC_MMODE_VEL:
{
/* Update motor velocity destination */
ret = foc_motor_vel(motor, sp);
if (ret < 0)
{
PRINTF("ERROR: foc_motor_vel failed %d!\n", ret);
goto errout;
}
break;
}
#endif
#ifdef CONFIG_EXAMPLES_FOC_HAVE_POS
case FOC_MMODE_POS:
{
/* Update motor position destination */
ret = foc_motor_pos(motor, sp);
if (ret < 0)
{
PRINTF("ERROR: foc_motor_pos failed %d!\n", ret);
goto errout;
}
break;
}
#endif
#ifdef CONFIG_EXAMPLES_FOC_HAVE_ALIGN
case FOC_MMODE_ALIGN_ONLY:
#endif
#ifdef CONFIG_EXAMPLES_FOC_HAVE_IDENT
case FOC_MMODE_IDENT_ONLY:
#endif
case FOC_MMODE_IDLE:
{
/* Do nothing */
break;
}
default:
{
PRINTF("ERROR: unsupported ctrl mode %d\n",
motor->envp->cfg->mmode);
ret = -EINVAL;
goto errout;
}
}
errout:
return ret;
}
/****************************************************************************
* Name: foc_motor_vel_reset
****************************************************************************/
static int foc_motor_vel_reset(FAR struct foc_motor_f32_s *motor)
{
int ret = OK;
/* Reset velocity observer */
#ifdef CONFIG_EXAMPLES_FOC_VELOBS_DIV
ret = foc_velocity_zero_f32(&motor->vel_div);
if (ret < 0)
{
PRINTF("ERROR: foc_velocity_zero failed %d\n", ret);
goto errout;
}
#endif
#ifdef CONFIG_EXAMPLES_FOC_VELOBS_PLL
ret = foc_velocity_zero_f32(&motor->vel_pll);
if (ret < 0)
{
PRINTF("ERROR: foc_velocity_zero failed %d\n", ret);
goto errout;
}
#endif
#ifdef CONFIG_EXAMPLES_FOC_VELOBS
errout:
#endif
return ret;
}
/****************************************************************************
* Name: foc_motor_state
****************************************************************************/
static int foc_motor_state(FAR struct foc_motor_f32_s *motor, int state)
{
int ret = OK;
DEBUGASSERT(motor);
/* Update motor state - this function is called every controller cycle */
switch (state)
{
case FOC_EXAMPLE_STATE_FREE:
{
motor->dir = DIR_NONE;
/* Force DQ vector to zero */
motor->dq_ref.q = 0.0f;
motor->dq_ref.d = 0.0f;
break;
}
case FOC_EXAMPLE_STATE_STOP:
{
#ifdef CONFIG_EXAMPLES_FOC_SENSORLESS
/* For sensorless we can just set Q reference to lock the motor */
motor->dir = DIR_NONE;
/* DQ vector not zero - active brake */
motor->dq_ref.q = CONFIG_EXAMPLES_FOC_STOP_CURRENT / 1000.0f;
motor->dq_ref.d = 0.0f;
#else
/* For sensored mode we set requested velocity to 0 */
# ifdef CONFIG_EXAMPLES_FOC_HAVE_VEL
motor->vel.des = 0.0f;
# else
# error STOP state for sensored mode requires velocity support
# endif
#endif
break;
}
case FOC_EXAMPLE_STATE_CW:
{
motor->dir = DIR_CW;
break;
}
case FOC_EXAMPLE_STATE_CCW:
{
motor->dir = DIR_CCW;
break;
}
default:
{
ret = -EINVAL;
goto errout;
}
}
#ifdef CONFIG_EXAMPLES_FOC_HAVE_OPENLOOP
/* Re-align motor if we change mode from FREE/STOP to CW/CCW otherwise,
* the open-loop may fail because the rotor position at the start is
* random.
*/
if ((motor->mq.app_state == FOC_EXAMPLE_STATE_FREE ||
motor->mq.app_state == FOC_EXAMPLE_STATE_STOP) &&
(state == FOC_EXAMPLE_STATE_CW ||
state == FOC_EXAMPLE_STATE_CCW))
{
motor->ctrl_state = FOC_CTRL_STATE_ALIGN;
motor->align_done = false;
motor->angle_now = 0.0f;
/* Reset velocity observer */
foc_motor_vel_reset(motor);
}
#endif
/* Reset current setpoint */
#ifdef CONFIG_EXAMPLES_FOC_HAVE_TORQ
motor->torq.set = 0.0f;
#endif
#ifdef CONFIG_EXAMPLES_FOC_HAVE_VEL
motor->vel.set = 0.0f;
#endif
#ifdef CONFIG_EXAMPLES_FOC_HAVE_POS
motor->pos.set = 0.0f;
#endif
errout:
return ret;
}
/****************************************************************************
* Name: foc_motor_start
****************************************************************************/
static int foc_motor_start(FAR struct foc_motor_f32_s *motor, bool start)
{
int ret = OK;
DEBUGASSERT(motor);
if (start == true)
{
/* Start motor if VBUS data present */
if (motor->mq.vbus > 0)
{
/* Configure motor controller */
PRINTF("Configure motor %d!\n", motor->envp->id);
ret = foc_motor_configure(motor);
if (ret < 0)
{
PRINTF("ERROR: foc_motor_configure failed %d!\n", ret);
goto errout;
}
/* Start/stop FOC dev request */
motor->startstop = true;
}
else
{
/* Return error if no VBUS data */
PRINTF("ERROR: start request without VBUS!\n");
goto errout;
}
}
else
{
/* Start/stop FOC dev request */
motor->startstop = true;
}
errout:
return ret;
}
#ifdef CONFIG_EXAMPLES_FOC_HAVE_RUN
/****************************************************************************
* Name: foc_motor_run_init
****************************************************************************/
static int foc_motor_run_init(FAR struct foc_motor_f32_s *motor)
{
int ret = OK;
#ifdef CONFIG_EXAMPLES_FOC_VELOBS
ret = foc_motor_vel_reset(motor);
#endif
return ret;
}
#ifdef CONFIG_EXAMPLES_FOC_ANGOBS
/****************************************************************************
* Name: foc_motor_openloop_trans
****************************************************************************/
static void foc_motor_openloop_trans(FAR struct foc_motor_f32_s *motor)
{
#ifdef CONFIG_EXAMPLES_FOC_VELCTRL_PI
/* Set the intergral part of the velocity PI controller equal to the
* open-loop Q current value.
*
* REVISIT: this may casue a velocity overshoot when going from open-loop
* to closed-loop. We can either use part of the open-loop Q
* current here or gradually reduce the Q current during
* transition.
*/
motor->vel_pi.part[1] = motor->dir * motor->openloop_q;
motor->vel_pi.part[0] = 0.0f;
#endif
}
/****************************************************************************
* Name: foc_motor_openloop_angobs
****************************************************************************/
static void foc_motor_openloop_angobs(FAR struct foc_motor_f32_s *motor)
{
float vel_abs = 0.0f;
vel_abs = fabsf(motor->vel_el);
/* Disable open-loop if velocity above threshold */
if (motor->openloop_now == FOC_OPENLOOP_ENABLED)
{
if (vel_abs >= motor->ol_thr)
{
/* Store angle error between the forced open-loop angle and
* observer output. The error will be gradually eliminated over
* the next controller cycles.
*/
#ifdef ANGLE_MERGE_FACTOR
motor->angle_err = motor->angle_ol - motor->angle_obs;
motor->angle_step = (motor->angle_err * ANGLE_MERGE_FACTOR);
#endif
motor->openloop_now = FOC_OPENLOOP_TRANSITION;
}
}
/* Handle transition end */
else if (motor->openloop_now == FOC_OPENLOOP_TRANSITION)
{
if (motor->angle_err == 0.0f)
{
/* Call open-open loop transition handler */
foc_motor_openloop_trans(motor);
motor->openloop_now = FOC_OPENLOOP_DISABLED;
}
}
/* Enable open-loop if velocity below threshold with hysteresis */
else if (motor->openloop_now == FOC_OPENLOOP_DISABLED)
{
/* For better stability we add hysteresis from transition
* from closed-loop to open-loop.
*/
if (vel_abs < (motor->ol_thr - motor->ol_hys))
{
motor->openloop_now = FOC_OPENLOOP_ENABLED;
}
}
}
#endif
/****************************************************************************
* Name: foc_motor_run
****************************************************************************/
static int foc_motor_run(FAR struct foc_motor_f32_s *motor)
{
#ifdef CONFIG_EXAMPLES_FOC_HAVE_VEL
float vel_err = 0.0f;
#endif
float q_ref = 0.0f;
float d_ref = 0.0f;
int ret = OK;
DEBUGASSERT(motor);
#ifdef CONFIG_EXAMPLES_FOC_ANGOBS
if (motor->envp->cfg->ol_force == false)
{
/* Handle open-loop to observer transition */
foc_motor_openloop_angobs(motor);
}
#endif
#ifdef CONFIG_EXAMPLES_FOC_HAVE_OPENLOOP
/* Open-loop works only in velocity control mode */
if (motor->openloop_now == FOC_OPENLOOP_ENABLED)
{
# ifdef CONFIG_EXAMPLES_FOC_HAVE_VEL
if (motor->envp->cfg->mmode != FOC_MMODE_VEL)
# endif
{
PRINTF("ERROR: open-loop only with FOC_MMODE_VEL\n");
ret = -EINVAL;
goto errout;
}
}
#endif
/* Get previous DQ */
q_ref = motor->dq_ref.q;
d_ref = motor->dq_ref.d;
/* Ignore controller if motor is free (sensorless and sensored mode)
* or stopped (only sensorless mode)
*/
if (motor->mq.app_state == FOC_EXAMPLE_STATE_FREE
#ifdef CONFIG_EXAMPLES_FOC_SENSORLESS
|| motor->mq.app_state == FOC_EXAMPLE_STATE_STOP
#endif
)
{
goto no_controller;
}
/* Controller.
*
* The FOC motor controller is a cascade controller:
*
* 1. Position controller sets requested velocity,
* 2. Velocity controller sets requested torque,
* 3. Torque controller sets requested motor phase voltages.
*
* NOTE: the motor torque is directly proportional to the motor
* current which is proportional to the motor set voltage
*/
switch (motor->envp->cfg->mmode)
{
#ifdef CONFIG_EXAMPLES_FOC_HAVE_VEL
case FOC_MMODE_VEL:
{
/* Saturate velocity */
f_saturate(&motor->vel.des, -motor->vel_sat,
motor->vel_sat);
/* Velocity controller */
if (motor->time % VEL_CONTROL_PRESCALER == 0)
{
/* Run velocity ramp controller */
ret = foc_ramp_run_f32(&motor->ramp,
motor->vel.des,
motor->vel.now,
&motor->vel.set);
if (ret < 0)
{
PRINTF("ERROR: foc_ramp_run failed %d\n", ret);
goto errout;
}
/* Run velocity controller if no in open-loop */
#ifdef CONFIG_EXAMPLES_FOC_HAVE_OPENLOOP
if (motor->openloop_now == FOC_OPENLOOP_DISABLED)
#endif
{
/* Get velocity error */
vel_err = motor->vel.set - motor->vel.now;
#ifdef CONFIG_EXAMPLES_FOC_VELCTRL_PI
/* PI velocit controller */
motor->torq.des = pi_controller(&motor->vel_pi,
vel_err);
#else
# error Missing velocity controller
#endif
}
}
/* Don't break here! pass to torque controller */
}
#endif
#ifdef CONFIG_EXAMPLES_FOC_HAVE_TORQ
case FOC_MMODE_TORQ:
{
/* Saturate torque */
f_saturate(&motor->torq.des, -motor->torq_sat,
motor->torq_sat);
/* Torque setpoint */
motor->torq.set = motor->torq.des;
break;
}
#endif
default:
{
ret = -EINVAL;
goto errout;
}
}
/* Get dq ref */
q_ref = motor->torq.set;
d_ref = 0.0f;
#ifdef CONFIG_EXAMPLES_FOC_HAVE_OPENLOOP
/* Force open-loop current */
if (motor->openloop_now == FOC_OPENLOOP_ENABLED ||
motor->openloop_now == FOC_OPENLOOP_TRANSITION)
{
/* Get open-loop currents. Positive for CW direction, negative for
* CCW direction. Id always set to 0.
*/
q_ref = motor->dir * motor->openloop_q;
d_ref = 0.0f;
}
#endif
no_controller:
/* Set DQ reference frame */
motor->dq_ref.q = q_ref;
motor->dq_ref.d = d_ref;
/* DQ compensation */
#ifdef CONFIG_EXAMPLES_FOC_FEEDFORWARD
foc_feedforward_pmsm_f32(&motor->phy, &motor->foc_state.idq,
motor->vel.now, &motor->vdq_comp);
#else
motor->vdq_comp.q = 0.0f;
motor->vdq_comp.d = 0.0f;
#endif
errout:
return ret;
}
#endif
/****************************************************************************
* Name: foc_motor_ang_get
****************************************************************************/
static int foc_motor_ang_get(FAR struct foc_motor_f32_s *motor)
{
struct foc_angle_in_f32_s ain;
struct foc_angle_out_f32_s aout;
int ret = OK;
DEBUGASSERT(motor);
/* Update open-loop angle handler */
#ifdef CONFIG_EXAMPLES_FOC_HAVE_VEL
ain.vel = motor->vel.set;
#endif
ain.state = &motor->foc_state;
ain.angle = motor->angle_now;
ain.dir = motor->dir;
#ifdef CONFIG_EXAMPLES_FOC_HAVE_OPENLOOP
if (motor->openloop_now != FOC_OPENLOOP_DISABLED)
{
foc_angle_run_f32(&motor->openloop, &ain, &aout);
/* Store open-loop angle */
motor->angle_ol = aout.angle;
}
#endif
#ifdef CONFIG_EXAMPLES_FOC_HAVE_QENCO
ret = foc_angle_run_f32(&motor->qenco, &ain, &aout);
if (ret < 0)
{
PRINTF("ERROR: foc_angle_run failed %d\n", ret);
goto errout;
}
#endif
#ifdef CONFIG_EXAMPLES_FOC_HAVE_HALL
ret = foc_angle_run_f32(&motor->hall, &ain, &aout);
if (ret < 0)
{
PRINTF("ERROR: foc_angle_run failed %d\n", ret);
goto errout;
}
#endif
#ifdef CONFIG_EXAMPLES_FOC_ANGOBS_SMO
ret = foc_angle_run_f32(&motor->ang_smo, &ain, &aout);
if (ret < 0)
{
PRINTF("ERROR: foc_angle_run failed %d\n", ret);
goto errout;
}
motor->angle_obs = aout.angle;
#endif
#ifdef CONFIG_EXAMPLES_FOC_ANGOBS_NFO
ret = foc_angle_run_f32(&motor->ang_nfo, &ain, &aout);
if (ret < 0)
{
PRINTF("ERROR: foc_angle_run failed %d\n", ret);
goto errout;
}
motor->angle_obs = aout.angle;
#endif
/* Store electrical angle from sensor or observer */
if (aout.type == FOC_ANGLE_TYPE_ELE)
{
/* Store as electrical angle */
motor->angle_el = aout.angle;
}
else if (aout.type == FOC_ANGLE_TYPE_MECH)
{
/* Store as mechanical angle */
motor->angle_m = aout.angle;
/* Convert mechanical angle to electrical angle */
motor->angle_el = fmodf(motor->angle_m * motor->phy.p,
MOTOR_ANGLE_E_MAX);
}
else
{
ASSERT(0);
}
#ifdef CONFIG_EXAMPLES_FOC_HAVE_OPENLOOP
/* Get open-loop phase angle */
if (motor->openloop_now == FOC_OPENLOOP_ENABLED)
{
motor->angle_now = motor->angle_ol;
motor->angle_el = motor->angle_ol;
}
else
#endif
#ifdef CONFIG_EXAMPLES_FOC_SENSORLESS
/* Handle smooth open-loop to closed-loop transition */
if (motor->openloop_now == FOC_OPENLOOP_TRANSITION)
{
#ifdef ANGLE_MERGE_FACTOR
if (fabsf(motor->angle_err) > fabsf(motor->angle_step))
{
motor->angle_now = motor->angle_obs + motor->angle_err;
/* Update error */
motor->angle_err -= motor->angle_step;
}
else
#endif
{
motor->angle_now = motor->angle_obs;
motor->angle_err = 0.0f;
}
}
/* Get angle from observer if closed-loop now */
else
{
motor->angle_now = motor->angle_obs;
}
#endif
#ifdef CONFIG_EXAMPLES_FOC_SENSORED
/* Get phase angle from sensor */
motor->angle_now = motor->angle_el;
#endif
#if defined(CONFIG_EXAMPLES_FOC_SENSORED) || defined(CONFIG_EXAMPLES_FOC_ANGOBS)
errout:
#endif
return ret;
}
#ifdef CONFIG_EXAMPLES_FOC_HAVE_VEL
/****************************************************************************
* Name: foc_motor_vel_get
****************************************************************************/
static int foc_motor_vel_get(FAR struct foc_motor_f32_s *motor)
{
struct foc_velocity_in_f32_s vin;
struct foc_velocity_out_f32_s vout;
int ret = OK;
DEBUGASSERT(motor);
/* Update velocity handler input data */
vin.state = &motor->foc_state;
vin.angle = motor->angle_now;
vin.vel = motor->vel.now;
vin.dir = motor->dir;
/* Get velocity */
#ifdef CONFIG_EXAMPLES_FOC_VELOBS_DIV
ret = foc_velocity_run_f32(&motor->vel_div, &vin, &vout);
if (ret < 0)
{
PRINTF("ERROR: foc_velocity_run failed %d\n", ret);
goto errout;
}
motor->vel_obs = vout.velocity;
#endif
#ifdef CONFIG_EXAMPLES_FOC_VELOBS_PLL
ret = foc_velocity_run_f32(&motor->vel_pll, &vin, &vout);
if (ret < 0)
{
PRINTF("ERROR: foc_velocity_run failed %d\n", ret);
goto errout;
}
motor->vel_obs = vout.velocity;
#endif
/* Get motor electrical velocity now */
#if defined(CONFIG_EXAMPLES_FOC_HAVE_OPENLOOP) && \
!defined(CONFIG_EXAMPLES_FOC_VELOBS)
/* No velocity feedback - assume that electical velocity is
* velocity set
*/
UNUSED(vin);
UNUSED(vout);
motor->vel_el = motor->vel.set;
#elif defined(CONFIG_EXAMPLES_FOC_VELOBS) && defined(CONFIG_EXAMPLES_FOC_SENSORLESS)
if (motor->openloop_now == FOC_OPENLOOP_DISABLED)
{
/* Get electrical velocity from observer if we are in closed-loop */
motor->vel_el = motor->vel_obs;
}
else
{
/* Otherwise use open-loop velocity */
motor->vel_el = motor->vel.set;
}
#elif defined(CONFIG_EXAMPLES_FOC_VELOBS) && defined(CONFIG_EXAMPLES_FOC_SENSORED)
/* Get electrical velocity from observer in sensored mode */
motor->vel_el = motor->vel_obs;
#else
/* Need electrical velocity source here - raise assertion */
ASSERT(0);
#endif
/* Store filtered velocity */
LP_FILTER(motor->vel.now, motor->vel_el, motor->vel_filter);
/* Get mechanical velocity (rad/s) */
motor->vel_mech = motor->vel_el * motor->phy.one_by_p;
#ifdef CONFIG_EXAMPLES_FOC_VELOBS
errout:
#endif
return ret;
}
#endif /* CONFIG_EXAMPLES_FOC_HAVE_VEL */
/****************************************************************************
* Public Functions
****************************************************************************/
/****************************************************************************
* Name: foc_motor_init
****************************************************************************/
int foc_motor_init(FAR struct foc_motor_f32_s *motor,
FAR struct foc_ctrl_env_s *envp)
{
#ifdef CONFIG_EXAMPLES_FOC_HAVE_OPENLOOP
struct foc_openloop_cfg_f32_s ol_cfg;
#endif
#ifdef CONFIG_EXAMPLES_FOC_HAVE_QENCO
struct foc_qenco_cfg_f32_s qe_cfg;
#endif
#ifdef CONFIG_EXAMPLES_FOC_HAVE_HALL
struct foc_hall_cfg_f32_s hl_cfg;
#endif
#ifdef CONFIG_EXAMPLES_FOC_ANGOBS_SMO
struct foc_angle_osmo_cfg_f32_s smo_cfg;
#endif
#ifdef CONFIG_EXAMPLES_FOC_ANGOBS_NFO
struct foc_angle_onfo_cfg_f32_s nfo_cfg;
#endif
#ifdef CONFIG_EXAMPLES_FOC_VELOBS_DIV
struct foc_vel_div_f32_cfg_s odiv_cfg;
#endif
#ifdef CONFIG_EXAMPLES_FOC_VELOBS_PLL
struct foc_vel_pll_f32_cfg_s opll_cfg;
#endif
#ifdef CONFIG_EXAMPLES_FOC_HAVE_ALIGN
struct foc_routine_align_cfg_f32_s align_cfg;
#endif
#ifdef CONFIG_EXAMPLES_FOC_HAVE_IDENT
struct foc_routine_ident_cfg_f32_s ident_cfg;
#endif
int ret = OK;
DEBUGASSERT(motor);
DEBUGASSERT(envp);
/* Reset data */
memset(motor, 0, sizeof(struct foc_motor_f32_s));
/* Connect envp with motor handler */
motor->envp = envp;
/* Initialize motor data */
motor->per = (float)(1.0f / CONFIG_EXAMPLES_FOC_NOTIFIER_FREQ);
#ifdef CONFIG_EXAMPLES_FOC_ANGOBS
motor->ol_thr = (motor->envp->cfg->ol_thr / 1.0f);
motor->ol_hys = (motor->envp->cfg->ol_hys / 1.0f);
#endif
#ifdef CONFIG_EXAMPLES_FOC_HAVE_TORQ
motor->torq_sat = (CONFIG_EXAMPLES_FOC_TORQ_MAX / 1000.0f);
#endif
#ifdef CONFIG_EXAMPLES_FOC_HAVE_VEL
motor->vel_sat = (CONFIG_EXAMPLES_FOC_VEL_MAX / 1.0f);
#endif
#ifdef CONFIG_EXAMPLES_FOC_HAVE_RUN
/* Initialize controller run mode */
ret = foc_runmode_init(motor);
if (ret < 0)
{
PRINTF("ERROR: foc_runmode_init failed %d!\n", ret);
goto errout;
}
#endif
/* Start with FOC IDLE mode */
motor->foc_mode = FOC_HANDLER_MODE_IDLE;
#ifdef CONFIG_EXAMPLES_FOC_HAVE_OPENLOOP
/* Initialize open-loop angle handler */
foc_angle_init_f32(&motor->openloop,
&g_foc_angle_ol_f32);
/* Configure open-loop angle handler */
ol_cfg.per = motor->per;
foc_angle_cfg_f32(&motor->openloop, &ol_cfg);
/* Store open-loop Q-param */
motor->openloop_q = motor->envp->cfg->qparam / 1000.0f;
#endif
#ifdef CONFIG_EXAMPLES_FOC_HAVE_QENCO
/* Initialize qenco angle handler */
ret = foc_angle_init_f32(&motor->qenco,
&g_foc_angle_qe_f32);
if (ret < 0)
{
PRINTFV("ERROR: foc_angle_init_f32 failed %d!\n", ret);
goto errout;
}
/* Get qenco devpath */
snprintf(motor->qedpath, sizeof(motor->qedpath),
"%s%d",
CONFIG_EXAMPLES_FOC_QENCO_DEVPATH,
motor->envp->id);
/* Configure qenco angle handler */
qe_cfg.posmax = CONFIG_EXAMPLES_FOC_QENCO_POSMAX;
qe_cfg.devpath = motor->qedpath;
ret = foc_angle_cfg_f32(&motor->qenco, &qe_cfg);
if (ret < 0)
{
PRINTFV("ERROR: foc_angle_cfg_f32 failed %d!\n", ret);
goto errout;
}
#endif
#ifdef CONFIG_EXAMPLES_FOC_HAVE_HALL
/* Initialize hall angle handler */
ret = foc_angle_init_f32(&motor->hall,
&g_foc_angle_hl_f32);
if (ret < 0)
{
PRINTFV("ERROR: foc_angle_init_f32 failed %d!\n", ret);
goto errout;
}
/* Get hall devpath */
snprintf(motor->hldpath, sizeof(motor->hldpath),
"%s%d",
CONFIG_EXAMPLES_FOC_HALL_DEVPATH,
motor->envp->id);
/* Configure hall angle handler */
hl_cfg.devpath = motor->hldpath;
hl_cfg.per = motor->per;
ret = foc_angle_cfg_f32(&motor->hall, &hl_cfg);
if (ret < 0)
{
PRINTFV("ERROR: foc_angle_cfg_f32 failed %d!\n", ret);
goto errout;
}
#endif
#ifdef CONFIG_EXAMPLES_FOC_ANGOBS_SMO
/* Initialzie SMO angle observer handler */
ret = foc_angle_init_f32(&motor->ang_smo,
&g_foc_angle_osmo_f32);
if (ret < 0)
{
PRINTFV("ERROR: foc_angle_init_f32 failed %d!\n", ret);
goto errout;
}
/* Configure SMO angle handler */
smo_cfg.per = motor->per;
smo_cfg.k_slide = (CONFIG_EXAMPLES_FOC_ANGOBS_SMO_KSLIDE / 1000.0f);
smo_cfg.err_max = (CONFIG_EXAMPLES_FOC_ANGOBS_SMO_ERRMAX / 1000.0f);
memcpy(&smo_cfg.phy, &motor->phy, sizeof(struct motor_phy_params_f32_s));
ret = foc_angle_cfg_f32(&motor->ang_smo, &smo_cfg);
if (ret < 0)
{
PRINTFV("ERROR: foc_angle_cfg_f32 failed %d!\n", ret);
goto errout;
}
#endif
#ifdef CONFIG_EXAMPLES_FOC_ANGOBS_NFO
/* Initialzie NFO angle observer handler */
ret = foc_angle_init_f32(&motor->ang_nfo,
&g_foc_angle_onfo_f32);
if (ret < 0)
{
PRINTFV("ERROR: foc_angle_init_f32 failed %d!\n", ret);
goto errout;
}
/* Configure NFO angle handler */
nfo_cfg.per = motor->per;
nfo_cfg.gain = (motor->envp->cfg->ang_nfo_gain / 1.0f);
nfo_cfg.gain_slow = (motor->envp->cfg->ang_nfo_slow / 1.0f);
memcpy(&nfo_cfg.phy, &motor->phy, sizeof(struct motor_phy_params_f32_s));
ret = foc_angle_cfg_f32(&motor->ang_nfo, &nfo_cfg);
if (ret < 0)
{
PRINTFV("ERROR: foc_angle_cfg_f32 failed %d!\n", ret);
goto errout;
}
#endif
#ifdef CONFIG_EXAMPLES_FOC_VELOBS_DIV
/* Initialize velocity observer */
ret = foc_velocity_init_f32(&motor->vel_div,
&g_foc_velocity_odiv_f32);
if (ret < 0)
{
PRINTFV("ERROR: foc_angle_init_f32 failed %d!\n", ret);
goto errout;
}
/* Configure velocity observer */
odiv_cfg.samples = (motor->envp->cfg->vel_div_samples);
odiv_cfg.filter = (motor->envp->cfg->vel_div_samples / 1000.0f);
odiv_cfg.per = motor->per;
ret = foc_velocity_cfg_f32(&motor->vel_div, &odiv_cfg);
if (ret < 0)
{
PRINTFV("ERROR: foc_velocity_cfg_f32 failed %d!\n", ret);
goto errout;
}
#endif
#ifdef CONFIG_EXAMPLES_FOC_VELOBS_PLL
/* Initialize velocity observer */
ret = foc_velocity_init_f32(&motor->vel_pll,
&g_foc_velocity_opll_f32);
if (ret < 0)
{
PRINTFV("ERROR: foc_angle_init_f32 failed %d!\n", ret);
goto errout;
}
/* Configure velocity observer */
opll_cfg.kp = (motor->envp->cfg->vel_pll_kp / 1.0f);
opll_cfg.ki = (motor->envp->cfg->vel_pll_ki / 1.0f);
opll_cfg.per = motor->per;
ret = foc_velocity_cfg_f32(&motor->vel_pll, &opll_cfg);
if (ret < 0)
{
PRINTFV("ERROR: foc_velocity_cfg_f32 failed %d!\n", ret);
goto errout;
}
#endif
#ifdef CONFIG_EXAMPLES_FOC_VELCTRL_PI
/* Initialize velocity controller */
pi_controller_init(&motor->vel_pi,
(motor->envp->cfg->vel_pi_kp / 1000000.0f),
(motor->envp->cfg->vel_pi_ki / 1000000.0f));
pi_saturation_set(&motor->vel_pi, -motor->torq_sat, motor->torq_sat);
pi_antiwindup_enable(&motor->vel_pi, 0.99f, true);
#endif
#ifdef CONFIG_EXAMPLES_FOC_HAVE_ALIGN
/* Initialize motor alignment routine */
ret = foc_routine_init_f32(&motor->align, &g_foc_routine_align_f32);
if (ret < 0)
{
PRINTFV("ERROR: foc_routine_init_f32 failed %d!\n", ret);
goto errout;
}
/* Initialize motor alignment data */
align_cfg.volt = (CONFIG_EXAMPLES_FOC_ALIGN_VOLT / 1000.0f);
align_cfg.offset_steps = (CONFIG_EXAMPLES_FOC_NOTIFIER_FREQ * \
CONFIG_EXAMPLES_FOC_ALIGN_SEC / 1000);
/* Connect align callbacks */
align_cfg.cb.zero = foc_align_zero_cb;
align_cfg.cb.dir = foc_align_dir_cb;
/* Connect align callbacks private data */
# ifdef CONFIG_EXAMPLES_FOC_SENSORLESS
align_cfg.cb.priv = &motor->openloop;
# endif
# ifdef CONFIG_EXAMPLES_FOC_HAVE_QENCO
align_cfg.cb.priv = &motor->qenco;
# endif
# ifdef CONFIG_EXAMPLES_FOC_HAVE_HALL
align_cfg.cb.priv = &motor->hall;
# endif
ret = foc_routine_cfg_f32(&motor->align, &align_cfg);
if (ret < 0)
{
PRINTFV("ERROR: foc_routine_cfg_f32 failed %d!\n", ret);
goto errout;
}
#endif
#ifdef CONFIG_EXAMPLES_FOC_HAVE_IDENT
/* Initialize motor identifiaction routine */
ret = foc_routine_init_f32(&motor->ident, &g_foc_routine_ident_f32);
if (ret < 0)
{
PRINTFV("ERROR: foc_routine_init_f32 failed %d!\n", ret);
goto errout;
}
/* Initialize motor identification data */
ident_cfg.per = motor->per;
ident_cfg.res_current = (motor->envp->cfg->ident_res_curr / 1000.0f);
ident_cfg.res_ki = (motor->envp->cfg->ident_res_ki / 1000.0f);
ident_cfg.ind_volt = (motor->envp->cfg->ident_ind_volt / 1000.0f);
ident_cfg.res_steps = (CONFIG_EXAMPLES_FOC_NOTIFIER_FREQ * \
motor->envp->cfg->ident_res_sec / 1000);
ident_cfg.ind_steps = (CONFIG_EXAMPLES_FOC_NOTIFIER_FREQ * \
motor->envp->cfg->ident_ind_sec / 1000);
ident_cfg.idle_steps = CONFIG_EXAMPLES_FOC_IDENT_IDLE;
ret = foc_routine_cfg_f32(&motor->ident, &ident_cfg);
if (ret < 0)
{
PRINTFV("ERROR: foc_ident_cfg_f32 failed %d!\n", ret);
goto errout;
}
#endif
#ifdef CONFIG_EXAMPLES_FOC_HAVE_VEL
/* Store velocity filter value */
motor->vel_filter = motor->envp->cfg->vel_filter / 1000.0f;
#endif
/* Initialize controller state */
#ifdef CONFIG_EXAMPLES_FOC_HAVE_ALIGN
if (motor->envp->cfg->mmode == FOC_MMODE_ALIGN_ONLY)
{
motor->ctrl_state = FOC_CTRL_STATE_ALIGN;
}
else
#endif
#ifdef CONFIG_EXAMPLES_FOC_HAVE_IDENT
if (motor->envp->cfg->mmode == FOC_MMODE_IDENT_ONLY)
{
motor->ctrl_state = FOC_CTRL_STATE_IDENT;
}
else
#endif
if (motor->envp->cfg->mmode == FOC_MMODE_IDLE)
{
motor->ctrl_state = FOC_CTRL_STATE_IDLE;
}
else
{
motor->ctrl_state = FOC_CTRL_STATE_INIT;
}
#if defined(CONFIG_EXAMPLES_FOC_SENSORED) || \
defined(CONFIG_EXAMPLES_FOC_HAVE_RUN) || \
defined(CONFIG_EXAMPLES_FOC_HAVE_IDENT)
errout:
#endif
return ret;
}
/****************************************************************************
* Name: foc_motor_deinit
****************************************************************************/
int foc_motor_deinit(FAR struct foc_motor_f32_s *motor)
{
int ret = OK;
DEBUGASSERT(motor);
#ifdef CONFIG_EXAMPLES_FOC_HAVE_OPENLOOP
/* Deinitialzie open-loop handler */
ret = foc_angle_deinit_f32(&motor->openloop);
if (ret < 0)
{
PRINTFV("ERROR: foc_angle_deinit_f32 failed %d!\n", ret);
goto errout;
}
#endif
#ifdef CONFIG_EXAMPLES_FOC_HAVE_QENCO
/* Deinitialzie qenco handler */
ret = foc_angle_deinit_f32(&motor->qenco);
if (ret < 0)
{
PRINTFV("ERROR: foc_angle_deinit_f32 failed %d!\n", ret);
goto errout;
}
#endif
#ifdef CONFIG_EXAMPLES_FOC_HAVE_HALL
/* Deinitialzie hall handler */
ret = foc_angle_deinit_f32(&motor->hall);
if (ret < 0)
{
PRINTFV("ERROR: foc_angle_deinit_f32 failed %d!\n", ret);
goto errout;
}
#endif
#ifdef CONFIG_EXAMPLES_FOC_ANGOBS_SMO
/* Deinitialize SMO observer handler */
ret = foc_angle_deinit_f32(&motor->ang_smo);
if (ret < 0)
{
PRINTFV("ERROR: foc_angle_deinit_f32 failed %d!\n", ret);
goto errout;
}
#endif
#ifdef CONFIG_EXAMPLES_FOC_ANGOBS_NFO
/* Deinitialize NFO observer handler */
ret = foc_angle_deinit_f32(&motor->ang_nfo);
if (ret < 0)
{
PRINTFV("ERROR: foc_angle_deinit_f32 failed %d!\n", ret);
goto errout;
}
#endif
#ifdef CONFIG_EXAMPLES_FOC_VELOBS_DIV
/* Deinitialize DIV observer handler */
ret = foc_velocity_deinit_f32(&motor->vel_div);
if (ret < 0)
{
PRINTFV("ERROR: foc_velocity_deinit_f32 failed %d!\n", ret);
goto errout;
}
#endif
#ifdef CONFIG_EXAMPLES_FOC_VELOBS_PLL
/* Deinitialize PLL observer handler */
ret = foc_velocity_deinit_f32(&motor->vel_pll);
if (ret < 0)
{
PRINTFV("ERROR: foc_velocity_deinit_f32 failed %d!\n", ret);
goto errout;
}
#endif
#ifdef CONFIG_EXAMPLES_FOC_HAVE_ALIGN
/* Deinitialize motor alignment routine */
ret = foc_routine_deinit_f32(&motor->align);
if (ret < 0)
{
PRINTFV("ERROR: foc_routine_deinit_f32 failed %d!\n", ret);
goto errout;
}
#endif
#ifdef CONFIG_EXAMPLES_FOC_HAVE_IDENT
/* Deinitialize motor identification routine */
ret = foc_routine_deinit_f32(&motor->ident);
if (ret < 0)
{
PRINTFV("ERROR: foc_routine_deinit_f32 failed %d!\n", ret);
goto errout;
}
#endif
#ifdef CONFIG_EXAMPLES_FOC_STATE_USE_MODEL_PMSM
/* Deinitialize PMSM model */
ret = foc_model_deinit_f32(&motor->model);
if (ret < 0)
{
PRINTF("ERROR: foc_model_deinit failed %d\n", ret);
goto errout;
}
#endif
/* Deinitialize FOC handler */
ret = foc_handler_deinit_f32(&motor->handler);
if (ret < 0)
{
PRINTF("ERROR: foc_handler_deinit failed %d\n", ret);
goto errout;
}
/* Reset data */
memset(motor, 0, sizeof(struct foc_motor_f32_s));
errout:
return ret;
}
/****************************************************************************
* Name: foc_motor_get
****************************************************************************/
int foc_motor_get(FAR struct foc_motor_f32_s *motor)
{
int ret = OK;
DEBUGASSERT(motor);
/* Get motor angle */
ret = foc_motor_ang_get(motor);
if (ret < 0)
{
goto errout;
}
#ifdef CONFIG_EXAMPLES_FOC_HAVE_VEL
/* Get motor velocity */
ret = foc_motor_vel_get(motor);
if (ret < 0)
{
goto errout;
}
#endif
errout:
return ret;
}
/****************************************************************************
* Name: foc_motor_control
****************************************************************************/
int foc_motor_control(FAR struct foc_motor_f32_s *motor)
{
int ret = OK;
DEBUGASSERT(motor);
/* Controller state machine */
switch (motor->ctrl_state)
{
case FOC_CTRL_STATE_INIT:
{
/* Next state */
motor->ctrl_state += 1;
motor->foc_mode = FOC_HANDLER_MODE_IDLE;
break;
}
#ifdef CONFIG_EXAMPLES_FOC_HAVE_ALIGN
case FOC_CTRL_STATE_ALIGN:
{
/* Run motor align procedure */
ret = foc_motor_align(motor, &motor->align_done);
if (ret < 0)
{
PRINTF("ERROR: foc_motor_align failed %d!\n", ret);
goto errout;
}
if (motor->align_done == true)
{
/* Next state */
motor->ctrl_state += 1;
motor->foc_mode = FOC_HANDLER_MODE_IDLE;
if (motor->envp->cfg->mmode == FOC_MMODE_ALIGN_ONLY)
{
motor->ctrl_state = FOC_CTRL_STATE_TERMINATE;
}
}
break;
}
#endif
#ifdef CONFIG_EXAMPLES_FOC_HAVE_IDENT
case FOC_CTRL_STATE_IDENT:
{
/* Run motor identification procedure */
ret = foc_motor_ident(motor, &motor->ident_done);
if (ret < 0)
{
PRINTF("ERROR: foc_motor_ident failed %d!\n", ret);
goto errout;
}
if (motor->ident_done == true)
{
/* Next state */
motor->ctrl_state += 1;
motor->foc_mode = FOC_HANDLER_MODE_IDLE;
if (motor->envp->cfg->mmode == FOC_MMODE_IDENT_ONLY)
{
motor->ctrl_state = FOC_CTRL_STATE_TERMINATE;
}
}
break;
}
#endif
#ifdef CONFIG_EXAMPLES_FOC_HAVE_RUN
case FOC_CTRL_STATE_RUN_INIT:
{
/* Initialize controller run mode */
ret = foc_motor_run_init(motor);
if (ret < 0)
{
PRINTF("ERROR: foc_motor_run_init failed %d!\n", ret);
goto errout;
}
/* Next state */
motor->ctrl_state += 1;
motor->foc_mode = FOC_HANDLER_MODE_IDLE;
break;
}
case FOC_CTRL_STATE_RUN:
{
/* Get FOC run mode */
motor->foc_mode = motor->foc_mode_run;
/* Run motor */
ret = foc_motor_run(motor);
if (ret < 0)
{
PRINTF("ERROR: foc_motor_run failed %d!\n", ret);
goto errout;
}
break;
}
#endif
case FOC_CTRL_STATE_IDLE:
{
motor->foc_mode = FOC_HANDLER_MODE_IDLE;
#ifndef CONFIG_EXAMPLES_FOC_HAVE_RUN
/* Terminate */
motor->ctrl_state += 1;
#endif
break;
}
case FOC_CTRL_STATE_TERMINATE:
{
/* Do nothing */
break;
}
default:
{
PRINTF("ERROR: invalid ctrl_state=%d\n", motor->ctrl_state);
ret = -EINVAL;
goto errout;
}
}
errout:
return ret;
}
/****************************************************************************
* Name: foc_motor_handle
****************************************************************************/
int foc_motor_handle(FAR struct foc_motor_f32_s *motor,
FAR struct foc_mq_s *handle)
{
int ret = OK;
DEBUGASSERT(motor);
DEBUGASSERT(handle);
/* Terminate */
if (handle->quit == true)
{
motor->mq.quit = true;
}
/* Update motor VBUS */
if (motor->mq.vbus != handle->vbus)
{
PRINTFV("Set vbus=%" PRIu32 " for FOC driver %d!\n",
handle->vbus, motor->envp->id);
ret = foc_motor_vbus(motor, handle->vbus);
if (ret < 0)
{
PRINTF("ERROR: foc_motor_vbus failed %d!\n", ret);
goto errout;
}
motor->mq.vbus = handle->vbus;
}
/* Update motor velocity destination
* NOTE: only velocity control supported for now
*/
if (motor->mq.setpoint != handle->setpoint)
{
PRINTFV("Set setpoint=%" PRIu32 " for FOC driver %d!\n",
handle->setpoint, motor->envp->id);
/* Update motor setpoint */
ret = foc_motor_setpoint(motor, handle->setpoint);
if (ret < 0)
{
PRINTF("ERROR: foc_motor_setpoint failed %d!\n", ret);
goto errout;
}
motor->mq.setpoint = handle->setpoint;
}
/* Update motor state */
if (motor->mq.app_state != handle->app_state)
{
PRINTFV("Set app_state=%d for FOC driver %d!\n",
handle->app_state, motor->envp->id);
ret = foc_motor_state(motor, handle->app_state);
if (ret < 0)
{
PRINTF("ERROR: foc_motor_state failed %d!\n", ret);
goto errout;
}
motor->mq.app_state = handle->app_state;
}
/* Start/stop controller */
if (motor->mq.start != handle->start)
{
PRINTFV("Set start=%d for FOC driver %d!\n",
handle->start, motor->envp->id);
/* Start/stop motor controller */
ret = foc_motor_start(motor, handle->start);
if (ret < 0)
{
PRINTF("ERROR: foc_motor_start failed %d!\n", ret);
goto errout;
}
motor->mq.start = handle->start;
}
errout:
return ret;
}
Reference in New Issue View Git Blame Copy Permalink