sergiotarxz/nuttx-apps
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity
e55f28594c
nuttx-apps/examples/foc/foc_fixed16_thr.c
raiden00pl 8398297518 examples/foc: add an option to call nxscope work from control thread 
This alows nxscope data to be sent with every cycle of the control loop,
which increases the execution time of the control loop, but allows data
to be sent at a highier frequency using the small nxscope buffer.

Useful when tuning and debuging using RTT transfer.
2023-10-19 01:24:07 +08:00

525 lines
14 KiB
C
Raw Blame History

/****************************************************************************
* apps/examples/foc/foc_fixed16_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 <dspb16.h>
#include <fcntl.h>
#include <stdio.h>
#include <string.h>
#include <unistd.h>
#include "foc_cfg.h"
#include "foc_debug.h"
#include "foc_motor_b16.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_FIXED16
# error
#endif
/****************************************************************************
* Private Type Definition
****************************************************************************/
/****************************************************************************
* Private Functions
****************************************************************************/
/****************************************************************************
* Name: foc_handler_run
****************************************************************************/
static int foc_handler_run(FAR struct foc_motor_b16_s *motor,
FAR struct foc_device_s *dev)
{
struct foc_handler_input_b16_s input;
struct foc_handler_output_b16_s output;
b16_t 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;
motor->dq_ref.d = 0;
motor->angle_now = 0;
motor->vbus = 0;
/* Force setpoints to zero */
#ifdef CONFIG_EXAMPLES_FOC_HAVE_TORQ
motor->torq.des = 0;
#endif
#ifdef CONFIG_EXAMPLES_FOC_HAVE_VEL
motor->vel.des = 0;
#endif
#ifdef CONFIG_EXAMPLES_FOC_HAVE_POS
motor->pos.des = 0;
#endif
}
/* Get real currents */
for (i = 0; i < CONFIG_MOTOR_FOC_PHASES; i += 1)
{
current[i] = b16muli(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_b16(&motor->handler, &input, &output);
/* Get duty from controller */
for (i = 0; i < CONFIG_MOTOR_FOC_PHASES; i += 1)
{
dev->params.duty[i] = FOCDUTY_FROM_FIXED16(output.duty[i]);
}
/* Get FOC handler state */
foc_handler_state_b16(&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_b16_s *motor,
FAR struct foc_device_s *dev)
{
int i = 0;
DEBUGASSERT(motor);
DEBUGASSERT(dev);
/* Get model state */
foc_model_state_b16(&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_b16_s *motor)
{
DEBUGASSERT(motor);
PRINTF("b16 inst %d:\n", motor->envp->inst);
foc_handler_state_print_b16(&motor->foc_state);
return OK;
}
#endif
#ifdef CONFIG_EXAMPLES_FOC_NXSCOPE
/****************************************************************************
* Name: foc_fixed16_nxscope
****************************************************************************/
static void foc_fixed16_nxscope(FAR struct foc_nxscope_s *nxs,
FAR struct foc_motor_b16_s *motor,
FAR struct foc_device_s *dev)
{
#if (CONFIG_EXAMPLES_FOC_NXSCOPE_CFG != 0)
FAR b16_t *ptr = NULL;
int i = nxs->ch_per_inst * motor->envp->id;
#endif
nxscope_lock(&nxs->nxs);
#if (CONFIG_EXAMPLES_FOC_NXSCOPE_CFG & FOC_NXSCOPE_IABC)
ptr = (FAR b16_t *)&motor->foc_state.curr;
nxscope_put_vb16(&nxs->nxs, i++, ptr, CONFIG_MOTOR_FOC_PHASES);
#endif
#if (CONFIG_EXAMPLES_FOC_NXSCOPE_CFG & FOC_NXSCOPE_IDQ)
ptr = (FAR b16_t *)&motor->foc_state.idq;
nxscope_put_vb16(&nxs->nxs, i++, ptr, 2);
#endif
#if (CONFIG_EXAMPLES_FOC_NXSCOPE_CFG & FOC_NXSCOPE_IAB)
ptr = (FAR b16_t *)&motor->foc_state.iab;
nxscope_put_vb16(&nxs->nxs, i++, ptr, 2);
#endif
#if (CONFIG_EXAMPLES_FOC_NXSCOPE_CFG & FOC_NXSCOPE_VABC)
ptr = (FAR b16_t *)&motor->foc_state.volt;
nxscope_put_vb16(&nxs->nxs, i++, ptr, CONFIG_MOTOR_FOC_PHASES);
#endif
#if (CONFIG_EXAMPLES_FOC_NXSCOPE_CFG & FOC_NXSCOPE_VDQ)
ptr = (FAR b16_t *)&motor->foc_state.vdq;
nxscope_put_vb16(&nxs->nxs, i++, ptr, 2);
#endif
#if (CONFIG_EXAMPLES_FOC_NXSCOPE_CFG & FOC_NXSCOPE_VAB)
ptr = (FAR b16_t *)&motor->foc_state.vab;
nxscope_put_vb16(&nxs->nxs, i++, ptr, 2);
#endif
#if (CONFIG_EXAMPLES_FOC_NXSCOPE_CFG & FOC_NXSCOPE_AEL)
ptr = (FAR b16_t *)&motor->angle_el;
nxscope_put_vb16(&nxs->nxs, i++, ptr, 1);
#endif
#if (CONFIG_EXAMPLES_FOC_NXSCOPE_CFG & FOC_NXSCOPE_AM)
ptr = (FAR b16_t *)&motor->angle_m;
nxscope_put_vb16(&nxs->nxs, i++, ptr, 1);
#endif
#if (CONFIG_EXAMPLES_FOC_NXSCOPE_CFG & FOC_NXSCOPE_VEL)
ptr = (FAR b16_t *)&motor->vel_el;
nxscope_put_vb16(&nxs->nxs, i++, ptr, 1);
#endif
#if (CONFIG_EXAMPLES_FOC_NXSCOPE_CFG & FOC_NXSCOPE_VM)
ptr = (FAR b16_t *)&motor->vel_mech;
nxscope_put_vb16(&nxs->nxs, i++, ptr, 1);
#endif
#if (CONFIG_EXAMPLES_FOC_NXSCOPE_CFG & FOC_NXSCOPE_VBUS)
ptr = (FAR b16_t *)&motor->vbus;
nxscope_put_vb16(&nxs->nxs, i++, ptr, 1);
#endif
#if (CONFIG_EXAMPLES_FOC_NXSCOPE_CFG & FOC_NXSCOPE_SPTORQ)
ptr = (FAR b16_t *)&motor->torq;
nxscope_put_vb16(&nxs->nxs, i++, ptr, 3);
#endif
#if (CONFIG_EXAMPLES_FOC_NXSCOPE_CFG & FOC_NXSCOPE_SPVEL)
ptr = (FAR b16_t *)&motor->vel;
nxscope_put_vb16(&nxs->nxs, i++, ptr, 3);
#endif
#if (CONFIG_EXAMPLES_FOC_NXSCOPE_CFG & FOC_NXSCOPE_SPPOS)
ptr = (FAR b16_t *)&motor->pos;
nxscope_put_vb16(&nxs->nxs, i++, ptr, 3);
#endif
#if (CONFIG_EXAMPLES_FOC_NXSCOPE_CFG & FOC_NXSCOPE_DQREF)
ptr = (FAR b16_t *)&motor->dq_ref;
nxscope_put_vb16(&nxs->nxs, i++, ptr, 2);
#endif
#if (CONFIG_EXAMPLES_FOC_NXSCOPE_CFG & FOC_NXSCOPE_VDQCOMP)
ptr = (FAR b16_t *)&motor->vdq_comp;
nxscope_put_vb16(&nxs->nxs, i++, ptr, 2);
#endif
#if (CONFIG_EXAMPLES_FOC_NXSCOPE_CFG & FOC_NXSCOPE_SVM3)
b16_t svm3_tmp[4];
/* Convert sector to b16_t.
* Normally, a sector value is an integer in the range 1-6 but we convert
* it to b16_t and range to 0.1-0.6. This is to send the entire SVM3 state
* as b16_t 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] = b16mulb16(itob16(motor->mod_state.sector), ftob16(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_vb16(&nxs->nxs, i++, ptr, 4);
#endif
nxscope_unlock(&nxs->nxs);
}
#endif
/****************************************************************************
* Public Functions
****************************************************************************/
/****************************************************************************
* Name: foc_fixed16_thr
****************************************************************************/
int foc_fixed16_thr(FAR struct foc_ctrl_env_s *envp)
{
struct foc_mq_s handle;
struct foc_motor_b16_s motor;
struct foc_device_s dev;
int ret = OK;
DEBUGASSERT(envp);
PRINTFV("foc_fixed_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_FIXED16(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)
{
PRINTFV("foc_fixed16_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;
}
/* Run control logic if controller started */
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;
}
#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_b16(&motor.model,
ftob16(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_params_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_fixed16_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;
}
}
else
{
usleep(1000);
}
/* Increase counter */
motor.time += 1;
}
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_fixed16_thr %d exit\n", envp->id);
return ret;
}
Reference in New Issue View Git Blame Copy Permalink