From 72635a88ca338316ec4002c096f5e64ec8f7fdd8 Mon Sep 17 00:00:00 2001 From: raiden00pl Date: Sat, 27 Apr 2024 10:05:11 +0200 Subject: [PATCH] system: add sensor streaming tool based on nxscope This tool allows you to send sensor data via the nxscope interface. Useful when we test sensors or when we just need a tool to visualize data from sensors. Works only with the new sensor framework. --- system/sensorscope/CMakeLists.txt | 31 ++ system/sensorscope/Kconfig | 72 ++++ system/sensorscope/Make.defs | 23 ++ system/sensorscope/Makefile | 34 ++ system/sensorscope/sensorscope_main.c | 555 ++++++++++++++++++++++++++ 5 files changed, 715 insertions(+) create mode 100644 system/sensorscope/CMakeLists.txt create mode 100644 system/sensorscope/Kconfig create mode 100644 system/sensorscope/Make.defs create mode 100644 system/sensorscope/Makefile create mode 100644 system/sensorscope/sensorscope_main.c diff --git a/system/sensorscope/CMakeLists.txt b/system/sensorscope/CMakeLists.txt new file mode 100644 index 000000000..554114ff2 --- /dev/null +++ b/system/sensorscope/CMakeLists.txt @@ -0,0 +1,31 @@ +# ############################################################################## +# apps/system/sensorscope/CMakeLists.txt +# +# 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. +# +# ############################################################################## + +if(CONFIG_SYSTEM_SENSORSCOPE) + nuttx_add_application( + NAME + ${CONFIG_SYSTEM_SENSORSCOPE_PROGNAME} + SRCS + sensorscope_main.c + STACKSIZE + ${CONFIG_SYSTEM_SENSORSCOPE_STACKSIZE} + PRIORITY + ${CONFIG_SYSTEM_SENSORSCOPE_PRIORITY}) +endif() diff --git a/system/sensorscope/Kconfig b/system/sensorscope/Kconfig new file mode 100644 index 000000000..a8dd84ec1 --- /dev/null +++ b/system/sensorscope/Kconfig @@ -0,0 +1,72 @@ +# +# For a description of the syntax of this configuration file, +# see the file kconfig-language.txt in the NuttX tools repository. +# + +menuconfig SYSTEM_SENSORSCOPE + bool "NxScope sensors stream app" + default n + depends on LOGGING_NXSCOPE + select LOGGING_NXSCOPE_INTF_SERIAL + select LOGGING_NXSCOPE_PROTO_SER + +if SYSTEM_SENSORSCOPE + +config SYSTEM_SENSORSCOPE_PROGNAME + string "Program name" + default "sensorscope" + +config SYSTEM_SENSORSCOPE_PRIORITY + int "nxscope task priority" + default 100 + +config SYSTEM_SENSORSCOPE_STACKSIZE + int "nxscope stack size" + default DEFAULT_TASK_STACKSIZE + +config SYSTEM_SENSORSCOPE_SERIAL_PATH + string "nxscope serial path" + default "/dev/ttyUSB0" + +config SYSTEM_SENSORSCOPE_SERIAL_BAUD + int "nxscope serial baud" + default 115200 + ---help--- + Ignored if set to 0 (for example for RTT interface) + +config SYSTEM_SENSORSCOPE_CDCACM + bool "nxscope CDCACM device support" + depends on CDCACM + default n + +config SYSTEM_SENSORSCOPE_FORCE_ENABLE + bool "nxscope force enable" + default n + +config SYSTEM_SENSORSCOPE_STREAMBUF_LEN + int "nxscope stream buffer length" + default 512 + +config SYSTEM_SENSORSCOPE_RXBUF_LEN + int "nxscope RX buffer length" + default 32 + +config SYSTEM_SENSORSCOPE_RX_PADDING + int "nxscope RX padding" + default 0 + +config SYSTEM_SENSORSCOPE_MAIN_INTERVAL + int "nxscope main interval (microseconds)" + default 100000 + ---help--- + This value is responsible for the frequency at which stream + frames will be sent and incoming frames will be received. + +config SYSTEM_SENSORSCOPE_FETCH_INTERVAL + int "nxscope sensor fetch interval (microseconds)" + default 1000 + ---help--- + This value is responsible for the frequency at which the sensors + are read + +endif # SYSTEM_SENSORSCOPE diff --git a/system/sensorscope/Make.defs b/system/sensorscope/Make.defs new file mode 100644 index 000000000..2e7664771 --- /dev/null +++ b/system/sensorscope/Make.defs @@ -0,0 +1,23 @@ +############################################################################ +# apps/systen/sensorscope/Make.defs +# +# 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. +# +############################################################################ + +ifneq ($(CONFIG_SYSTEM_SENSORSCOPE),) +CONFIGURED_APPS += $(APPDIR)/system/sensorscope +endif diff --git a/system/sensorscope/Makefile b/system/sensorscope/Makefile new file mode 100644 index 000000000..18ee63133 --- /dev/null +++ b/system/sensorscope/Makefile @@ -0,0 +1,34 @@ +############################################################################ +# apps/system/sensorscope/Makefile +# +# 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. +# +############################################################################ + +include $(APPDIR)/Make.defs + +# sensorscope built-in application info + +PROGNAME = $(CONFIG_SYSTEM_SENSORSCOPE_PROGNAME) +PRIORITY = $(CONFIG_SYSTEM_SENSORSCOPE_PRIORITY) +STACKSIZE = $(CONFIG_SYSTEM_SENSORSCOPE_STACKSIZE) +MODULE = $(CONFIG_SYSTEM_SENSORSCOPE) + +MAINSRC = sensorscope_main.c + +CSRCS = + +include $(APPDIR)/Application.mk diff --git a/system/sensorscope/sensorscope_main.c b/system/sensorscope/sensorscope_main.c new file mode 100644 index 000000000..ab80be527 --- /dev/null +++ b/system/sensorscope/sensorscope_main.c @@ -0,0 +1,555 @@ +/**************************************************************************** + * apps/system/sensorscope/sensorscope_main.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 + +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +#include + +#include "logging/nxscope/nxscope.h" + +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ + +#define SENSOR_PATH "/dev/uorb/" +#define SENSOR_PATH_MAX 62 +#define SENSOR_CHNAME_MAX 16 + +/**************************************************************************** + * Private Type Definition + ****************************************************************************/ + +struct nxsensor_info_s +{ + FAR const char *name; + size_t data_size; /* Sensor read data size */ + size_t data_offset; /* Read data offset (no timestamp) */ + size_t dim; /* Data vector dimenstion */ + int dtype; /* Data vector type */ +}; + +struct listen_object_s +{ + /* Node of object info list */ + + struct list_node node; + + /* Sensor data */ + + int fd; + FAR uint8_t *data; + + /* Sensor info */ + + FAR struct nxsensor_info_s *info; + + /* nxscope channel data */ + + int chanid; + char chname[SENSOR_CHNAME_MAX]; +}; + +struct nxscope_thr_env_s +{ + struct nxscope_s nxs; + struct list_node objlist; +}; + +/**************************************************************************** + * Private Data + ****************************************************************************/ + +/* This array defines supported sensors. */ + +struct nxsensor_info_s g_nxsensor[] = +{ + {"accel", sizeof(struct sensor_accel), 0, 3, NXSCOPE_TYPE_FLOAT}, + {"mag", sizeof(struct sensor_mag), 0, 3, NXSCOPE_TYPE_FLOAT}, + {"gyro", sizeof(struct sensor_gyro), 0, 3, NXSCOPE_TYPE_FLOAT}, + {"light", sizeof(struct sensor_light), 0, 2, NXSCOPE_TYPE_FLOAT}, + {"baro", sizeof(struct sensor_baro), 0, 2, NXSCOPE_TYPE_FLOAT}, + {"prox", sizeof(struct sensor_prox), 0, 1, NXSCOPE_TYPE_FLOAT}, + {"humi", sizeof(struct sensor_humi), 0, 1, NXSCOPE_TYPE_FLOAT}, + {"temp", sizeof(struct sensor_temp), 0, 1, NXSCOPE_TYPE_FLOAT}, + {"rgb", sizeof(struct sensor_rgb), 0, 3, NXSCOPE_TYPE_FLOAT}, + {"hall", sizeof(struct sensor_hall), 0, 1, NXSCOPE_TYPE_INT32}, + {"ir", sizeof(struct sensor_ir), 0, 1, NXSCOPE_TYPE_FLOAT}, + {"gas", sizeof(struct sensor_gas), 0, 1, NXSCOPE_TYPE_FLOAT}, +}; + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: nxscope_samples_thr + ****************************************************************************/ + +static FAR void *nxscope_samples_thr(FAR void *arg) +{ + FAR struct nxscope_thr_env_s *envp = arg; + FAR struct listen_object_s *tmp; + FAR float *data; + int ret; + size_t offset; + + DEBUGASSERT(envp); + + printf("nxscope_samples_thr\n"); + + while (1) + { + list_for_every_entry(&envp->objlist, tmp, struct listen_object_s, node) + { + /* Read data from sensor */ + + ret = read(tmp->fd, tmp->data, tmp->info->data_size); + if (ret < 0) + { + printf("ERROR: read failed %d\n", -errno); + } + else + { + /* Get vector from a given offset */ + + offset = tmp->info->data_offset + sizeof(uint64_t); + data = (float *)&tmp->data[offset]; + + nxscope_put_vfloat(&envp->nxs, tmp->chanid, data, + tmp->info->dim); + } + } + + usleep(CONFIG_SYSTEM_SENSORSCOPE_FETCH_INTERVAL); + } + + return NULL; +} + +#ifdef CONFIG_SYSTEM_SENSORSCOPE_CDCACM +/**************************************************************************** + * Name: nxscope_cdcacm_init + ****************************************************************************/ + +static int nxscope_cdcacm_init(void) +{ + struct boardioc_usbdev_ctrl_s ctrl; + FAR void *handle; + int ret; + + ctrl.usbdev = BOARDIOC_USBDEV_CDCACM; + ctrl.action = BOARDIOC_USBDEV_CONNECT; + ctrl.instance = 0; + ctrl.handle = &handle; + + ret = boardctl(BOARDIOC_USBDEV_CONTROL, (uintptr_t)&ctrl); + if (ret < 0) + { + printf("ERROR: BOARDIOC_USBDEV_CONTROL failed %d\n", ret); + goto errout; + } + +errout: + return ret; +} +#endif + +/**************************************************************************** + * Name: listener_add_object + ****************************************************************************/ + +static FAR struct listen_object_s * +listener_add_object(FAR struct list_node *objlist, + FAR struct nxsensor_info_s *info, + int fd, int chanid, FAR char *chname) +{ + FAR struct listen_object_s *tmp; + + tmp = malloc(sizeof(struct listen_object_s)); + if (tmp == NULL) + { + return NULL; + } + + /* Initialize object */ + + strncpy(tmp->chname, chname, SENSOR_CHNAME_MAX); + + tmp->info = info; + tmp->chanid = chanid; + tmp->fd = fd; + + /* Allocate space for data */ + + tmp->data = malloc(info->data_size); + + /* Add node to list */ + + list_add_tail(objlist, &tmp->node); + + return tmp; +} + +/**************************************************************************** + * Name: listener_delete_object_list + ****************************************************************************/ + +static void listener_delete_object_list(FAR struct list_node *objlist) +{ + FAR struct listen_object_s *tmp; + FAR struct listen_object_s *next; + + list_for_every_entry_safe(objlist, tmp, next, struct listen_object_s, node) + { + free(tmp->data); + list_delete(&tmp->node); + free(tmp); + } + + list_initialize(objlist); +} + +/**************************************************************************** + * Name: sensorscope_chinfo + ****************************************************************************/ + +static int sensorscope_chinfo(FAR char *path, + FAR struct nxsensor_info_s **info) +{ + int i; + + for (i = 0; i < nitems(g_nxsensor); i++) + { + if (strstr(path, g_nxsensor[i].name) != NULL) + { + *info = &g_nxsensor[i]; + return OK; + } + } + + return -EINVAL; +} + +/**************************************************************************** + * Name: nxscope_channels_num + ****************************************************************************/ + +static int nxscope_channels_num(void) +{ + FAR struct dirent *entry; + FAR DIR *dir; + int i = 0; + + dir = opendir(SENSOR_PATH); + if (!dir) + { + return 0; + } + + while ((entry = readdir(dir)) != NULL) + { + if (entry->d_type == DT_CHR) + { + i++; + } + } + + closedir(dir); + + return i; +} + +/**************************************************************************** + * Name: nxscope_channels + ****************************************************************************/ + +static int nxscope_channels(FAR struct nxscope_thr_env_s *envp) +{ + union nxscope_chinfo_type_u u; + FAR struct nxsensor_info_s *info; + FAR struct dirent *entry; + FAR struct listen_object_s *obj; + FAR DIR *dir; + int chanid; + int ret; + int fd; + char path[SENSOR_PATH_MAX]; + + /* Initialize objects list */ + + list_initialize(&envp->objlist); + + /* Open sensors direcotry */ + + dir = opendir(SENSOR_PATH); + if (!dir) + { + return 0; + } + + /* Get available sensors */ + + chanid = 0; + while ((entry = readdir(dir)) != NULL) + { + if (entry->d_type != DT_CHR) + { + continue; + } + + /* Get sensor info */ + + ret = sensorscope_chinfo(entry->d_name, &info); + if (ret != OK) + { + printf("ERROR: not supported sensor %s\n", entry->d_name); + } + else + { + snprintf(path, SENSOR_PATH_MAX, SENSOR_PATH"%s", entry->d_name); + fd = open(path, O_CLOEXEC | O_RDWR | O_NONBLOCK); + if (fd < 0) + { + printf("ERROR: failed to open %s %d\n", entry->d_name, -errno); + } + else + { + /* Add new object */ + + obj = listener_add_object(&envp->objlist, + info, fd, chanid, + basename(entry->d_name)); + if (obj == NULL) + { + printf("ERROR: failed to add listener object %d\n", + chanid); + return -ENOMEM; + } + + /* Register nxscope channel */ + + u.s.dtype = NXSCOPE_TYPE_FLOAT; + u.s._res = 0; + u.s.cri = 0; + + nxscope_chan_init(&envp->nxs, obj->chanid, obj->chname, + u.u8, obj->info->dim, 0); + + /* Next channel */ + + chanid++; + } + } + } + + return OK; +} + +/**************************************************************************** + * Name: nxscope_cb_start + ****************************************************************************/ + +static int nxscope_cb_start(FAR void *priv, bool start) +{ + UNUSED(priv); + + printf("--> nxscope_cb_start: start=%d\n", start); + + return OK; +} + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: nxscope_main + ****************************************************************************/ + +int main(int argc, FAR char *argv[]) +{ + struct nxscope_thr_env_s env; + struct nxscope_cfg_s nxs_cfg; + struct nxscope_intf_s intf; + struct nxscope_proto_s proto; + struct nxscope_ser_cfg_s nxs_ser_cfg; + struct nxscope_callbacks_s cbs; + pthread_t thread; + int ret; + +#ifndef CONFIG_NSH_ARCHINIT + /* Perform architecture-specific initialization (if configured) */ + + boardctl(BOARDIOC_INIT, 0); + +# ifdef CONFIG_BOARDCTL_FINALINIT + /* Perform architecture-specific final-initialization (if configured) */ + + boardctl(BOARDIOC_FINALINIT, 0); +# endif +#endif + +#ifdef CONFIG_SYSTEM_SENSORSCOPE_CDCACM + /* Initialize the USB CDCACM device */ + + ret = nxscope_cdcacm_init(); + if (ret < 0) + { + printf("ERROR: nxscope_cdcacm_init failed %d\n", ret); + goto errout_noproto; + } +#endif + + /* Default serial protocol */ + + ret = nxscope_proto_ser_init(&proto, NULL); + if (ret < 0) + { + printf("ERROR: nxscope_proto_ser_init failed %d\n", ret); + goto errout_noproto; + } + + /* Configuration */ + + nxs_ser_cfg.path = CONFIG_SYSTEM_SENSORSCOPE_SERIAL_PATH; + nxs_ser_cfg.nonblock = true; + nxs_ser_cfg.baud = CONFIG_SYSTEM_SENSORSCOPE_SERIAL_BAUD; + + /* Initialize serial interface */ + + ret = nxscope_ser_init(&intf, &nxs_ser_cfg); + if (ret < 0) + { + printf("ERROR: nxscope_ser_init failed %d\n", ret); + goto errout_nointf; + } + + /* Connect callbacks */ + + cbs.start_priv = NULL; + cbs.start = nxscope_cb_start; + + /* Initialize nxscope */ + + nxs_cfg.intf_cmd = &intf; + nxs_cfg.intf_stream = &intf; + nxs_cfg.proto_cmd = &proto; + nxs_cfg.proto_stream = &proto; + nxs_cfg.callbacks = &cbs; + nxs_cfg.channels = nxscope_channels_num(); + nxs_cfg.streambuf_len = CONFIG_SYSTEM_SENSORSCOPE_STREAMBUF_LEN; + nxs_cfg.rxbuf_len = CONFIG_SYSTEM_SENSORSCOPE_RXBUF_LEN; + nxs_cfg.rx_padding = CONFIG_SYSTEM_SENSORSCOPE_RX_PADDING; + + ret = nxscope_init(&env.nxs, &nxs_cfg); + if (ret < 0) + { + printf("ERROR: nxscope_init failed %d\n", ret); + goto errout_nonxscope; + } + + /* Create channels */ + + ret = nxscope_channels(&env); + if (ret != OK) + { + printf("ERROR: nxscope_channels failed %d\n", ret); + goto errout; + } + + /* Create samples thread */ + + ret = pthread_create(&thread, NULL, nxscope_samples_thr, &env); + if (ret != OK) + { + printf("ERROR: pthread_create failed %d\n", ret); + goto errout; + } + +#ifdef CONFIG_SYSTEM_SENSORSCOPE_FORCE_ENABLE + /* Enable channels and enable stream */ + + nxscope_chan_all_en(&nxs, true); + nxscope_stream_start(&nxs, true); +#endif + + /* Main loop */ + + while (1) + { + /* Flush stream data */ + + ret = nxscope_stream(&env.nxs); + if (ret < 0) + { + printf("ERROR: nxscope_stream failed %d\n", ret); + } + + /* Handle recv data */ + + ret = nxscope_recv(&env.nxs); + if (ret < 0) + { + printf("ERROR: nxscope_recv failed %d\n", ret); + } + + usleep(CONFIG_SYSTEM_SENSORSCOPE_MAIN_INTERVAL); + } + +errout: + + /* Delete objects */ + + listener_delete_object_list(&env.objlist); + + /* Deinit nxscope */ + + nxscope_deinit(&env.nxs); + +errout_nonxscope: + + /* Deinit interface */ + + nxscope_ser_deinit(&intf); + +errout_nointf: + + /* Deinit protocol */ + + nxscope_proto_ser_deinit(&proto); + +errout_noproto: + return 0; +}