320 lines
9.3 KiB
C
320 lines
9.3 KiB
C
/****************************************************************************
|
|
* 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 (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, ¶m);
|
|
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);
|
|
}
|
|
}
|