mirror of
https://github.com/apache/nuttx-apps.git
synced 2026-08-01 20:29:00 +00:00
uorb: add generator tool.
e.g.: 1. uorb_generator -f /data/sensor/19700101000003/sensor_accel_uncal0.csv -t sensor_accel_uncal2 2. uorb_generator -s -r 5 -n 200 -t sensor_accel_uncal2 timestamp:1,x:-2.0,y:3,z:4,temperature:5 Signed-off-by: likun17 <likun17@xiaomi.com>
This commit is contained in:
likun17
Xiang Xiao
parent
1c769d1410
commit
9cf9b862c1
4 changed files with 416 additions and 0 deletions
|
|
@ -51,6 +51,22 @@ if(CONFIG_UORB)
|
|||
uorb)
|
||||
endif()
|
||||
|
||||
if(CONFIG_UORB_GENERATOR)
|
||||
nuttx_add_application(
|
||||
NAME
|
||||
uorb_generator
|
||||
PRIORITY
|
||||
${CONFIG_UORB_PRIORITY}
|
||||
STACKSIZE
|
||||
${CONFIG_UORB_STACKSIZE}
|
||||
MODULE
|
||||
${CONFIG_UORB}
|
||||
SRCS
|
||||
generator.c
|
||||
DEPENDS
|
||||
uorb)
|
||||
endif()
|
||||
|
||||
if(CONFIG_UORB_TEST)
|
||||
nuttx_add_application(
|
||||
NAME
|
||||
|
|
|
|||
|
|
@ -22,6 +22,10 @@ config UORB_LISTENER
|
|||
bool "uorb listener"
|
||||
default n
|
||||
|
||||
config UORB_GENERATOR
|
||||
bool "uorb generator"
|
||||
default n
|
||||
|
||||
config UORB_TESTS
|
||||
bool "uorb unit tests"
|
||||
default n
|
||||
|
|
|
|||
|
|
@ -36,6 +36,11 @@ MAINSRC += listener.c
|
|||
PROGNAME += uorb_listener
|
||||
endif
|
||||
|
||||
ifneq ($(CONFIG_UORB_GENERATOR),)
|
||||
MAINSRC += generator.c
|
||||
PROGNAME += uorb_generator
|
||||
endif
|
||||
|
||||
ifneq ($(CONFIG_UORB_TESTS),)
|
||||
CSRCS += test/utility.c
|
||||
MAINSRC += test/unit_test.c
|
||||
|
|
|
|||
391
system/uorb/generator.c
Normal file
391
system/uorb/generator.c
Normal file
|
|
@ -0,0 +1,391 @@
|
|||
/****************************************************************************
|
||||
* apps/system/uorb/generator.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/signal.h>
|
||||
#include <nuttx/streams.h>
|
||||
|
||||
#include <ctype.h>
|
||||
#include <uORB/uORB.h>
|
||||
|
||||
/****************************************************************************
|
||||
* Pre-processor Definitions
|
||||
****************************************************************************/
|
||||
|
||||
#define GENERATOR_CACHE_BUFF 4096
|
||||
|
||||
/****************************************************************************
|
||||
* Private Types
|
||||
****************************************************************************/
|
||||
|
||||
struct sensor_gen_info_s
|
||||
{
|
||||
struct orb_object obj;
|
||||
FAR FILE *file;
|
||||
};
|
||||
|
||||
/****************************************************************************
|
||||
* Private Data
|
||||
****************************************************************************/
|
||||
|
||||
static bool g_should_exit = false;
|
||||
|
||||
/****************************************************************************
|
||||
* Private Functions
|
||||
****************************************************************************/
|
||||
|
||||
static void usage(void)
|
||||
{
|
||||
uorbinfo_raw("\n\
|
||||
The tool publishes topic data via uorb.\n\
|
||||
Notice:NSH_LINELEN must be set to 128 or more.\n\
|
||||
\n\
|
||||
generator <command> [arguments...]\n\
|
||||
Commands:\n\
|
||||
\t<topics_name> The playback topic name.\n\
|
||||
\t[-h ] Listener commands help.\n\
|
||||
\t[-f <val> ] File path to be played back(absolute path).\n\
|
||||
\t[-n <val> ] Number of playbacks(fake model), default: 1\n\
|
||||
\t[-r <val> ] The rate for playing fake data is only valid\
|
||||
when parameter 's' is used. default:10hz.\n\
|
||||
\t[-s <val> ] Playback fake data.\n\
|
||||
\t[-t <val> ] Playback topic.\n\
|
||||
\t e.g.:\n\
|
||||
\t\tsim - sensor_accel0:\n\
|
||||
\t\t uorb_generator -n 100 -r 5 -s -t sensor_accel0 timestamp:23191100,\
|
||||
x:0.1,y:9.7,z:0.81,temperature:22.15\n\n\
|
||||
\t\tsim - sensor_baro0:\n\
|
||||
\t\t uorb_generator -n 100 -r 5 -s -t sensor_baro0 timestamp:23191100,\
|
||||
pressure:999.12,temperature:26.34\n\n\
|
||||
\t\tfies - sensor_accel1\n\
|
||||
\t\t uorb_generator -f /data/uorb/20240823061723/sensor_accel0.csv\
|
||||
-t sensor_accel1\
|
||||
\t\t\n\
|
||||
");
|
||||
}
|
||||
|
||||
static void exit_handler(int signo)
|
||||
{
|
||||
g_should_exit = true;
|
||||
}
|
||||
|
||||
static int get_play_orb_id(FAR const char *filter,
|
||||
FAR struct sensor_gen_info_s *info)
|
||||
{
|
||||
struct orb_object object;
|
||||
|
||||
if (filter)
|
||||
{
|
||||
object.meta = orb_get_meta(filter);
|
||||
if (object.meta == NULL)
|
||||
{
|
||||
uorbinfo_raw("Input error built-in name:[%s]", filter);
|
||||
return ERROR;
|
||||
}
|
||||
|
||||
object.instance = 0;
|
||||
if (isdigit(filter[strlen(filter) - 1]))
|
||||
{
|
||||
object.instance = filter[strlen(filter) - 1] - '0';
|
||||
}
|
||||
|
||||
info->obj = object;
|
||||
return OK;
|
||||
}
|
||||
|
||||
uorbinfo_raw("The entered built-in topic name is empty.");
|
||||
return ERROR;
|
||||
}
|
||||
|
||||
/****************************************************************************
|
||||
* Name: replay_worker
|
||||
*
|
||||
* Description:
|
||||
* Playback data files.
|
||||
*
|
||||
* Input Parameters:
|
||||
* sensor_gen Generator objects.
|
||||
*
|
||||
* Returned Value:
|
||||
* 0 on success, otherwise -1
|
||||
****************************************************************************/
|
||||
|
||||
static int replay_worker(FAR struct sensor_gen_info_s *sensor_gen)
|
||||
{
|
||||
struct lib_meminstream_s meminstream;
|
||||
bool is_frist = true;
|
||||
uint64_t last_time;
|
||||
uint64_t tmp_time;
|
||||
FAR uint8_t *data;
|
||||
FAR char *line;
|
||||
int sleep_time;
|
||||
int lastc;
|
||||
int ret;
|
||||
int fd;
|
||||
|
||||
line = kmm_zalloc(GENERATOR_CACHE_BUFF);
|
||||
if (line == NULL)
|
||||
{
|
||||
return -ENOMEM;
|
||||
}
|
||||
|
||||
if (fgets(line, GENERATOR_CACHE_BUFF, sensor_gen->file) != NULL)
|
||||
{
|
||||
if (strstr(line, sensor_gen->obj.meta->o_name) == NULL)
|
||||
{
|
||||
uorbinfo_raw("Topic and file do not match!");
|
||||
return -EINVAL;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
uorbinfo_raw("Playback file format error!");
|
||||
return -EINVAL;
|
||||
}
|
||||
|
||||
data = kmm_zalloc(sensor_gen->obj.meta->o_size);
|
||||
if (data == NULL)
|
||||
{
|
||||
return -ENOMEM;
|
||||
}
|
||||
|
||||
fd = orb_advertise_multi_queue_persist(sensor_gen->obj.meta,
|
||||
data, &sensor_gen->obj.instance, 1);
|
||||
if (fd < 0)
|
||||
{
|
||||
uorbinfo_raw("Playback orb advertise failed[%d]!", fd);
|
||||
kmm_free(data);
|
||||
return fd;
|
||||
}
|
||||
|
||||
while (fgets(line, GENERATOR_CACHE_BUFF, sensor_gen->file) != NULL)
|
||||
{
|
||||
lib_meminstream(&meminstream, line, GENERATOR_CACHE_BUFF);
|
||||
ret = lib_bscanf(&meminstream.common, &lastc,
|
||||
sensor_gen->obj.meta->o_format, data);
|
||||
if (ret >= 0)
|
||||
{
|
||||
tmp_time = *(uint64_t *)data;
|
||||
if (is_frist)
|
||||
{
|
||||
last_time = *(uint64_t *)data;
|
||||
is_frist = false;
|
||||
}
|
||||
else
|
||||
{
|
||||
sleep_time = tmp_time - last_time;
|
||||
if (sleep_time > 0)
|
||||
{
|
||||
nxsig_usleep(sleep_time);
|
||||
}
|
||||
|
||||
last_time = tmp_time;
|
||||
}
|
||||
|
||||
if (OK != orb_publish(sensor_gen->obj.meta, fd, data))
|
||||
{
|
||||
uorbinfo_raw("Topic publish error!");
|
||||
break;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
uorbinfo_raw("[ignore] The text data for this line is wrong![%s]!",
|
||||
line);
|
||||
}
|
||||
}
|
||||
|
||||
orb_unadvertise(fd);
|
||||
kmm_free(data);
|
||||
return 0;
|
||||
}
|
||||
|
||||
/****************************************************************************
|
||||
* Name: fake_worker
|
||||
*
|
||||
* Description:
|
||||
* Publish fake data.
|
||||
*
|
||||
* Input Parameters:
|
||||
* sensor_gen Generator objects.
|
||||
* nb_cycle Number of publications.
|
||||
* topic_rate Publish frequency.
|
||||
* buffer String data to publish.
|
||||
*
|
||||
* Returned Value:
|
||||
* 0 on success, otherwise -1
|
||||
****************************************************************************/
|
||||
|
||||
static int fake_worker(FAR struct sensor_gen_info_s *sensor_gen,
|
||||
int nb_cycle, float topic_rate, FAR char *buffer)
|
||||
{
|
||||
struct lib_meminstream_s meminstream;
|
||||
FAR uint8_t *data;
|
||||
uint64_t interval;
|
||||
int lastc;
|
||||
int fd;
|
||||
int i;
|
||||
|
||||
if (buffer == NULL)
|
||||
{
|
||||
return -EINVAL;
|
||||
}
|
||||
|
||||
data = kmm_zalloc(sensor_gen->obj.meta->o_size);
|
||||
if (data == NULL)
|
||||
{
|
||||
return -ENOMEM;
|
||||
}
|
||||
|
||||
lib_meminstream(&meminstream, buffer, strlen(buffer));
|
||||
if (lib_bscanf(&meminstream.common, &lastc,
|
||||
sensor_gen->obj.meta->o_format, data) < 0)
|
||||
{
|
||||
uorbinfo_raw("Input string data parsing error![%s]", buffer);
|
||||
goto error;
|
||||
}
|
||||
|
||||
fd = orb_advertise_multi_queue_persist(sensor_gen->obj.meta,
|
||||
data, &sensor_gen->obj.instance, 1);
|
||||
if (fd < 0)
|
||||
{
|
||||
uorbinfo_raw("Fake orb advertise failed[%d]!", fd);
|
||||
goto error;
|
||||
}
|
||||
|
||||
interval = topic_rate ? (1000000 / topic_rate) : 500000;
|
||||
|
||||
for (i = 0; i < nb_cycle; ++i)
|
||||
{
|
||||
*(uint64_t *)data = orb_absolute_time();
|
||||
if (OK != orb_publish(sensor_gen->obj.meta, fd, data))
|
||||
{
|
||||
uorbinfo_raw("Topic publish error!");
|
||||
goto error;
|
||||
}
|
||||
|
||||
nxsig_usleep(interval);
|
||||
}
|
||||
|
||||
kmm_free(data);
|
||||
return 0;
|
||||
|
||||
error:
|
||||
kmm_free(data);
|
||||
return -1;
|
||||
}
|
||||
|
||||
/****************************************************************************
|
||||
* Public Functions
|
||||
****************************************************************************/
|
||||
|
||||
int main(int argc, FAR char *argv[])
|
||||
{
|
||||
FAR struct sensor_gen_info_s sensor_tmp;
|
||||
float topic_rate = 0.0f;
|
||||
FAR char *filter = NULL;
|
||||
FAR char *topic = NULL;
|
||||
FAR char *path = NULL;
|
||||
int nb_cycle = 1;
|
||||
bool sim = false;
|
||||
int opt;
|
||||
int ret;
|
||||
|
||||
g_should_exit = false;
|
||||
if (signal(SIGINT, exit_handler) == SIG_ERR)
|
||||
{
|
||||
return 1;
|
||||
}
|
||||
|
||||
while ((opt = getopt(argc, argv, "f:t:r:n:sh")) != -1)
|
||||
{
|
||||
switch (opt)
|
||||
{
|
||||
case 'f':
|
||||
path = optarg;
|
||||
break;
|
||||
|
||||
case 't':
|
||||
topic = optarg;
|
||||
break;
|
||||
|
||||
case 'r':
|
||||
topic_rate = atof(optarg);
|
||||
if (topic_rate < 0)
|
||||
{
|
||||
goto error;
|
||||
}
|
||||
break;
|
||||
|
||||
case 'n':
|
||||
nb_cycle = strtol(optarg, NULL, 0);
|
||||
if (nb_cycle < 1)
|
||||
{
|
||||
goto error;
|
||||
}
|
||||
break;
|
||||
|
||||
case 's':
|
||||
sim = true;
|
||||
break;
|
||||
|
||||
case 'h':
|
||||
default:
|
||||
goto error;
|
||||
}
|
||||
}
|
||||
|
||||
if (optind < argc)
|
||||
{
|
||||
filter = argv[optind];
|
||||
}
|
||||
|
||||
ret = get_play_orb_id(topic, &sensor_tmp);
|
||||
if (ret < 0)
|
||||
{
|
||||
return -ERROR;
|
||||
}
|
||||
|
||||
if (sim)
|
||||
{
|
||||
ret = fake_worker(&sensor_tmp, nb_cycle, topic_rate, filter);
|
||||
}
|
||||
else
|
||||
{
|
||||
sensor_tmp.file = fopen(path, "r");
|
||||
if (sensor_tmp.file == NULL)
|
||||
{
|
||||
uorbinfo_raw("Failed to open file:[%s]!", path);
|
||||
return -ERROR;
|
||||
}
|
||||
|
||||
ret = replay_worker(&sensor_tmp);
|
||||
fclose(sensor_tmp.file);
|
||||
}
|
||||
|
||||
return ret;
|
||||
|
||||
error:
|
||||
usage();
|
||||
return -ERROR;
|
||||
}
|
||||
Loading…
Add table
Add a link
Reference in a new issue