sergiotarxz/nuttx-apps
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity
9eb5bd4091
nuttx-apps/examples/foc/foc_float_thr.c
raiden00pl 1095aec162 examples/foc: start from the beginning if control loop is started 
After the start request to FOC device, we should return to the
beginning of the loop and wait for the first synchronization event.

This also fixes the problem with the extended first loop cycle,
so we can remove the workaround in foc_perf.
2023-11-13 08:48:22 -08:00

551 lines
15 KiB
C
Raw Blame History

/****************************************************************************
* apps/examples/foc/foc_float_thr.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 <dsp.h>
#include <fcntl.h>
#include <stdio.h>
#include <string.h>
#include <unistd.h>
#include "foc_cfg.h"
#include "foc_debug.h"
#include "foc_motor_f32.h"
#include "industry/foc/foc_utils.h"
#include "industry/foc/foc_common.h"
#ifdef CONFIG_EXAMPLES_FOC_NXSCOPE
# include "logging/nxscope/nxscope.h"
#endif
/****************************************************************************
* Pre-processor Definitions
****************************************************************************/
#ifndef CONFIG_INDUSTRY_FOC_FLOAT
# error
#endif
/****************************************************************************
* Private Type Definition
****************************************************************************/
/****************************************************************************
* Private Functions
****************************************************************************/
/****************************************************************************
* Name: foc_handler_run
****************************************************************************/
static int foc_handler_run(FAR struct foc_motor_f32_s *motor,
FAR struct foc_device_s *dev)
{
struct foc_handler_input_f32_s input;
struct foc_handler_output_f32_s output;
float current[CONFIG_MOTOR_FOC_PHASES];
int ret = OK;
int i = 0;
DEBUGASSERT(motor);
DEBUGASSERT(dev);
/* FOC device fault */
if (motor->fault == true)
{
/* Stop motor */
motor->dq_ref.q = 0.0f;
motor->dq_ref.d = 0.0f;
motor->angle_now = 0.0f;
motor->vbus = 0.0f;
/* Force velocity to zero */
#ifdef CONFIG_EXAMPLES_FOC_HAVE_TORQ
motor->torq.des = 0.0f;
#endif
#ifdef CONFIG_EXAMPLES_FOC_HAVE_VEL
motor->vel.des = 0.0f;
#endif
#ifdef CONFIG_EXAMPLES_FOC_HAVE_POS
motor->pos.des = 0.0f;
#endif
}
/* Get real currents */
for (i = 0; i < CONFIG_MOTOR_FOC_PHASES; i += 1)
{
current[i] = (motor->iphase_adc * dev->state.curr[i]);
}
/* Get input for FOC handler */
input.current = current;
input.dq_ref = &motor->dq_ref;
input.vdq_comp = &motor->vdq_comp;
input.angle = motor->angle_now;
input.vbus = motor->vbus;
input.mode = motor->foc_mode;
/* Run FOC controller */
ret = foc_handler_run_f32(&motor->handler, &input, &output);
/* Get duty from controller */
for (i = 0; i < CONFIG_MOTOR_FOC_PHASES; i += 1)
{
dev->params.duty[i] = FOCDUTY_FROM_FLOAT(output.duty[i]);
}
/* Get FOC handler state */
foc_handler_state_f32(&motor->handler,
&motor->foc_state,
&motor->mod_state);
return ret;
}
#ifdef CONFIG_EXAMPLES_FOC_STATE_USE_MODEL_PMSM
/****************************************************************************
* Name: foc_model_state_get
****************************************************************************/
static int foc_model_state_get(FAR struct foc_motor_f32_s *motor,
FAR struct foc_device_s *dev)
{
int i = 0;
DEBUGASSERT(motor);
DEBUGASSERT(dev);
/* Get model state */
foc_model_state_f32(&motor->model, &motor->model_state);
/* Get model currents */
for (i = 0; i < CONFIG_MOTOR_FOC_PHASES; i += 1)
{
dev->state.curr[i] = motor->model_state.curr_raw[i];
}
return OK;
}
#endif
#ifdef FOC_STATE_PRINT_PRE
/****************************************************************************
* Name: foc_state_print
****************************************************************************/
static int foc_state_print(FAR struct foc_motor_f32_s *motor)
{
DEBUGASSERT(motor);
PRINTF("f32 inst %d:\n", motor->envp->inst);
foc_handler_state_print_f32(&motor->foc_state);
return OK;
}
#endif
#ifdef CONFIG_EXAMPLES_FOC_NXSCOPE
/****************************************************************************
* Name: foc_float_nxscope
****************************************************************************/
static void foc_float_nxscope(FAR struct foc_nxscope_s *nxs,
FAR struct foc_motor_f32_s *motor,
FAR struct foc_device_s *dev)
{
#if (CONFIG_EXAMPLES_FOC_NXSCOPE_CFG != 0)
FAR float *ptr = NULL;
int i = nxs->ch_per_inst * motor->envp->id;
#endif
#ifndef CONFIG_EXAMPLES_FOC_NXSCOPE_CONTROL
nxscope_lock(&nxs->nxs);
#endif
#if (CONFIG_EXAMPLES_FOC_NXSCOPE_CFG & FOC_NXSCOPE_IABC)
ptr = (FAR float *)&motor->foc_state.curr;
nxscope_put_vfloat(&nxs->nxs, i++, ptr, CONFIG_MOTOR_FOC_PHASES);
#endif
#if (CONFIG_EXAMPLES_FOC_NXSCOPE_CFG & FOC_NXSCOPE_IDQ)
ptr = (FAR float *)&motor->foc_state.idq;
nxscope_put_vfloat(&nxs->nxs, i++, ptr, 2);
#endif
#if (CONFIG_EXAMPLES_FOC_NXSCOPE_CFG & FOC_NXSCOPE_IAB)
ptr = (FAR float *)&motor->foc_state.iab;
nxscope_put_vfloat(&nxs->nxs, i++, ptr, 2);
#endif
#if (CONFIG_EXAMPLES_FOC_NXSCOPE_CFG & FOC_NXSCOPE_VABC)
ptr = (FAR float *)&motor->foc_state.volt;
nxscope_put_vfloat(&nxs->nxs, i++, ptr, CONFIG_MOTOR_FOC_PHASES);
#endif
#if (CONFIG_EXAMPLES_FOC_NXSCOPE_CFG & FOC_NXSCOPE_VDQ)
ptr = (FAR float *)&motor->foc_state.vdq;
nxscope_put_vfloat(&nxs->nxs, i++, ptr, 2);
#endif
#if (CONFIG_EXAMPLES_FOC_NXSCOPE_CFG & FOC_NXSCOPE_VAB)
ptr = (FAR float *)&motor->foc_state.vab;
nxscope_put_vfloat(&nxs->nxs, i++, ptr, 2);
#endif
#if (CONFIG_EXAMPLES_FOC_NXSCOPE_CFG & FOC_NXSCOPE_AEL)
ptr = (FAR float *)&motor->angle_el;
nxscope_put_vfloat(&nxs->nxs, i++, ptr, 1);
#endif
#if (CONFIG_EXAMPLES_FOC_NXSCOPE_CFG & FOC_NXSCOPE_AM)
ptr = (FAR float *)&motor->angle_m;
nxscope_put_vfloat(&nxs->nxs, i++, ptr, 1);
#endif
#if (CONFIG_EXAMPLES_FOC_NXSCOPE_CFG & FOC_NXSCOPE_VEL)
ptr = (FAR float *)&motor->vel_el;
nxscope_put_vfloat(&nxs->nxs, i++, ptr, 1);
#endif
#if (CONFIG_EXAMPLES_FOC_NXSCOPE_CFG & FOC_NXSCOPE_VM)
ptr = (FAR float *)&motor->vel_mech;
nxscope_put_vfloat(&nxs->nxs, i++, ptr, 1);
#endif
#if (CONFIG_EXAMPLES_FOC_NXSCOPE_CFG & FOC_NXSCOPE_VBUS)
ptr = (FAR float *)&motor->vbus;
nxscope_put_vfloat(&nxs->nxs, i++, ptr, 1);
#endif
#if (CONFIG_EXAMPLES_FOC_NXSCOPE_CFG & FOC_NXSCOPE_SPTORQ)
ptr = (FAR float *)&motor->torq;
nxscope_put_vfloat(&nxs->nxs, i++, ptr, 3);
#endif
#if (CONFIG_EXAMPLES_FOC_NXSCOPE_CFG & FOC_NXSCOPE_SPVEL)
ptr = (FAR float *)&motor->vel;
nxscope_put_vfloat(&nxs->nxs, i++, ptr, 3);
#endif
#if (CONFIG_EXAMPLES_FOC_NXSCOPE_CFG & FOC_NXSCOPE_SPPOS)
ptr = (FAR float *)&motor->pos;
nxscope_put_vfloat(&nxs->nxs, i++, ptr, 3);
#endif
#if (CONFIG_EXAMPLES_FOC_NXSCOPE_CFG & FOC_NXSCOPE_DQREF)
ptr = (FAR float *)&motor->dq_ref;
nxscope_put_vfloat(&nxs->nxs, i++, ptr, 2);
#endif
#if (CONFIG_EXAMPLES_FOC_NXSCOPE_CFG & FOC_NXSCOPE_VDQCOMP)
ptr = (FAR float *)&motor->vdq_comp;
nxscope_put_vfloat(&nxs->nxs, i++, ptr, 2);
#endif
#if (CONFIG_EXAMPLES_FOC_NXSCOPE_CFG & FOC_NXSCOPE_SVM3)
float svm3_tmp[4];
/* Convert sector to float.
* Normally, a sector value is an integer in the range 1-6 but we convert
* it to float and range to 0.1-0.6. This is to send the entire SVM3 state
* as float array and scale the sector value closer to PWM duty values
* (range 0.0 to 0.5) which makes it easier to visualize the data later.
*/
svm3_tmp[0] = (float)motor->mod_state.sector * 0.1f;
svm3_tmp[1] = motor->mod_state.d_u;
svm3_tmp[2] = motor->mod_state.d_v;
svm3_tmp[3] = motor->mod_state.d_w;
ptr = svm3_tmp;
nxscope_put_vfloat(&nxs->nxs, i++, ptr, 4);
#endif
#if (CONFIG_EXAMPLES_FOC_NXSCOPE_CFG & FOC_NXSCOPE_VOBS)
ptr = (FAR float *)&motor->vel_obs;
nxscope_put_vfloat(&nxs->nxs, i++, ptr, 1);
#endif
#if (CONFIG_EXAMPLES_FOC_NXSCOPE_CFG & FOC_NXSCOPE_AOBS)
ptr = (FAR float *)&motor->angle_obs;
nxscope_put_vfloat(&nxs->nxs, i++, ptr, 1);
#endif
#ifndef CONFIG_EXAMPLES_FOC_NXSCOPE_CONTROL
nxscope_unlock(&nxs->nxs);
#endif
}
#endif
/****************************************************************************
* Public Functions
****************************************************************************/
/****************************************************************************
* Name: foc_float_thr
****************************************************************************/
int foc_float_thr(FAR struct foc_ctrl_env_s *envp)
{
struct foc_mq_s handle;
struct foc_motor_f32_s motor;
struct foc_device_s dev;
int ret = OK;
DEBUGASSERT(envp);
PRINTFV("foc_float_thr, id=%d\n", envp->id);
/* Reset data */
memset(&handle, 0, sizeof(struct foc_mq_s));
/* Initialize motor controller */
ret = foc_motor_init(&motor, envp);
if (ret < 0)
{
PRINTF("ERROR: foc_motor_init failed %d!\n", ret);
goto errout;
}
/* Initialize FOC device as blocking */
ret = foc_device_init(&dev, envp->id);
if (ret < 0)
{
PRINTF("ERROR: foc_device_init failed %d!\n", ret);
goto errout;
}
/* Get PWM max duty */
motor.pwm_duty_max = FOCDUTY_TO_FLOAT(dev.info.hw_cfg.pwm_max);
/* Start with motor free */
handle.app_state = FOC_EXAMPLE_STATE_FREE;
/* Wait some time */
usleep(1000);
/* Control loop */
while (motor.mq.quit == false)
{
if (motor.mq.start == true)
{
/* Get FOC device state */
ret = foc_dev_state_get(&dev);
if (ret < 0)
{
PRINTF("ERROR: foc_dev_state_get failed %d!\n", ret);
goto errout;
}
}
PRINTFV("foc_float_thr %d %d\n", envp->id, motor.time);
/* Handle mqueue */
ret = foc_mq_handle(envp->mqd, &handle);
if (ret < 0)
{
PRINTF("ERROR: foc_mq_handle failed %d!\n", ret);
goto errout;
}
/* Handle motor data */
ret = foc_motor_handle(&motor, &handle);
if (ret < 0)
{
PRINTF("ERROR: foc_motor_handle failed %d!\n", ret);
goto errout;
}
if (motor.startstop == true)
{
/* Start or stop device */
PRINTF("Start FOC device %d state=%d!\n",
motor.envp->id, motor.mq.start);
ret = foc_device_start(&dev, motor.mq.start);
if (ret < 0)
{
PRINTFV("ERROR: foc_device_start failed %d!\n", ret);
goto errout;
}
motor.startstop = false;
/* Start from the beginning of the control loop */
continue;
}
/* Ignore control logic if controller not started yet */
if (motor.mq.start == false)
{
usleep(1000);
continue;
}
#ifdef CONFIG_EXAMPLES_FOC_STATE_USE_MODEL_PMSM
/* Get model state */
ret = foc_model_state_get(&motor, &dev);
if (ret < 0)
{
PRINTF("ERROR: foc_model_state_get failed %d!\n", ret);
goto errout;
}
#endif
/* Handle controller state */
ret = foc_dev_state_handle(&dev, &motor.fault);
if (ret < 0)
{
PRINTF("ERROR: foc_dev_state_handle failed %d!\n", ret);
goto errout;
}
/* Get motor state */
ret = foc_motor_get(&motor);
if (ret < 0)
{
PRINTF("ERROR: foc_motor_get failed %d!\n", ret);
goto errout;
}
/* Motor control */
ret = foc_motor_control(&motor);
if (ret < 0)
{
PRINTF("ERROR: foc_motor_control failed %d!\n", ret);
goto errout;
}
/* Run FOC */
ret = foc_handler_run(&motor, &dev);
if (ret < 0)
{
PRINTF("ERROR: foc_handler_run failed %d!\n", ret);
goto errout;
}
#ifdef FOC_STATE_PRINT_PRE
/* Print state if configured */
if (motor.time % FOC_STATE_PRINT_PRE == 0)
{
foc_state_print(&motor);
}
#endif
#ifdef CONFIG_EXAMPLES_FOC_STATE_USE_MODEL_PMSM
/* Feed FOC model with data */
foc_model_run_f32(&motor.model,
FOC_MODEL_LOAD,
&motor.foc_state.vab);
#endif
/* Set FOC device parameters */
ret = foc_dev_params_set(&dev);
if (ret < 0)
{
PRINTF("ERROR: foc_dev_prams_set failed %d!\n", ret);
goto errout;
}
#ifdef CONFIG_EXAMPLES_FOC_NXSCOPE
/* Capture nxscope samples */
if (motor.time % CONFIG_EXAMPLES_FOC_NXSCOPE_PRESCALER == 0)
{
foc_float_nxscope(envp->nxs, &motor, &dev);
}
#endif
#ifdef CONFIG_EXAMPLES_FOC_NXSCOPE_CONTROL
/* Handle nxscope work */
if (motor.time % CONFIG_EXAMPLES_FOC_NXSCOPE_WORK_PRESCALER == 0)
{
foc_nxscope_work(envp->nxs);
}
#endif
/* Terminate control thread */
if (motor.ctrl_state == FOC_CTRL_STATE_TERMINATE)
{
PRINTF("TERMINATE CTRL THREAD\n");
goto errout;
}
/* Increase counter */
motor.time += 1;
#ifdef CONFIG_EXAMPLES_FOC_PERF
if (dev.perf.max_changed)
{
PRINTF_PERF("max=%" PRId32 "\n", dev.perf.max);
}
#endif
}
errout:
/* Deinit motor controller */
ret = foc_motor_deinit(&motor);
if (ret != OK)
{
PRINTF("ERROR: foc_motor_deinit failed %d!\n", ret);
}
PRINTF("Stop FOC device %d!\n", envp->id);
/* De-initialize FOC device */
ret = foc_device_deinit(&dev);
if (ret < 0)
{
PRINTF("ERROR: foc_device_deinit %d failed %d\n", envp->id, ret);
}
PRINTF("foc_float_thr %d exit\n", envp->id);
return ret;
}
Reference in New Issue View Git Blame Copy Permalink