From 459f0edbff0d54c3fefc8ed974686a5fe4ab46ff Mon Sep 17 00:00:00 2001 From: likun17 Date: Tue, 11 Feb 2025 22:07:23 +0800 Subject: [PATCH 1/5] system/uorb: add new features. Added nine-axis uncalibrated type topic. Signed-off-by: likun17 --- system/uorb/sensor/accel.c | 11 +++++++---- system/uorb/sensor/gyro.c | 6 +++++- system/uorb/sensor/mag.c | 6 +++++- 3 files changed, 17 insertions(+), 6 deletions(-) diff --git a/system/uorb/sensor/accel.c b/system/uorb/sensor/accel.c index f2c67a34d06..5c569fbd361 100644 --- a/system/uorb/sensor/accel.c +++ b/system/uorb/sensor/accel.c @@ -33,7 +33,9 @@ #ifdef CONFIG_DEBUG_UORB static const char sensor_accel_format[] = "timestamp:%" PRIu64 ",x:%hf,y:%hf,z:%hf,temperature:%hf"; - +static const char sensor_accel_uncal_format[] = + "timestamp:%" PRIu64 ",x:%hf,y:%hf,z:%hf,x_bias:%hf,y_bias:%hf," + "z_bias:%hf,temperature:%hf"; #endif /**************************************************************************** @@ -41,7 +43,8 @@ static const char sensor_accel_format[] = ****************************************************************************/ ORB_DEFINE(sensor_accel, struct sensor_accel, sensor_accel_format); -ORB_DEFINE(sensor_accel_uncal, struct sensor_accel, sensor_accel_format); +ORB_DEFINE(sensor_accel_uncal, struct sensor_accel_uncal, + sensor_accel_uncal_format); ORB_DEFINE(sensor_linear_accel, struct sensor_accel, sensor_accel_format); -ORB_DEFINE(sensor_linear_accel_uncal, struct sensor_accel, - sensor_accel_format); +ORB_DEFINE(sensor_linear_accel_uncal, struct sensor_accel_uncal, + sensor_accel_uncal_format); diff --git a/system/uorb/sensor/gyro.c b/system/uorb/sensor/gyro.c index d1988f56d16..18aa26a2161 100644 --- a/system/uorb/sensor/gyro.c +++ b/system/uorb/sensor/gyro.c @@ -33,6 +33,9 @@ #ifdef CONFIG_DEBUG_UORB static const char sensor_gyro_format[] = "timestamp:%" PRIu64 ",x:%hf,y:%hf,z:%hf,temperature:%hf"; +static const char sensor_gyro_uncal_format[] = + "timestamp:%" PRIu64 ",x:%hf,y:%hf,z:%hf,x_bias:%hf,y_bias:%hf," + "z_bias:%hf,temperature:%hf"; #endif /**************************************************************************** @@ -40,4 +43,5 @@ static const char sensor_gyro_format[] = ****************************************************************************/ ORB_DEFINE(sensor_gyro, struct sensor_gyro, sensor_gyro_format); -ORB_DEFINE(sensor_gyro_uncal, struct sensor_gyro, sensor_gyro_format); +ORB_DEFINE(sensor_gyro_uncal, struct sensor_gyro_uncal, + sensor_gyro_uncal_format); diff --git a/system/uorb/sensor/mag.c b/system/uorb/sensor/mag.c index 97077975b55..49b7b2992a2 100644 --- a/system/uorb/sensor/mag.c +++ b/system/uorb/sensor/mag.c @@ -34,6 +34,9 @@ static const char sensor_mag_format[] = "timestamp:%" PRIu64 ",x:%hf,y:%hf,z:%hf,temperature:%hf," "status:%" PRId32 ""; +static const char sensor_mag_uncal_format[] = + "timestamp:%" PRIu64 ",x:%hf,y:%hf,z:%hf,x_bias:%hf,y_bias:%hf," + "z_bias:%hf,temperature:%hf,status:%" PRId32 ""; #endif /**************************************************************************** @@ -41,4 +44,5 @@ static const char sensor_mag_format[] = ****************************************************************************/ ORB_DEFINE(sensor_mag, struct sensor_mag, sensor_mag_format); -ORB_DEFINE(sensor_mag_uncal, struct sensor_mag, sensor_mag_format); +ORB_DEFINE(sensor_mag_uncal, struct sensor_mag_uncal, + sensor_mag_uncal_format); From 94b1b4ab02bcbb0ceba7b5242c95e451a9390281 Mon Sep 17 00:00:00 2001 From: dongjiuzhu1 Date: Fri, 9 May 2025 12:17:06 +0800 Subject: [PATCH 2/5] system/uorb: Modify the default subscription mode to "non-wakeup" to align with Android's approach And add the corresponding "wakeup" API: orb_subscirbe_wakeup/orb_subscribe_multi_wakeup Signed-off-by: dongjiuzhu1 --- system/uorb/uORB/uORB.c | 10 ++++----- system/uorb/uORB/uORB.h | 48 +++++++++++++++++++++++++++++++++++++---- 2 files changed, 49 insertions(+), 9 deletions(-) diff --git a/system/uorb/uORB/uORB.c b/system/uorb/uORB/uORB.c index 6fcc060c20a..8e98ab81a25 100644 --- a/system/uorb/uORB/uORB.c +++ b/system/uorb/uORB/uORB.c @@ -124,7 +124,7 @@ static int orb_advsub_open(FAR const struct orb_metadata *meta, int flags, ioctl(fd, SNIOC_SET_BUFFER_NUMBER, (unsigned long)queue_size); } - if (non_wakeup) + if (!non_wakeup) { ioctl(fd, SNIOC_SET_NONWAKEUP, (unsigned long)non_wakeup); } @@ -145,7 +145,7 @@ orb_advertise_multi_queue_flags(FAR const struct orb_metadata *meta, inst = instance ? *instance : orb_group_count(meta); - fd = orb_advsub_open(meta, flags, inst, queue_size, info, false); + fd = orb_advsub_open(meta, flags, inst, queue_size, info, true); if (fd < 0) { uorberr("%s advertise failed (%i)", meta->o_name, fd); @@ -216,13 +216,13 @@ ssize_t orb_publish_multi(int fd, FAR const void *data, size_t len) int orb_subscribe_multi(FAR const struct orb_metadata *meta, unsigned instance) { - return orb_advsub_open(meta, O_RDONLY, instance, 0, NULL, false); + return orb_advsub_open(meta, O_RDONLY, instance, 0, NULL, true); } -int orb_subscribe_multi_nonwakeup(FAR const struct orb_metadata *meta, +int orb_subscribe_multi_wakeup(FAR const struct orb_metadata *meta, unsigned instance) { - return orb_advsub_open(meta, O_RDONLY, instance, 0, NULL, true); + return orb_advsub_open(meta, O_RDONLY, instance, 0, NULL, false); } ssize_t orb_copy_multi(int fd, FAR void *buffer, size_t len) diff --git a/system/uorb/uORB/uORB.h b/system/uorb/uORB/uORB.h index e30c52fdd62..714249a4d3a 100644 --- a/system/uorb/uORB/uORB.h +++ b/system/uorb/uORB/uORB.h @@ -449,7 +449,7 @@ static inline int orb_publish_auto(FAR const struct orb_metadata *meta, * Name: orb_subscribe_multi * * Description: - * Subscribe to a topic. + * Subscribe to a topic in a non-wakeup ways. * * The data is published to the topic and any waiting subscribers will be * notified. Subscribers that are not waiting can check the topic for @@ -490,7 +490,7 @@ static inline int orb_subscribe(FAR const struct orb_metadata *meta) * Description: * Subscribe to a topic in a non-wakeup ways. * - * The usage of orb_subscribe_multi_nonwakeup is similar to that of + * The usage of orb_subscribe_multi_nonwakeup is same to that of * orb_subscribe_multi, with the key difference lying in whether the * system's wakeup status needs to be concerned. This distinction is * particularly beneficial for low-power consumption scenarios. @@ -513,13 +513,53 @@ static inline int orb_subscribe(FAR const struct orb_metadata *meta) * this function will return -1 and set errno to ENOENT. ****************************************************************************/ +static inline int orb_subscribe_multi_nonwakeup(FAR const struct orb_metadata *meta, - unsigned instance); + unsigned instance) +{ + return orb_subscribe_multi(meta, instance); +} static inline int orb_subscribe_nonwakeup(FAR const struct orb_metadata *meta) { - return orb_subscribe_multi_nonwakeup(meta, 0); + return orb_subscribe(meta); +} + +/**************************************************************************** + * Name: orb_subscribe_multi_wakeup/orb_subscribe_wakeup + * + * Description: + * Subscribe to a topic in a wakeup ways. + * + * The usage of orb_subscribe_multi_wakeup is similar to that of + * orb_subscribe_multi, with the key difference lying in whether the + * system's wakeup status needs to be concerned. This distinction is + * particularly beneficial for low-power consumption scenarios. + * If the subscription is in a wakeup mode, the subscriber will be wokeup + * and it receives data while the system is in sleep mode. Typically, the + * subscriber and publisher of a topic reside in two separate systems. + * + * Input Parameters: + * meta The uORB metadata (usually from the ORB_ID() macro) + * instance The instance of the topic. Instance 0 matches the topic of + * the orb_subscribe() call. + * + * Returned Value: + * -1 on error, otherwise returns a fd + * that can be used to read and update the topic. + * If the topic in question is not known (due to an + * ORB_DEFINE_OPTIONAL with no corresponding ORB_DECLARE) + * this function will return -1 and set errno to ENOENT. + ****************************************************************************/ + +int orb_subscribe_multi_wakeup(FAR const struct orb_metadata *meta, + unsigned instance); + +static inline +int orb_subscribe_wakeup(FAR const struct orb_metadata *meta) +{ + return orb_subscribe_multi_wakeup(meta, 0); } /**************************************************************************** From 31e64f9d6a10bac96de19535dcb859e9bcda9070 Mon Sep 17 00:00:00 2001 From: dongjiuzhu1 Date: Fri, 9 May 2025 20:57:55 +0800 Subject: [PATCH 3/5] system/uorb: remove nonwakeup api and using wakeup api. remove API orb_subscribe_multi_nonwakeup add API orb_subscribe_multi_wakeup Signed-off-by: dongjiuzhu1 --- system/uorb/listener.c | 6 +++--- system/uorb/uORB/uORB.h | 42 ----------------------------------------- 2 files changed, 3 insertions(+), 45 deletions(-) diff --git a/system/uorb/listener.c b/system/uorb/listener.c index 4a60b518573..42153281caa 100644 --- a/system/uorb/listener.c +++ b/system/uorb/listener.c @@ -202,12 +202,12 @@ static int listener_subscribe(FAR struct listen_object_s *tmp, { if (nonwakeup) { - return orb_subscribe_multi_nonwakeup(tmp->object.meta, - tmp->object.instance); + return orb_subscribe_multi(tmp->object.meta, tmp->object.instance); } else { - return orb_subscribe_multi(tmp->object.meta, tmp->object.instance); + return orb_subscribe_multi_wakeup(tmp->object.meta, + tmp->object.instance); } } diff --git a/system/uorb/uORB/uORB.h b/system/uorb/uORB/uORB.h index 714249a4d3a..67736fd590e 100644 --- a/system/uorb/uORB/uORB.h +++ b/system/uorb/uORB/uORB.h @@ -484,48 +484,6 @@ static inline int orb_subscribe(FAR const struct orb_metadata *meta) return orb_subscribe_multi(meta, 0); } -/**************************************************************************** - * Name: orb_subscribe_multi_nonwakeup/orb_subscribe_nonwakeup - * - * Description: - * Subscribe to a topic in a non-wakeup ways. - * - * The usage of orb_subscribe_multi_nonwakeup is same to that of - * orb_subscribe_multi, with the key difference lying in whether the - * system's wakeup status needs to be concerned. This distinction is - * particularly beneficial for low-power consumption scenarios. - * If the subscription is in a non-wakeup mode, the subscriber will not - * receive data while the system is in sleep mode. In such cases, new data - * will overwrite old data until the system is awakened, at which point - * the subscriber will be notified. Typically, the subscriber and - * publisher of a topic reside in two separate systems. - * - * Input Parameters: - * meta The uORB metadata (usually from the ORB_ID() macro) - * instance The instance of the topic. Instance 0 matches the topic of - * the orb_subscribe() call. - * - * Returned Value: - * -1 on error, otherwise returns a fd - * that can be used to read and update the topic. - * If the topic in question is not known (due to an - * ORB_DEFINE_OPTIONAL with no corresponding ORB_DECLARE) - * this function will return -1 and set errno to ENOENT. - ****************************************************************************/ - -static inline -int orb_subscribe_multi_nonwakeup(FAR const struct orb_metadata *meta, - unsigned instance) -{ - return orb_subscribe_multi(meta, instance); -} - -static inline -int orb_subscribe_nonwakeup(FAR const struct orb_metadata *meta) -{ - return orb_subscribe(meta); -} - /**************************************************************************** * Name: orb_subscribe_multi_wakeup/orb_subscribe_wakeup * From 15e27b325d3ba17e17fc2f1340e1e0ed3eca0680 Mon Sep 17 00:00:00 2001 From: dongjiuzhu1 Date: Tue, 13 May 2025 15:59:10 +0800 Subject: [PATCH 4/5] system/uorb: change nonwakeup to wakeup. As nonwakeup is default mode Signed-off-by: dongjiuzhu1 Signed-off-by: likun17 --- system/uorb/listener.c | 48 ++++++++++++++++++++--------------------- system/uorb/uORB/uORB.c | 14 ++++++------ 2 files changed, 30 insertions(+), 32 deletions(-) diff --git a/system/uorb/listener.c b/system/uorb/listener.c index 42153281caa..27ae57da52b 100644 --- a/system/uorb/listener.c +++ b/system/uorb/listener.c @@ -84,7 +84,7 @@ static int listener_print(FAR const struct orb_metadata *meta, int fd); static void listener_monitor(FAR struct listen_list_s *objlist, int nb_objects, float topic_rate, int topic_latency, int nb_msgs, - int timeout, bool record, bool nonwakeup); + int timeout, bool record, bool wakeup); static int listener_update(FAR struct listen_list_s *objlist, FAR struct orb_object *object); static void listener_top(FAR struct listen_list_s *objlist, @@ -140,7 +140,7 @@ listener [arguments...]\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\ -\t[-u ] Subscribe in non-wakeup mode to save power.\n\ +\t[-w ] Subscribe in wakeup mode.\n\ "); } @@ -187,28 +187,26 @@ static int listener_create_dir(FAR char *dir, size_t size) * Name: listener_subscribe * * Description: - * Subscribe topic according to non wakeup mode. + * Subscribe topic according to wakeup mode. * * Input Parameters: * tmp Given object - * nonwakeup State of nonwakeup. + * wakeup State of wakeup. * * Returned Value: * fd on success, otherwise -1. ****************************************************************************/ static int listener_subscribe(FAR struct listen_object_s *tmp, - bool nonwakeup) + bool wakeup) { - if (nonwakeup) - { - return orb_subscribe_multi(tmp->object.meta, tmp->object.instance); - } - else + if (wakeup) { return orb_subscribe_multi_wakeup(tmp->object.meta, tmp->object.instance); } + + return orb_subscribe_multi(tmp->object.meta, tmp->object.instance); } /**************************************************************************** @@ -571,14 +569,14 @@ static int listener_print(FAR const struct orb_metadata *meta, int fd) * objlist Topic object list. * nb_objects Length of objects list. * timeout Maximum poll waiting time(microsecond). - * nonwakeup The state of non wakeup + * wakeup The state of wakeup * * Returned Value: * void ****************************************************************************/ static void listener_flush_topic(FAR const struct listen_list_s *objlist, - int nb_objects, int timeout, bool nonwakeup) + int nb_objects, int timeout, bool wakeup) { FAR struct listen_object_s *tmp; FAR struct pollfd *fds; @@ -607,7 +605,7 @@ static void listener_flush_topic(FAR const struct listen_list_s *objlist, { int fd; - fd = listener_subscribe(tmp, nonwakeup); + fd = listener_subscribe(tmp, wakeup); if (fd < 0) { fds[i].fd = -1; @@ -703,14 +701,14 @@ static void listener_flush_topic(FAR const struct listen_list_s *objlist, * * Input Parameters: * objlist topic object list. - * nonwakeup The state of non wakeup + * wakeup The state of wakeup * * Returned Value: * void ****************************************************************************/ static void listener_print_info(FAR const struct listen_list_s *objlist, - bool nonwakeup) + bool wakeup) { FAR struct listen_object_s *tmp; orb_info_t info; @@ -719,7 +717,7 @@ static void listener_print_info(FAR const struct listen_list_s *objlist, SLIST_FOREACH(tmp, objlist, node) { - fd = listener_subscribe(tmp, nonwakeup); + fd = listener_subscribe(tmp, wakeup); if (fd < 0) { continue; @@ -807,7 +805,7 @@ static int listener_record(FAR const struct orb_metadata *meta, int fd, static void listener_monitor(FAR struct listen_list_s *objlist, int nb_objects, float topic_rate, int topic_latency, int nb_msgs, - int timeout, bool record, bool nonwakeup) + int timeout, bool record, bool wakeup) { FAR struct pollfd *fds; char path[PATH_MAX]; @@ -838,7 +836,7 @@ static void listener_monitor(FAR struct listen_list_s *objlist, { int fd; - fd = listener_subscribe(tmp, nonwakeup); + fd = listener_subscribe(tmp, wakeup); if (fd < 0) { fds[i].fd = -1; @@ -1079,7 +1077,7 @@ int main(int argc, FAR char *argv[]) bool info = false; bool flush = false; bool record = false; - bool nonwakeup = false; + bool wakeup = false; bool only_once = false; FAR char *filter = NULL; int ret; @@ -1093,7 +1091,7 @@ int main(int argc, FAR char *argv[]) /* Pasrse Argument */ - while ((ch = getopt(argc, argv, "r:b:n:t:Tfslhiu")) != EOF) + while ((ch = getopt(argc, argv, "r:b:n:t:Tfslhiw")) != EOF) { switch (ch) { @@ -1151,8 +1149,8 @@ int main(int argc, FAR char *argv[]) info = true; break; - case 'u': - nonwakeup = true; + case 'w': + wakeup = true; break; case 'h': @@ -1177,13 +1175,13 @@ int main(int argc, FAR char *argv[]) if (flush) { - listener_flush_topic(&objlist, ret, timeout, nonwakeup); + listener_flush_topic(&objlist, ret, timeout, wakeup); goto exit; } if (info) { - listener_print_info(&objlist, nonwakeup); + listener_print_info(&objlist, wakeup); goto exit; } @@ -1202,7 +1200,7 @@ int main(int argc, FAR char *argv[]) } listener_monitor(&objlist, ret, topic_rate, topic_latency, - nb_msgs, timeout, record, nonwakeup); + nb_msgs, timeout, record, wakeup); } exit: diff --git a/system/uorb/uORB/uORB.c b/system/uorb/uORB/uORB.c index 8e98ab81a25..a9dfe7b7939 100644 --- a/system/uorb/uORB/uORB.c +++ b/system/uorb/uORB/uORB.c @@ -51,7 +51,7 @@ * flag The open flag. * instance Instance number to open. * queue_size Maximum number of buffered elements. - * non_wakeup The non wakeup flag. + * wakeup The wakeup flag. * * Returned Value: * fd on success, otherwise returns negative value and set errno. @@ -59,7 +59,7 @@ static int orb_advsub_open(FAR const struct orb_metadata *meta, int flags, int instance, unsigned int queue_size, - FAR orb_info_t *info, bool non_wakeup) + FAR orb_info_t *info, bool wakeup) { char path[ORB_PATH_MAX]; int fd; @@ -124,9 +124,9 @@ static int orb_advsub_open(FAR const struct orb_metadata *meta, int flags, ioctl(fd, SNIOC_SET_BUFFER_NUMBER, (unsigned long)queue_size); } - if (!non_wakeup) + if (wakeup) { - ioctl(fd, SNIOC_SET_NONWAKEUP, (unsigned long)non_wakeup); + ioctl(fd, SNIOC_SET_WAKEUP, (unsigned long)wakeup); } return fd; @@ -145,7 +145,7 @@ orb_advertise_multi_queue_flags(FAR const struct orb_metadata *meta, inst = instance ? *instance : orb_group_count(meta); - fd = orb_advsub_open(meta, flags, inst, queue_size, info, true); + fd = orb_advsub_open(meta, flags, inst, queue_size, info, false); if (fd < 0) { uorberr("%s advertise failed (%i)", meta->o_name, fd); @@ -216,13 +216,13 @@ ssize_t orb_publish_multi(int fd, FAR const void *data, size_t len) int orb_subscribe_multi(FAR const struct orb_metadata *meta, unsigned instance) { - return orb_advsub_open(meta, O_RDONLY, instance, 0, NULL, true); + return orb_advsub_open(meta, O_RDONLY, instance, 0, NULL, false); } int orb_subscribe_multi_wakeup(FAR const struct orb_metadata *meta, unsigned instance) { - return orb_advsub_open(meta, O_RDONLY, instance, 0, NULL, false); + return orb_advsub_open(meta, O_RDONLY, instance, 0, NULL, true); } ssize_t orb_copy_multi(int fd, FAR void *buffer, size_t len) From 673237abb31d3e25321beff4c33bfcf60fc75485 Mon Sep 17 00:00:00 2001 From: dongjiuzhu1 Date: Wed, 28 May 2025 17:48:40 +0800 Subject: [PATCH 5/5] system/uorb: remove latency test form unit_test we need to run latency test by command: unit_test latency_test, must add parameters: latency_test. Signed-off-by: dongjiuzhu1 --- system/uorb/test/unit_test.c | 9 ++------- 1 file changed, 2 insertions(+), 7 deletions(-) diff --git a/system/uorb/test/unit_test.c b/system/uorb/test/unit_test.c index 70cd059dfe3..4150cd4d006 100644 --- a/system/uorb/test/unit_test.c +++ b/system/uorb/test/unit_test.c @@ -544,11 +544,6 @@ static int test_multi(int *afds, int *sfds) return test_fail("sub #1 val. mismatch: %d", sub_sample.val); } - if (OK != latency_test(false)) - { - return test_fail("latency test failed"); - } - orb_unsubscribe(sfds[0]); orb_unsubscribe(sfds[1]); @@ -1162,12 +1157,12 @@ int main(int argc, FAR char *argv[]) { if (test() == OK) { - printf("PASS\n"); + printf("TEST PASS\n"); return 0; } else { - printf("FAIL\n"); + printf("TEST FAIL\n"); return -1; } }