This is an automated email from the ASF dual-hosted git repository. xiaoxiang pushed a commit to branch master in repository https://gitbox.apache.org/repos/asf/nuttx-apps.git
commit 7e5a436ff9a04ca77b8c4140e866f013a4da6776 Author: raiden00pl <[email protected]> AuthorDate: Wed May 17 12:31:25 2023 +0200 examples/foc: add real time data capture with the NxScope library --- examples/foc/Kconfig | 87 ++++++++++++ examples/foc/Makefile | 6 + examples/foc/foc_fixed16_thr.c | 98 +++++++++++++ examples/foc/foc_float_thr.c | 98 +++++++++++++ examples/foc/foc_main.c | 104 ++++++++++++-- examples/foc/foc_nxscope.c | 313 +++++++++++++++++++++++++++++++++++++++++ examples/foc/foc_nxscope.h | 92 ++++++++++++ examples/foc/foc_thr.h | 17 ++- 8 files changed, 797 insertions(+), 18 deletions(-) diff --git a/examples/foc/Kconfig b/examples/foc/Kconfig index 70aedd380..bb9dc9422 100644 --- a/examples/foc/Kconfig +++ b/examples/foc/Kconfig @@ -421,5 +421,92 @@ config EXAMPLES_FOC_RUN_DISABLE bool "FOC Disable FOC motor controller" default n +config EXAMPLES_FOC_NXSCOPE + bool "FOC nxscope support" + depends on LOGGING_NXSCOPE + select LOGGING_NXSCOPE_DISABLE_PUTLOCK + default n + ---help--- + This option enables a controller real-time data capture with + the NxScope library. + +if EXAMPLES_FOC_NXSCOPE + +config EXAMPLES_FOC_NXSCOPE_SERIAL + bool "FOC nxscope on serial port" + select LOGGING_NXSCOPE_INTF_SERIAL + default y + +if EXAMPLES_FOC_NXSCOPE_SERIAL + +config EXAMPLES_FOC_NXSCOPE_SERIAL_PATH + string "FOC nxscope serial path" + default "/dev/ttyS0" + +config EXAMPLES_FOC_NXSCOPE_SERIAL_BAUD + int "FOC nxscope serial baud" + default 115200 + +endif # EXAMPLES_FOC_NXSCOPE_SERIAL + +config EXAMPLES_FOC_NXSCOPE_STREAMBUF_LEN + int "FOC nxscope stream buffer length" + default 512 + +config EXAMPLES_FOC_NXSCOPE_RXBUF_LEN + int "FOC nxscope RX buffer length" + default 64 + +config EXAMPLES_FOC_NXSCOPE_RXPADDING + int "FOC nxscope RX padding" + default 0 + +config EXAMPLES_FOC_NXSCOPE_CHANNELS + int "FOC nxscope channels" + default 0 + +config EXAMPLES_FOC_NXSCOPE_PRESCALER + int "FOC nxscope prescaler" + default 1 + ---help--- + This option allows you to reduce the frequency of adding samples to + the NxScope buffer. + +config EXAMPLES_FOC_NXSCOPE_CFG + hex "FOC nxscope configuration" + default 0x00000000 + ---help--- + Each bit defines the controller state variable that can be captured. + Look at foc_nxscope.h for bits definitions. + +config EXAMPLES_FOC_NXSCOPE_START + bool "FOC nxscope start frame sync" + default n + ---help--- + If this option is set, the controller will be waiting for the start + frame from a NxScope master device. This allows us to capture + controller data from the very beginning of its operation. + +config EXAMPLES_FOC_NXSCOPE_THREAD + bool "FOC nxscope uses separate thread" + default n + ---help--- + Use a separate thread for NxScope communication. + +if EXAMPLES_FOC_NXSCOPE_THREAD + +config EXAMPLES_FOC_NXSCOPE_PRIO + int "FOC nxscope thread priority" + default 100 + +config EXAMPLES_FOC_NXSCOPE_STACKSIZE + int "FOC nxscope thread stack size" + default 2048 + +endif # EXAMPLES_FOC_NXSCOPE_THREAD + +endif # EXAMPLES_FOC_NXSCOPE + endif # EXAMPLES_FOC + diff --git a/examples/foc/Makefile b/examples/foc/Makefile index 68b0d61b2..51f603577 100644 --- a/examples/foc/Makefile +++ b/examples/foc/Makefile @@ -50,4 +50,10 @@ ifeq ($(CONFIG_INDUSTRY_FOC_FLOAT),y) CSRCS += foc_float_thr.c foc_motor_f32.c endif +# NxScope support + +ifeq ($(CONFIG_EXAMPLES_FOC_NXSCOPE),y) + CSRCS += foc_nxscope.c +endif + include $(APPDIR)/Application.mk diff --git a/examples/foc/foc_fixed16_thr.c b/examples/foc/foc_fixed16_thr.c index 5735adf29..87dc36ea1 100644 --- a/examples/foc/foc_fixed16_thr.c +++ b/examples/foc/foc_fixed16_thr.c @@ -38,6 +38,10 @@ #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 ****************************************************************************/ @@ -173,6 +177,91 @@ static int foc_state_print(FAR struct foc_motor_b16_s *motor) } #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) +# warning not supported yet + i++; +#endif +#if (CONFIG_EXAMPLES_FOC_NXSCOPE_CFG & FOC_NXSCOPE_VM) +# warning not supported yet + i++; +#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_t(&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_t(&nxs->nxs, i++, ptr, 2); +#endif + + nxscope_unlock(&nxs->nxs); +} +#endif + /**************************************************************************** * Public Functions ****************************************************************************/ @@ -355,6 +444,15 @@ int foc_fixed16_thr(FAR struct foc_ctrl_env_s *envp) goto errout; } +#ifdef CONFIG_EXAMPLES_FOC_NXSCOPE + /* Capture nxscope samples */ + + if (time % CONFIG_EXAMPLES_FOC_NXSCOPE_PRESCALER == 0) + { + foc_fixed16_nxscope(envp->nxs, &motor, &dev); + } +#endif + /* Terminate control thread */ if (motor.ctrl_state == FOC_CTRL_STATE_TERMINATE) diff --git a/examples/foc/foc_float_thr.c b/examples/foc/foc_float_thr.c index 131d3c8e4..913162d84 100644 --- a/examples/foc/foc_float_thr.c +++ b/examples/foc/foc_float_thr.c @@ -38,6 +38,10 @@ #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 ****************************************************************************/ @@ -174,6 +178,91 @@ static int foc_state_print(FAR struct foc_motor_f32_s *motor) } #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 + + nxscope_lock(&nxs->nxs); + +#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) +# warning not supported yet + i++; +#endif +#if (CONFIG_EXAMPLES_FOC_NXSCOPE_CFG & FOC_NXSCOPE_VM) +# warning not supported yet + i++; +#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 + + nxscope_unlock(&nxs->nxs); +} +#endif + /**************************************************************************** * Public Functions ****************************************************************************/ @@ -339,6 +428,15 @@ int foc_float_thr(FAR struct foc_ctrl_env_s *envp) } #endif +#ifdef CONFIG_EXAMPLES_FOC_NXSCOPE + /* Capture nxscope samples */ + + if (time % CONFIG_EXAMPLES_FOC_NXSCOPE_PRESCALER == 0) + { + foc_float_nxscope(envp->nxs, &motor, &dev); + } +#endif + #ifdef CONFIG_EXAMPLES_FOC_STATE_USE_MODEL_PMSM /* Feed FOC model with data */ diff --git a/examples/foc/foc_main.c b/examples/foc/foc_main.c index 3e8775616..f4d8f95d2 100644 --- a/examples/foc/foc_main.c +++ b/examples/foc/foc_main.c @@ -43,6 +43,10 @@ #include "industry/foc/foc_common.h" +#ifdef CONFIG_EXAMPLES_FOC_NXSCOPE +# include "foc_nxscope.h" +#endif + /**************************************************************************** * Pre-processor Definitions ****************************************************************************/ @@ -59,6 +63,8 @@ * Private Data ****************************************************************************/ +/* Default configuration */ + struct args_s g_args = { .time = CONFIG_EXAMPLES_FOC_TIME_DEFAULT, @@ -106,6 +112,11 @@ struct args_s g_args = } }; +/* Start allowed at defaule */ + +static bool g_start_allowed = true; +static pthread_mutex_t g_start_allowed_lock = PTHREAD_MUTEX_INITIALIZER; + /**************************************************************************** * Private Functions ****************************************************************************/ @@ -191,6 +202,20 @@ static int foc_kill_send(mqd_t mqd) return foc_mq_send(mqd, CONTROL_MQ_MSG_KILL, (FAR void *)&tmp); } +#ifdef CONFIG_EXAMPLES_FOC_NXSCOPE_START +/**************************************************************************** + * Name: foc_nxscope_cb_start + ****************************************************************************/ + +static int foc_nxscope_cb_start(FAR void *priv, bool start) +{ + pthread_mutex_lock(&g_start_allowed_lock); + g_start_allowed = start; + pthread_mutex_unlock(&g_start_allowed_lock); + return OK; +} +#endif + /**************************************************************************** * Public Functions ****************************************************************************/ @@ -209,6 +234,10 @@ int main(int argc, char *argv[]) int ret = OK; int i = 0; int time = 0; + bool startallowed = false; +#ifdef CONFIG_EXAMPLES_FOC_NXSCOPE + struct foc_nxscope_s nxs; +#endif /* Reset some data */ @@ -216,6 +245,9 @@ int main(int argc, char *argv[]) memset(foc, 0, sizeof(struct foc_ctrl_env_s) * CONFIG_MOTOR_FOC_INST); memset(threads, 0, sizeof(pthread_t) * CONFIG_MOTOR_FOC_INST); memset(&data, 0, sizeof(struct foc_intf_data_s)); +#ifdef CONFIG_EXAMPLES_FOC_NXSCOPE + memset(&nxs, 0, sizeof(struct foc_nxscope_s)); +#endif #ifdef CONFIG_BUILTIN /* Parse the command line */ @@ -229,7 +261,7 @@ int main(int argc, char *argv[]) if (ret < 0) { PRINTF("ERROR: validate args failed\n"); - goto errout_no_threads; + goto errout_no_nxscope; } #ifndef CONFIG_NSH_ARCHINIT @@ -246,6 +278,28 @@ int main(int argc, char *argv[]) PRINTF("\nStart foc_main application!\n\n"); +#ifdef CONFIG_EXAMPLES_FOC_NXSCOPE + +# ifdef CONFIG_EXAMPLES_FOC_NXSCOPE_START + /* Wait for nxscope */ + + g_start_allowed = false; + + /* Connect start callback */ + + nxs.cb.start = foc_nxscope_cb_start; +# endif + + /* Initialize nxscope */ + + ret = foc_nxscope_init(&nxs); + if (ret < 0) + { + PRINTF("ERROR: failed to initialize nxscope %d\n", ret); + goto errout_no_threads; + } +#endif + /* Initialize threads */ ret = foc_threads_init(); @@ -271,6 +325,9 @@ int main(int argc, char *argv[]) /* Get configuration */ foc[i].cfg = &g_args.cfg; +#ifdef CONFIG_EXAMPLES_FOC_NXSCOPE + foc[i].nxs = &nxs; +#endif if (g_args.en & (1 << i)) { @@ -311,6 +368,11 @@ int main(int argc, char *argv[]) { PRINTFV("foc_main loop %d\n", time); +#if defined(CONFIG_EXAMPLES_FOC_NXSCOPE) && \ + !defined(CONFIG_EXAMPLES_FOC_NXSCOPE_THREAD) + foc_nxscope_work(&nxs); +#endif + /* Get active control threads */ thrs_active = foc_threads_get(); @@ -407,26 +469,35 @@ int main(int argc, char *argv[]) if (data.started == false) { - for (i = 0; i < CONFIG_MOTOR_FOC_INST; i += 1) + /* Is start allowed now ? */ + + pthread_mutex_lock(&g_start_allowed_lock); + startallowed = g_start_allowed; + pthread_mutex_unlock(&g_start_allowed_lock); + + if (startallowed) { - if ((g_args.en & (1 << i)) && (thrs_active & (1 << i))) + for (i = 0; i < CONFIG_MOTOR_FOC_INST; i += 1) { - PRINTFV("Send start to %d\n", i); + if ((g_args.en & (1 << i)) && (thrs_active & (1 << i))) + { + PRINTFV("Send start to %d\n", i); - /* Send START to threads */ + /* Send START to threads */ - ret = foc_start_send(mqd[i]); - if (ret < 0) - { - PRINTF("ERROR: foc_start_send failed %d\n", ret); - goto errout; + ret = foc_start_send(mqd[i]); + if (ret < 0) + { + PRINTF("ERROR: foc_start_send failed %d\n", ret); + goto errout; + } } } - } - /* Set flag */ + /* Set flag */ - data.started = true; + data.started = true; + } } /* Handle run time */ @@ -514,8 +585,15 @@ errout_no_intf: foc_threads_deinit(); +#ifdef CONFIG_EXAMPLES_FOC_NXSCOPE errout_no_threads: + /* De-initialize NxScope */ + + foc_nxscope_deinit(&nxs); +#endif + +errout_no_nxscope: PRINTF("foc_main exit\n"); return 0; } diff --git a/examples/foc/foc_nxscope.c b/examples/foc/foc_nxscope.c new file mode 100644 index 000000000..fef6ae239 --- /dev/null +++ b/examples/foc/foc_nxscope.c @@ -0,0 +1,313 @@ +/**************************************************************************** + * 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 + +#ifdef CONFIG_LOGGING_NXSCOPE_INTF_SERIAL +# 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 (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); + } +} diff --git a/examples/foc/foc_nxscope.h b/examples/foc/foc_nxscope.h new file mode 100644 index 000000000..bfbdda833 --- /dev/null +++ b/examples/foc/foc_nxscope.h @@ -0,0 +1,92 @@ +/**************************************************************************** + * apps/examples/foc/foc_nxscope.h + * + * 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. + * + ****************************************************************************/ + +#ifndef __APPS_EXAMPLES_FOC_FOC_NXSCOPE_H +#define __APPS_EXAMPLES_FOC_FOC_NXSCOPE_H + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include <nuttx/config.h> + +#include "logging/nxscope/nxscope.h" + +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ + +/* Definitions for CONFIG_EXAMPLES_FOC_NXSCOPE_CFG bit-mask */ + +#define FOC_NXSCOPE_IABC (1 << 0) /* Phases current */ +#define FOC_NXSCOPE_IDQ (1 << 1) /* Current dq */ +#define FOC_NXSCOPE_IAB (1 << 2) /* Current alpha-beta */ +#define FOC_NXSCOPE_VABC (1 << 3) /* Phases voltage */ +#define FOC_NXSCOPE_VDQ (1 << 4) /* Voltage dq */ +#define FOC_NXSCOPE_VAB (1 << 5) /* Voltage alpha-beta */ +#define FOC_NXSCOPE_AEL (1 << 6) /* Electrical angle */ +#define FOC_NXSCOPE_AM (1 << 7) /* Mechanical angle */ +#define FOC_NXSCOPE_VEL (1 << 8) /* Electrical velocity */ +#define FOC_NXSCOPE_VM (1 << 9) /* Mechanical velocity */ +#define FOC_NXSCOPE_VBUS (1 << 10) /* VBUS */ +#define FOC_NXSCOPE_SPTORQ (1 << 11) /* Torque setpoint */ +#define FOC_NXSCOPE_SPVEL (1 << 12) /* Velolcity setpoint */ +#define FOC_NXSCOPE_SPPOS (1 << 13) /* Position setpoint */ +#define FOC_NXSCOPE_DQREF (1 << 14) /* DQ reference */ +#define FOC_NXSCOPE_VDQCOMP (1 << 15) /* VDQ compensation */ + /* Max 32-bit */ + +/**************************************************************************** + * Public Type Definition + ****************************************************************************/ + +struct foc_nxscope_s +{ + struct nxscope_s nxs; + struct nxscope_intf_s intf; + struct nxscope_proto_s proto; + struct nxscope_ser_cfg_s ser_cfg; + struct nxscope_callbacks_s cb; + uint8_t ch_per_inst; +}; + +/**************************************************************************** + * Public Function Prototypes + ****************************************************************************/ + +/**************************************************************************** + * Name: foc_nxscope_init + ****************************************************************************/ + +int foc_nxscope_init(FAR struct foc_nxscope_s *nxs); + +/**************************************************************************** + * Name: foc_nxscope_deinit + ****************************************************************************/ + +void foc_nxscope_deinit(FAR struct foc_nxscope_s *nxs); + +/**************************************************************************** + * Name: foc_nxscope_work + ****************************************************************************/ + +void foc_nxscope_work(FAR struct foc_nxscope_s *nxs); + +#endif /* __APPS_EXAMPLES_FOC_FOC_NXSCOPE_H */ diff --git a/examples/foc/foc_thr.h b/examples/foc/foc_thr.h index e3d8f1b41..53fe854a4 100644 --- a/examples/foc/foc_thr.h +++ b/examples/foc/foc_thr.h @@ -35,6 +35,10 @@ #include "foc_device.h" +#ifdef CONFIG_EXAMPLES_FOC_NXSCOPE +# include "foc_nxscope.h" +#endif + /**************************************************************************** * Public Type Definition ****************************************************************************/ @@ -107,11 +111,14 @@ enum foc_controller_state_e struct foc_ctrl_env_s { - mqd_t mqd; /* Control msg queue */ - int id; /* FOC device id */ - int inst; /* Type specific instance counter */ - int type; /* Controller type */ - struct foc_thr_cfg_s *cfg; /* Control thread configuration */ + mqd_t mqd; /* Control msg queue */ + int id; /* FOC device id */ + int inst; /* Type specific instance counter */ + int type; /* Controller type */ + FAR struct foc_thr_cfg_s *cfg; /* Control thread configuration */ +#ifdef CONFIG_EXAMPLES_FOC_NXSCOPE + FAR struct foc_nxscope_s *nxs; /* nxscope handler */ +#endif }; /****************************************************************************
