sergiotarxz/nuttx-apps
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity
144bf0bd1f
nuttx-apps/examples/foc/foc_nxscope.c
raiden00pl 1c922f2d2d examples/foc: add phase angle observer support (sensorless mode) 
This app can work now as sensorless ESC.

Also introduce a cmd line option that force open-loop control
which is useful when tuning angle/velocity observers
2023-10-19 19:33:43 +08:00

323 lines
9.5 KiB
C
Raw Blame History

/****************************************************************************
* apps/examples/foc/foc_nxscope.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 <errno.h>
#include <unistd.h>
#include <string.h>
#include "foc_debug.h"
#include "foc_nxscope.h"
#include "foc_thr.h"
#include "industry/foc/foc_common.h"
/****************************************************************************
* Pre-processor Definitions
****************************************************************************/
#ifndef CONFIG_LOGGING_NXSCOPE_DISABLE_PUTLOCK
# error CONFIG_LOGGING_NXSCOPE_DISABLE_PUTLOCK must be set to proper operation.
#endif
#if defined(CONFIG_LOGGING_NXSCOPE_INTF_SERIAL) && !defined(CONFIG_SERIAL_RTT)
# ifndef CONFIG_SERIAL_TERMIOS
# error CONFIG_SERIAL_TERMIOS must be set to proper operation.
# endif
#endif
/****************************************************************************
* Private Functions
****************************************************************************/
#ifdef CONFIG_EXAMPLES_FOC_NXSCOPE_THREAD
/****************************************************************************
* Name: foc_nxscope_thr
****************************************************************************/
static FAR void *foc_nxscope_thr(FAR void *arg)
{
FAR struct foc_nxscope_s *nxs = (FAR struct foc_nxscope_s *)arg;
DEBUGASSERT(nxs);
while (1)
{
/* NxScope work */
foc_nxscope_work(nxs);
usleep(10000);
}
return NULL;
}
#endif
/****************************************************************************
* Public Functions
****************************************************************************/
/****************************************************************************
* Name: foc_nxscope_init
****************************************************************************/
int foc_nxscope_init(FAR struct foc_nxscope_s *nxs)
{
union nxscope_chinfo_type_u u;
struct nxscope_cfg_s nxs_cfg;
#ifdef CONFIG_EXAMPLES_FOC_NXSCOPE_THREAD
struct sched_param param;
pthread_attr_t attr;
pthread_t nxsthr = 0;
#endif
int ret = OK;
int i = 0;
int j = 0;
DEBUGASSERT(nxs);
/* Initialize serial interface */
nxs->ser_cfg.path = CONFIG_EXAMPLES_FOC_NXSCOPE_SERIAL_PATH;
nxs->ser_cfg.nonblock = true;
nxs->ser_cfg.baud = CONFIG_EXAMPLES_FOC_NXSCOPE_SERIAL_BAUD;
ret = nxscope_ser_init(&nxs->intf, &nxs->ser_cfg);
if (ret < 0)
{
PRINTF("ERROR: nxscope_ser_init failed %d\n", ret);
goto errout;
}
/* Default serial protocol */
ret = nxscope_proto_ser_init(&nxs->proto, NULL);
if (ret < 0)
{
PRINTF("ERROR: nxscope_proto_ser_init failed %d\n", ret);
goto errout;
}
/* Initialize nxscope */
nxs_cfg.intf_cmd = &nxs->intf;
nxs_cfg.proto_cmd = &nxs->proto;
nxs_cfg.intf_stream = &nxs->intf;
nxs_cfg.proto_stream = &nxs->proto;
nxs_cfg.channels = CONFIG_EXAMPLES_FOC_NXSCOPE_CHANNELS;
nxs_cfg.streambuf_len = CONFIG_EXAMPLES_FOC_NXSCOPE_STREAMBUF_LEN;
nxs_cfg.rxbuf_len = CONFIG_EXAMPLES_FOC_NXSCOPE_RXBUF_LEN;
nxs_cfg.rx_padding = CONFIG_EXAMPLES_FOC_NXSCOPE_RXPADDING;
nxs_cfg.callbacks = &nxs->cb;
ret = nxscope_init(&nxs->nxs, &nxs_cfg);
if (ret < 0)
{
PRINTF("ERROR: nxscope_init failed %d\n", ret);
goto errout;
}
/* Get channels per instance */
for (j = 0; j < CONFIG_MOTOR_FOC_INST; j += 1)
{
if (CONFIG_EXAMPLES_FOC_NXSCOPE_CFG & (1 << j))
{
nxs->ch_per_inst += 1;
}
}
/* For all FOC controllers */
for (j = 0; j < CONFIG_MOTOR_FOC_INST; j += 1)
{
/* Get controller data type */
switch (foc_thread_type(j))
{
#ifdef CONFIG_INDUSTRY_FOC_FLOAT
case FOC_NUMBER_TYPE_FLOAT:
{
u.s.dtype = NXSCOPE_TYPE_FLOAT;
break;
}
#endif
#ifdef CONFIG_INDUSTRY_FOC_FIXED16
case FOC_NUMBER_TYPE_FIXED16:
{
u.s.dtype = NXSCOPE_TYPE_B16;
break;
}
#endif
default:
{
ASSERT(0);
}
}
/* Create channels */
#if (CONFIG_EXAMPLES_FOC_NXSCOPE_CFG == 0)
UNUSED(u);
#endif
#if (CONFIG_EXAMPLES_FOC_NXSCOPE_CFG & FOC_NXSCOPE_IABC)
nxscope_chan_init(&nxs->nxs, i++, "iabc", u.u8,
CONFIG_MOTOR_FOC_PHASES, 0);
#endif
#if (CONFIG_EXAMPLES_FOC_NXSCOPE_CFG & FOC_NXSCOPE_IDQ)
nxscope_chan_init(&nxs->nxs, i++, "idq", u.u8, 2, 0);
#endif
#if (CONFIG_EXAMPLES_FOC_NXSCOPE_CFG & FOC_NXSCOPE_IAB)
nxscope_chan_init(&nxs->nxs, i++, "iab", u.u8, 2, 0);
#endif
#if (CONFIG_EXAMPLES_FOC_NXSCOPE_CFG & FOC_NXSCOPE_VABC)
nxscope_chan_init(&nxs->nxs, i++, "vabc", u.u8,
CONFIG_MOTOR_FOC_PHASES, 0);
#endif
#if (CONFIG_EXAMPLES_FOC_NXSCOPE_CFG & FOC_NXSCOPE_VDQ)
nxscope_chan_init(&nxs->nxs, i++, "vdq", u.u8, 2, 0);
#endif
#if (CONFIG_EXAMPLES_FOC_NXSCOPE_CFG & FOC_NXSCOPE_VAB)
nxscope_chan_init(&nxs->nxs, i++, "vab", u.u8, 2, 0);
#endif
#if (CONFIG_EXAMPLES_FOC_NXSCOPE_CFG & FOC_NXSCOPE_AEL)
nxscope_chan_init(&nxs->nxs, i++, "a_el", u.u8, 1, 0);
#endif
#if (CONFIG_EXAMPLES_FOC_NXSCOPE_CFG & FOC_NXSCOPE_AM)
nxscope_chan_init(&nxs->nxs, i++, "a_m", u.u8, 1, 0);
#endif
#if (CONFIG_EXAMPLES_FOC_NXSCOPE_CFG & FOC_NXSCOPE_VEL)
nxscope_chan_init(&nxs->nxs, i++, "v_el", u.u8, 1, 0);
#endif
#if (CONFIG_EXAMPLES_FOC_NXSCOPE_CFG & FOC_NXSCOPE_VM)
nxscope_chan_init(&nxs->nxs, i++, "v_m", u.u8, 1, 0);
#endif
#if (CONFIG_EXAMPLES_FOC_NXSCOPE_CFG & FOC_NXSCOPE_VBUS)
nxscope_chan_init(&nxs->nxs, i++, "vbus", u.u8, 1, 0);
#endif
#if (CONFIG_EXAMPLES_FOC_NXSCOPE_CFG & FOC_NXSCOPE_SPTORQ)
nxscope_chan_init(&nxs->nxs, i++, "sp_torq", u.u8, 3, 0);
#endif
#if (CONFIG_EXAMPLES_FOC_NXSCOPE_CFG & FOC_NXSCOPE_SPVEL)
nxscope_chan_init(&nxs->nxs, i++, "sp_vel", u.u8, 3, 0);
#endif
#if (CONFIG_EXAMPLES_FOC_NXSCOPE_CFG & FOC_NXSCOPE_SPPOS)
nxscope_chan_init(&nxs->nxs, i++, "sp_pos", u.u8, 3, 0);
#endif
#if (CONFIG_EXAMPLES_FOC_NXSCOPE_CFG & FOC_NXSCOPE_DQREF)
nxscope_chan_init(&nxs->nxs, i++, "dqref", u.u8, 2, 0);
#endif
#if (CONFIG_EXAMPLES_FOC_NXSCOPE_CFG & FOC_NXSCOPE_VDQCOMP)
nxscope_chan_init(&nxs->nxs, i++, "vdqcomp", u.u8, 2, 0);
#endif
#if (CONFIG_EXAMPLES_FOC_NXSCOPE_CFG & FOC_NXSCOPE_SVM3)
nxscope_chan_init(&nxs->nxs, i++, "svm3", u.u8, 4, 0);
#endif
#if (CONFIG_EXAMPLES_FOC_NXSCOPE_CFG & FOC_NXSCOPE_VOBS)
nxscope_chan_init(&nxs->nxs, i++, "vobs", u.u8, 1, 0);
#endif
#if (CONFIG_EXAMPLES_FOC_NXSCOPE_CFG & FOC_NXSCOPE_AOBS)
nxscope_chan_init(&nxs->nxs, i++, "aobs", u.u8, 1, 0);
#endif
if (i > CONFIG_EXAMPLES_FOC_NXSCOPE_CHANNELS)
{
PRINTF("ERROR: invalid nxscope channels value %d\n", i);
ret = -ENOBUFS;
goto errout;
}
}
#ifdef CONFIG_EXAMPLES_FOC_NXSCOPE_THREAD
/* Configure thread */
pthread_attr_init(&attr);
param.sched_priority = CONFIG_EXAMPLES_FOC_NXSCOPE_PRIO;
pthread_attr_setschedparam(&attr, &param);
pthread_attr_setstacksize(&attr, CONFIG_EXAMPLES_FOC_NXSCOPE_STACKSIZE);
/* Create nxscope thread */
ret = pthread_create(&nxsthr, &attr, foc_nxscope_thr, nxs);
if (ret != OK)
{
PRINTF("ERROR: pthread_create failed %d\n", ret);
goto errout;
}
/* Set thread name */
ret = pthread_setname_np(nxsthr, "nxsthr");
if (ret != OK)
{
PRINTF("ERROR: pthread_setname_np failed %d\n", ret);
goto errout;
}
#endif
errout:
return ret;
}
/****************************************************************************
* Name: foc_nxscope_deinit
****************************************************************************/
void foc_nxscope_deinit(FAR struct foc_nxscope_s *nxs)
{
DEBUGASSERT(nxs);
nxscope_ser_deinit(nxs->nxs.intf_cmd);
nxscope_proto_ser_deinit(nxs->nxs.proto_cmd);
nxscope_deinit(&nxs->nxs);
}
/****************************************************************************
* Name: foc_nxscope_work
****************************************************************************/
void foc_nxscope_work(FAR struct foc_nxscope_s *nxs)
{
int ret = OK;
/* Flush stream data */
ret = nxscope_stream(&nxs->nxs);
if (ret < 0)
{
PRINTF("ERROR: nxscope_stream failed %d\n", ret);
}
ret = nxscope_recv(&nxs->nxs);
if (ret < 0)
{
PRINTF("ERROR: nxscope_recv failed %d\n", ret);
}
}
Reference in New Issue View Git Blame Copy Permalink