From 81edbcf9570ca80060980c9d32ef1813a44a1626 Mon Sep 17 00:00:00 2001 From: likun17 Date: Sat, 14 Sep 2024 14:11:46 +0800 Subject: [PATCH 1/3] system/uorb: uorb_listener adds new features. add get sensor device information based on topic. Signed-off-by: likun17 --- system/uorb/listener.c | 70 +++++++++++++++++++++++++++++++++++++++++- 1 file changed, 69 insertions(+), 1 deletion(-) diff --git a/system/uorb/listener.c b/system/uorb/listener.c index b9013a06a99..5185b968e3f 100644 --- a/system/uorb/listener.c +++ b/system/uorb/listener.c @@ -138,6 +138,7 @@ listener [arguments...]\n\ \t[-t ] Time of listener, in seconds, default: 5\n\ \t[-T ] Top, continuously print updating objects\n\ \t[-l ] Top only execute once.\n\ +\t[-i ] Get sensor device information based on topic.\n\ "); } @@ -530,6 +531,61 @@ static int listener_print(FAR const struct orb_metadata *meta, int fd) return ret; } +/**************************************************************************** + * Name: listener_print_info + * + * Description: + * Print sensor device information. + * + * Input Parameters: + * objlist topic object list. + * + * Returned Value: + * void + ****************************************************************************/ + +static void listener_print_info(FAR const struct listen_list_s *objlist) +{ + FAR struct listen_object_s *tmp; + orb_info_t info; + int ret; + int fd; + + SLIST_FOREACH(tmp, objlist, node) + { + fd = orb_subscribe_multi(tmp->object.meta, tmp->object.instance); + if (fd < 0) + { + continue; + } + + ret = orb_get_info(fd, &info); + orb_unsubscribe(fd); + uorbinfo_raw("Topic [%s%d] info:", tmp->object.meta->o_name, + tmp->object.instance); + if (ret < 0) + { + uorbinfo_raw("\t NULL"); + continue; + } + + uorbinfo_raw("\tname:%s" \ + "\n\tvendor:%s" \ + "\n\tversion:%" PRIu32 "" \ + "\n\tpower:%f" \ + "\n\tmax_range:%f" \ + "\n\tresolution:%f" \ + "\n\tmin_delay:%" PRId32 "" \ + "\n\tmax_delay:%" PRId32 "" \ + "\n\tfifo_reserved_event_count:%" PRIu32 ""\ + "\n\tfifo_max_event_count:%" PRIu32 "\n", + info.name, info.vendor, info.version, info.power, + info.max_range, info.resolution, info.min_delay, + info.max_delay, info.fifo_reserved_event_count, + info.fifo_max_event_count); + } +} + /**************************************************************************** * Name: listener_record * @@ -854,6 +910,7 @@ int main(int argc, FAR char *argv[]) int nb_msgs = 0; int timeout = 5; bool top = false; + bool info = false; bool record = false; bool only_once = false; FAR char *filter = NULL; @@ -868,7 +925,7 @@ int main(int argc, FAR char *argv[]) /* Pasrse Argument */ - while ((ch = getopt(argc, argv, "r:b:n:t:Tflh")) != EOF) + while ((ch = getopt(argc, argv, "r:b:n:t:Tflhi")) != EOF) { switch (ch) { @@ -918,6 +975,10 @@ int main(int argc, FAR char *argv[]) only_once = true; break; + case 'i': + info = true; + break; + case 'h': default: goto error; @@ -938,6 +999,12 @@ int main(int argc, FAR char *argv[]) return 0; } + if (info) + { + listener_print_info(&objlist); + goto exit; + } + if (top) { listener_top(&objlist, filter, only_once); @@ -956,6 +1023,7 @@ int main(int argc, FAR char *argv[]) nb_msgs, timeout, record); } +exit: listener_delete_object_list(&objlist); return 0; From 415ed6c4b66fe71975f604dcbb329386e9658538 Mon Sep 17 00:00:00 2001 From: likun17 Date: Thu, 19 Sep 2024 10:37:36 +0800 Subject: [PATCH 2/3] system/uorb: uorb_listener adds new features. uorb/listener.c: Added flush function. aurora:/ # uorb_listener -f sensor_accel_uncal,sensor_mag_uncal,sensor_aaa Waited for 5 seconds without flush complete event. Giving up. err:25 Result: Topic [sensor_mag_uncal0] flush: FAILURE. [-1] Topic [sensor_accel_uncal0] flush: SUCCESS. Total number of flush topics: 1/2 Signed-off-by: likun17 --- system/uorb/listener.c | 151 +++++++++++++++++++++++++++++++++++++++-- 1 file changed, 147 insertions(+), 4 deletions(-) diff --git a/system/uorb/listener.c b/system/uorb/listener.c index 5185b968e3f..0795dcb2f58 100644 --- a/system/uorb/listener.c +++ b/system/uorb/listener.c @@ -130,7 +130,7 @@ listener [arguments...]\n\ Commands:\n\ \t Topic name. Multi name are separated by ','\n\ \t[-h ] Listener commands help\n\ -\t[-f ] Record uorb data to file\n\ +\t[-s ] Record uorb data to file\n\ \t[-n ] Number of messages, default: 0\n\ \t[-r ] Subscription rate (unlimited if 0), default: 0\n\ \t[-b ] Subscription maximum report latency in us(unlimited if 0),\n\ @@ -139,6 +139,7 @@ listener [arguments...]\n\ \t[-T ] Top, continuously print updating objects\n\ \t[-l ] Top only execute once.\n\ \t[-i ] Get sensor device information based on topic.\n\ +\t[-f ] Flush sensor drive data.\n\ "); } @@ -531,6 +532,137 @@ static int listener_print(FAR const struct orb_metadata *meta, int fd) return ret; } +/**************************************************************************** + * Name: listener_flush_topic + * + * Description: + * Flush sensor device. + * + * Input Parameters: + * objlist Topic object list. + * nb_objects Length of objects list. + * timeout Maximum poll waiting time(microsecond). + * + * Returned Value: + * void + ****************************************************************************/ + +static void listener_flush_topic(FAR const struct listen_list_s *objlist, + int nb_objects, int timeout) +{ + FAR struct listen_object_s *tmp; + FAR struct pollfd *fds; + unsigned int events; + FAR int8_t *result; + int nb_flush = 0; + int i = 0; + int ret; + + fds = calloc(nb_objects, sizeof(struct pollfd)); + if (!fds) + { + return; + } + + result = calloc(nb_objects, sizeof(int8_t)); + if (!result) + { + free(fds); + return; + } + + /* Prepare pollfd for all flush objects */ + + SLIST_FOREACH(tmp, objlist, node) + { + int fd; + + fd = orb_subscribe_multi(tmp->object.meta, tmp->object.instance); + if (fd < 0) + { + fds[i].fd = -1; + fds[i].events = 0; + } + else + { + fds[i].fd = fd; + fds[i].events = POLLPRI; + i++; + } + } + + i = 0; + SLIST_FOREACH(tmp, objlist, node) + { + ret = orb_flush(fds[i].fd); + if (ret < 0) + { + result[i] = ret; + } + else + { + nb_flush++; + } + + i++; + } + + while (nb_flush && !g_should_exit) + { + if (poll(&fds[0], nb_objects, timeout * 1000) > 0) + { + i = 0; + SLIST_FOREACH(tmp, objlist, node) + { + if (fds[i].revents & POLLPRI) + { + ret = orb_get_events(fds[i].fd, &events); + if (ret < 0) + { + result[i] = ret; + } + else if (events & SENSOR_EVENT_FLUSH_COMPLETE) + { + nb_flush--; + } + } + + i++; + } + } + else if (errno != EINTR) + { + uorbinfo_raw("Waited for %d seconds without flush complete event. " + "Giving up. err:%d\n", timeout, errno); + break; + } + } + + i = 0; + uorbinfo_raw("Result:"); + SLIST_FOREACH(tmp, objlist, node) + { + if (result[i] == 0) + { + uorbinfo_raw("\tTopic [%s%d] flush: SUCCESS.", + tmp->object.meta->o_name, tmp->object.instance); + } + else + { + uorbinfo_raw("\tTopic [%s%d] flush: FAILURE. [%d]", + tmp->object.meta->o_name, tmp->object.instance, + result[i]); + } + + orb_unsubscribe(fds[i].fd); + i++; + } + + uorbinfo_raw("Total number of flush topics: %d", nb_objects); + free(fds); + free(result); +} + /**************************************************************************** * Name: listener_print_info * @@ -632,7 +764,7 @@ static int listener_record(FAR const struct orb_metadata *meta, int fd, * topic_rate Subscribe frequency. * topic_latency Subscribe report latency. * nb_msgs Subscribe amount of messages. - * timeout Maximum poll waiting time , ms. + * timeout Maximum poll waiting time(microseconds). * * Returned Value: * None @@ -911,6 +1043,7 @@ int main(int argc, FAR char *argv[]) int timeout = 5; bool top = false; bool info = false; + bool flush = false; bool record = false; bool only_once = false; FAR char *filter = NULL; @@ -925,7 +1058,7 @@ int main(int argc, FAR char *argv[]) /* Pasrse Argument */ - while ((ch = getopt(argc, argv, "r:b:n:t:Tflhi")) != EOF) + while ((ch = getopt(argc, argv, "r:b:n:t:Tfslhi")) != EOF) { switch (ch) { @@ -962,11 +1095,15 @@ int main(int argc, FAR char *argv[]) break; #ifdef CONFIG_DEBUG_UORB - case 'f': + case 's': record = true; break; #endif + case 'f': + flush = true; + break; + case 'T': top = true; break; @@ -999,6 +1136,12 @@ int main(int argc, FAR char *argv[]) return 0; } + if (flush) + { + listener_flush_topic(&objlist, ret, timeout); + goto exit; + } + if (info) { listener_print_info(&objlist); From 80dc7fb0f57d99a01b083491bf68cbeb075e0137 Mon Sep 17 00:00:00 2001 From: likun17 Date: Tue, 4 Mar 2025 17:16:49 +0800 Subject: [PATCH 3/3] system/uorb: uorb_listener flush functionality optimization. uorb/listener:Added flush failure prompt information. Signed-off-by: likun17 --- system/uorb/listener.c | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/system/uorb/listener.c b/system/uorb/listener.c index 0795dcb2f58..8cd63b098d0 100644 --- a/system/uorb/listener.c +++ b/system/uorb/listener.c @@ -559,13 +559,13 @@ static void listener_flush_topic(FAR const struct listen_list_s *objlist, int ret; fds = calloc(nb_objects, sizeof(struct pollfd)); - if (!fds) + if (fds == NULL) { return; } result = calloc(nb_objects, sizeof(int8_t)); - if (!result) + if (result == NULL) { free(fds); return; @@ -598,6 +598,8 @@ static void listener_flush_topic(FAR const struct listen_list_s *objlist, if (ret < 0) { result[i] = ret; + uorbinfo_raw("topic [%s%d] call flush failed! return:%d\n", + tmp->object.meta->o_name, tmp->object.instance, ret); } else {