From 9585055e9e87ef565efd19fa429e175423ac81e3 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Wed, 11 Mar 2020 09:06:33 -0400 Subject: [PATCH] uORB: add bitset for faster orb_exists check and remove uORB::Subscription lazy subscribe hack/optimization - add PX4 bitset and atomic_bitset with testing - add uORB::Subscription constructor to take ORB_ID enum - move orb test messages into msg/ --- msg/CMakeLists.txt | 9 +- msg/orb_test.msg | 5 + msg/orb_test_large.msg | 5 + msg/orb_test_medium.msg | 7 + msg/templates/uorb/msg.cpp.em | 5 +- msg/templates/uorb/uORBTopics.cpp.em | 22 ++- msg/templates/uorb/uORBTopics.hpp.em | 76 ++++++++ msg/templates/uorb_microcdr/uORBTopics.hpp.em | 49 +++++ msg/tools/px_generate_uorb_topic_files.py | 26 ++- msg/tools/uorb_rtps_message_ids.yaml | 6 + .../include/px4_platform_common/atomic.h | 6 +- .../px4_platform_common/atomic_bitset.h | 95 ++++++++++ platforms/posix/cmake/sitl_tests.cmake | 2 + src/drivers/power_monitor/ina226/ina226.h | 2 +- src/include/containers/Bitset.hpp | 88 +++++++++ src/modules/commander/Commander.cpp | 12 +- src/modules/logger/logged_topics.cpp | 6 +- src/modules/logger/logged_topics.h | 3 +- src/modules/logger/logger.cpp | 4 +- src/modules/logger/logger.h | 4 +- src/modules/replay/Replay.hpp | 2 +- src/modules/uORB/CMakeLists.txt | 1 - src/modules/uORB/Subscription.cpp | 74 +++----- src/modules/uORB/Subscription.hpp | 49 +++-- src/modules/uORB/SubscriptionInterval.hpp | 12 ++ src/modules/uORB/uORB.h | 7 +- src/modules/uORB/uORBDeviceMaster.cpp | 14 ++ src/modules/uORB/uORBDeviceMaster.hpp | 8 + src/modules/uORB/uORBDeviceNode.hpp | 2 + .../uORB/uORB_tests/uORBTest_UnitTest.cpp | 70 +++---- .../uORB/uORB_tests/uORBTest_UnitTest.hpp | 46 ++--- .../uORB/uORB_tests/uORB_tests_main.cpp | 9 +- src/systemcmds/tests/CMakeLists.txt | 2 + src/systemcmds/tests/test_atomic_bitset.cpp | 175 ++++++++++++++++++ src/systemcmds/tests/test_bitset.cpp | 175 ++++++++++++++++++ src/systemcmds/tests/test_microbench_uorb.cpp | 36 +++- src/systemcmds/tests/tests_main.c | 3 +- src/systemcmds/tests/tests_main.h | 2 + 38 files changed, 919 insertions(+), 200 deletions(-) create mode 100644 msg/orb_test.msg create mode 100644 msg/orb_test_large.msg create mode 100644 msg/orb_test_medium.msg create mode 100644 msg/templates/uorb/uORBTopics.hpp.em create mode 100644 msg/templates/uorb_microcdr/uORBTopics.hpp.em create mode 100644 platforms/common/include/px4_platform_common/atomic_bitset.h create mode 100644 src/include/containers/Bitset.hpp create mode 100644 src/systemcmds/tests/test_atomic_bitset.cpp create mode 100644 src/systemcmds/tests/test_bitset.cpp diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index 2a6ee45ccf..92d3c1aac8 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -88,6 +88,9 @@ set(msg_files offboard_control_mode.msg onboard_computer_status.msg optical_flow.msg + orb_test.msg + orb_test_large.msg + orb_test_medium.msg orbit_status.msg parameter_update.msg ping.msg @@ -98,13 +101,13 @@ set(msg_files power_button_state.msg power_monitor.msg pwm_input.msg - rpm.msg qshell_req.msg qshell_retval.msg radio_status.msg rate_ctrl_status.msg rc_channels.msg rc_parameter_map.msg + rpm.msg safety.msg satellite_info.msg sensor_accel.msg @@ -206,7 +209,7 @@ endif() set(msg_out_path ${PX4_BINARY_DIR}/uORB/topics) set(msg_source_out_path ${CMAKE_CURRENT_BINARY_DIR}/topics_sources) -set(uorb_headers) +set(uorb_headers ${msg_out_path}/uORBTopics.hpp) set(uorb_sources ${msg_source_out_path}/uORBTopics.cpp) foreach(msg_file ${msg_files}) get_filename_component(msg ${msg_file} NAME_WE) @@ -232,6 +235,7 @@ add_custom_command(OUTPUT ${uorb_headers} DEPENDS ${msg_files} templates/uorb/msg.h.em + templates/uorb/uORBTopics.hpp.em tools/px_generate_uorb_topic_files.py COMMENT "Generating uORB topic headers" WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR} @@ -253,6 +257,7 @@ add_custom_command(OUTPUT ${uorb_sources} DEPENDS ${msg_files} templates/uorb/msg.cpp.em + templates/uorb/uORBTopics.cpp.em tools/px_generate_uorb_topic_files.py COMMENT "Generating uORB topic sources" WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR} diff --git a/msg/orb_test.msg b/msg/orb_test.msg new file mode 100644 index 0000000000..bbbca412fd --- /dev/null +++ b/msg/orb_test.msg @@ -0,0 +1,5 @@ +uint64 timestamp # time since system start (microseconds) + +int32 val + +# TOPICS orb_test orb_multitest diff --git a/msg/orb_test_large.msg b/msg/orb_test_large.msg new file mode 100644 index 0000000000..48d6a427ee --- /dev/null +++ b/msg/orb_test_large.msg @@ -0,0 +1,5 @@ +uint64 timestamp # time since system start (microseconds) + +int32 val + +uint8[512] junk diff --git a/msg/orb_test_medium.msg b/msg/orb_test_medium.msg new file mode 100644 index 0000000000..57334b213f --- /dev/null +++ b/msg/orb_test_medium.msg @@ -0,0 +1,7 @@ +uint64 timestamp # time since system start (microseconds) + +int32 val + +uint8[64] junk + +# TOPICS orb_test_medium orb_test_medium_multi orb_test_medium_queue orb_test_medium_queue_poll diff --git a/msg/templates/uorb/msg.cpp.em b/msg/templates/uorb/msg.cpp.em index aed55170f8..15fb4a75a8 100644 --- a/msg/templates/uorb/msg.cpp.em +++ b/msg/templates/uorb/msg.cpp.em @@ -70,6 +70,7 @@ topic_fields = ["%s %s" % (convert_type(field.type), field.name) for field in so #include #include #include +#include #include #include @@ -78,10 +79,10 @@ topic_fields = ["%s %s" % (convert_type(field.type), field.name) for field in so constexpr char __orb_@(topic_name)_fields[] = "@( ";".join(topic_fields) );"; @[for multi_topic in topics]@ -ORB_DEFINE(@multi_topic, struct @uorb_struct, @(struct_size-padding_end_size), __orb_@(topic_name)_fields); +ORB_DEFINE(@multi_topic, struct @uorb_struct, @(struct_size-padding_end_size), __orb_@(topic_name)_fields, static_cast(ORB_ID::@multi_topic)); @[end for] -void print_message(const @uorb_struct& message) +void print_message(const @uorb_struct &message) { @[if constrained_flash] (void)message; diff --git a/msg/templates/uorb/uORBTopics.cpp.em b/msg/templates/uorb/uORBTopics.cpp.em index 5e0ca94417..bce579b1a4 100644 --- a/msg/templates/uorb/uORBTopics.cpp.em +++ b/msg/templates/uorb/uORBTopics.cpp.em @@ -13,7 +13,7 @@ @############################################### /**************************************************************************** * - * Copyright (C) 2013-2015 PX4 Development Team. All rights reserved. + * Copyright (C) 2013-2020 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -44,31 +44,35 @@ * ****************************************************************************/ -#include +#include #include @{ msg_names = [mn.replace(".msg", "") for mn in msgs] msgs_count = len(msg_names) msg_names_all = list(set(msg_names + multi_topics)) # set() filters duplicates +msg_names_all.sort() msgs_count_all = len(msg_names_all) }@ @[for msg_name in msg_names]@ #include @[end for] -const size_t _uorb_topics_count = @(msgs_count_all); -const constexpr struct orb_metadata* const _uorb_topics_list[_uorb_topics_count] = { +const constexpr struct orb_metadata *const uorb_topics_list[ORB_TOPICS_COUNT] = { @[for idx, msg_name in enumerate(msg_names_all, 1)]@ - ORB_ID(@(msg_name))@[if idx != msgs_count_all],@[end if] + ORB_ID(@(msg_name))@[if idx != msgs_count_all], @[end if] @[end for] }; -size_t orb_topics_count() +const struct orb_metadata *const *orb_get_topics() { - return _uorb_topics_count; + return uorb_topics_list; } -const struct orb_metadata *const*orb_get_topics() +const struct orb_metadata *get_orb_meta(ORB_ID id) { - return _uorb_topics_list; + if (id == ORB_ID::INVALID) { + return nullptr; + } + + return uorb_topics_list[static_cast(id)]; } diff --git a/msg/templates/uorb/uORBTopics.hpp.em b/msg/templates/uorb/uORBTopics.hpp.em new file mode 100644 index 0000000000..d1d33f4816 --- /dev/null +++ b/msg/templates/uorb/uORBTopics.hpp.em @@ -0,0 +1,76 @@ +@############################################### +@# +@# EmPy template for generating uORBTopics.hpp file +@# for logging purposes +@# +@############################################### +@# Start of Template +@# +@# Context: +@# - msgs (List) list of all msg files +@# - multi_topics (List) list of all multi-topic names +@# - ids (List) list of all RTPS msg ids +@############################################### +/**************************************************************************** + * + * Copyright (C) 2020 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +@{ +msg_names = [mn.replace(".msg", "") for mn in msgs] +msgs_count = len(msg_names) +msg_names_all = list(set(msg_names + multi_topics)) # set() filters duplicates +msg_names_all.sort() +msgs_count_all = len(msg_names_all) +}@ + +#pragma once + +#include + +#include + +static constexpr size_t ORB_TOPICS_COUNT{@(msgs_count_all)}; +static constexpr size_t orb_topics_count() { return ORB_TOPICS_COUNT; } + +/* + * Returns array of topics metadata + */ +extern const struct orb_metadata *const *orb_get_topics() __EXPORT; + +enum class ORB_ID : uint8_t { +@[for idx, msg_name in enumerate(msg_names_all)]@ + @(msg_name) = @(idx), +@[end for] + INVALID +}; + +const struct orb_metadata *get_orb_meta(ORB_ID id); diff --git a/msg/templates/uorb_microcdr/uORBTopics.hpp.em b/msg/templates/uorb_microcdr/uORBTopics.hpp.em new file mode 100644 index 0000000000..5b1954d17a --- /dev/null +++ b/msg/templates/uorb_microcdr/uORBTopics.hpp.em @@ -0,0 +1,49 @@ +@############################################### +@# +@# EmPy template for generating uORBTopics.cpp file +@# for logging purposes +@# +@############################################### +@# Start of Template +@# +@# Context: +@# - msgs (List) list of all msg files +@# - multi_topics (List) list of all multi-topic names +@# - ids (List) list of all RTPS msg ids +@############################################### +/**************************************************************************** + * + * Copyright (C) 2020 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +#include diff --git a/msg/tools/px_generate_uorb_topic_files.py b/msg/tools/px_generate_uorb_topic_files.py index af5e1bceb4..765b7f8b91 100755 --- a/msg/tools/px_generate_uorb_topic_files.py +++ b/msg/tools/px_generate_uorb_topic_files.py @@ -72,7 +72,7 @@ __email__ = "thomasgubler@gmail.com" TEMPLATE_FILE = ['msg.h.em', 'msg.cpp.em'] -TOPICS_LIST_TEMPLATE_FILE = 'uORBTopics.cpp.em' +TOPICS_LIST_TEMPLATE_FILE = ['uORBTopics.hpp.em', 'uORBTopics.cpp.em'] OUTPUT_FILE_EXT = ['.h', '.cpp'] INCL_DEFAULT = ['std_msgs:./msg/std_msgs'] PACKAGE = 'px4' @@ -434,12 +434,12 @@ def convert_dir_save(format_idx, inputdir, outputdir, package, templatedir, temp # Create new headers in temporary output directory convert_dir(format_idx, inputdir, temporarydir, package, templatedir) if generate_idx == 1: - generate_topics_list_file(inputdir, temporarydir, templatedir) + generate_topics_list_file(inputdir, temporarydir, TOPICS_LIST_TEMPLATE_FILE[1], templatedir) # Copy changed headers from temporary dir to output dir copy_changed(temporarydir, outputdir, prefix, quiet) -def generate_topics_list_file(msgdir, outputdir, templatedir): +def generate_topics_list_file(msgdir, outputdir, template_filename, templatedir): # generate cpp file with topics list msgs = get_msgs_list(msgdir) multi_topics = [] @@ -447,13 +447,12 @@ def generate_topics_list_file(msgdir, outputdir, templatedir): msg_filename = os.path.join(msgdir, msg) multi_topics.extend(get_multi_topics(msg_filename)) tl_globals = {"msgs": msgs, "multi_topics": multi_topics} - tl_template_file = os.path.join(templatedir, TOPICS_LIST_TEMPLATE_FILE) - tl_out_file = os.path.join( - outputdir, TOPICS_LIST_TEMPLATE_FILE.replace(".em", "")) + tl_template_file = os.path.join(templatedir, template_filename) + tl_out_file = os.path.join(outputdir, template_filename.replace(".em", "")) generate_by_template(tl_out_file, tl_template_file, tl_globals) -def generate_topics_list_file_from_files(files, outputdir, templatedir): +def generate_topics_list_file_from_files(files, outputdir, template_filename, templatedir): # generate cpp file with topics list filenames = [os.path.basename( p) for p in files if os.path.basename(p).endswith(".msg")] @@ -461,9 +460,8 @@ def generate_topics_list_file_from_files(files, outputdir, templatedir): for msg_filename in files: multi_topics.extend(get_multi_topics(msg_filename)) tl_globals = {"msgs": filenames, "multi_topics": multi_topics} - tl_template_file = os.path.join(templatedir, TOPICS_LIST_TEMPLATE_FILE) - tl_out_file = os.path.join( - outputdir, TOPICS_LIST_TEMPLATE_FILE.replace(".em", "")) + tl_template_file = os.path.join(templatedir, template_filename) + tl_out_file = os.path.join(outputdir, template_filename.replace(".em", "")) generate_by_template(tl_out_file, tl_template_file, tl_globals) @@ -520,11 +518,9 @@ if __name__ == "__main__": for f in args.file: generate_output_from_file( generate_idx, f, args.temporarydir, args.package, args.templatedir, INCL_DEFAULT) - if generate_idx == 1: - generate_topics_list_file_from_files( - args.file, args.outputdir, args.templatedir) - copy_changed(args.temporarydir, args.outputdir, - args.prefix, args.quiet) + + generate_topics_list_file_from_files(args.file, args.outputdir, TOPICS_LIST_TEMPLATE_FILE[generate_idx], args.templatedir) + copy_changed(args.temporarydir, args.outputdir, args.prefix, args.quiet) elif args.dir is not None: convert_dir_save( generate_idx, diff --git a/msg/tools/uorb_rtps_message_ids.yaml b/msg/tools/uorb_rtps_message_ids.yaml index ea45f437cf..c3a08c4413 100644 --- a/msg/tools/uorb_rtps_message_ids.yaml +++ b/msg/tools/uorb_rtps_message_ids.yaml @@ -293,6 +293,12 @@ rtps: id: 130 - msg: timesync_status id: 131 + - msg: orb_test + id: 132 + - msg: orb_test_medium + id: 133 + - msg: orb_test_large + id: 134 ########## multi topics: begin ########## - msg: actuator_controls_0 id: 150 diff --git a/platforms/common/include/px4_platform_common/atomic.h b/platforms/common/include/px4_platform_common/atomic.h index 98c7710deb..f40bc00865 100644 --- a/platforms/common/include/px4_platform_common/atomic.h +++ b/platforms/common/include/px4_platform_common/atomic.h @@ -72,7 +72,7 @@ public: static_assert(__atomic_always_lock_free(sizeof(T), 0), "atomic is not lock-free for the given type T"); #endif - + atomic() = default; explicit atomic(T value) : _value(value) {} /** @@ -175,9 +175,9 @@ private: #ifdef __PX4_QURT // It seems that __atomic_store and __atomic_load are not supported on Qurt, // so the best that we can do is to use volatile. - volatile T _value; + volatile T _value{}; #else - T _value; + T _value {}; #endif }; diff --git a/platforms/common/include/px4_platform_common/atomic_bitset.h b/platforms/common/include/px4_platform_common/atomic_bitset.h new file mode 100644 index 0000000000..1cd97eef49 --- /dev/null +++ b/platforms/common/include/px4_platform_common/atomic_bitset.h @@ -0,0 +1,95 @@ +/**************************************************************************** + * + * Copyright (c) 2020 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +#include "atomic.h" + +namespace px4 +{ + +template +class AtomicBitset +{ +public: + + AtomicBitset() = default; + + size_t count() const + { + size_t total = 0; + + for (const auto &x : _data) { + uint32_t y = x.load(); + + while (y) { + total += y & 1; + y >>= 1; + } + } + + return total; + } + + size_t size() const { return N; } + + bool operator[](size_t position) const + { + return _data[array_index(position)].load() & element_mask(position); + } + + void set(size_t pos, bool val = true) + { + const uint32_t bitmask = element_mask(pos); + + if (val) { + _data[array_index(pos)].fetch_or(bitmask); + + } else { + _data[array_index(pos)].fetch_and(~bitmask); + } + } + +private: + static constexpr uint8_t BITS_PER_ELEMENT = 32; + static constexpr size_t ARRAY_SIZE = ((N % BITS_PER_ELEMENT) == 0) ? (N / BITS_PER_ELEMENT) : + (N / BITS_PER_ELEMENT + 1); + static constexpr size_t ALLOCATED_BITS = ARRAY_SIZE * BITS_PER_ELEMENT; + + size_t array_index(size_t position) const { return position / BITS_PER_ELEMENT; } + uint32_t element_mask(size_t position) const { return (1 << (position % BITS_PER_ELEMENT)); } + + px4::atomic _data[ARRAY_SIZE]; +}; + +} // namespace px4 diff --git a/platforms/posix/cmake/sitl_tests.cmake b/platforms/posix/cmake/sitl_tests.cmake index 755f0fbbc4..737c272db5 100644 --- a/platforms/posix/cmake/sitl_tests.cmake +++ b/platforms/posix/cmake/sitl_tests.cmake @@ -4,8 +4,10 @@ # TODO: find a way to keep this in sync with tests_main set(tests + atomic_bitset autodeclination bezier + bitset bson commander controllib diff --git a/src/drivers/power_monitor/ina226/ina226.h b/src/drivers/power_monitor/ina226/ina226.h index 1b29aef055..ccd81f7041 100644 --- a/src/drivers/power_monitor/ina226/ina226.h +++ b/src/drivers/power_monitor/ina226/ina226.h @@ -163,7 +163,7 @@ private: actuator_controls_s _actuator_controls{}; Battery _battery; - uORB::Subscription _actuators_sub{ORB_ID(actuator_controls)}; + uORB::Subscription _actuators_sub{ORB_ID(actuator_controls_0)}; uORB::Subscription _parameters_sub{ORB_ID(parameter_update)}; /** diff --git a/src/include/containers/Bitset.hpp b/src/include/containers/Bitset.hpp new file mode 100644 index 0000000000..26d0c74e44 --- /dev/null +++ b/src/include/containers/Bitset.hpp @@ -0,0 +1,88 @@ +/**************************************************************************** + * + * Copyright (c) 2020 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +namespace px4 +{ + +template +class Bitset +{ +public: + + size_t count() const + { + size_t total = 0; + + for (auto x : _data) { + while (x) { + total += x & 1; + x >>= 1; + } + } + + return total; + } + + size_t size() const { return N; } + + bool operator[](size_t position) const + { + return _data[array_index(position)] & element_mask(position); + } + + void set(size_t pos, bool val = true) + { + const uint8_t bitmask = element_mask(pos); + + if (val) { + _data[array_index(pos)] |= bitmask; + + } else { + _data[array_index(pos)] &= ~bitmask; + } + } + +private: + static constexpr uint8_t BITS_PER_ELEMENT = 8; + static constexpr size_t ARRAY_SIZE = (N % BITS_PER_ELEMENT == 0) ? N / BITS_PER_ELEMENT : N / BITS_PER_ELEMENT + 1; + static constexpr size_t ALLOCATED_BITS = ARRAY_SIZE * BITS_PER_ELEMENT; + + size_t array_index(size_t position) const { return position / BITS_PER_ELEMENT; } + uint8_t element_mask(size_t position) const { return (1 << position % BITS_PER_ELEMENT); } + + uint8_t _data[ARRAY_SIZE] {}; +}; + +} // namespace px4 diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index e600fda154..6998bf1e6f 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -3966,17 +3966,7 @@ Commander::offboard_control_update() { const offboard_control_mode_s &offboard_control_mode = _offboard_control_mode_sub.get(); - // if this is the first time entering OFFBOARD the subscription may not be active yet - bool force_update = false; - - if (commander_state_s::MAIN_STATE_OFFBOARD) { - if (offboard_control_mode.timestamp == 0) { - _offboard_control_mode_sub.subscribe(); - force_update = true; - } - } - - if (_offboard_control_mode_sub.updated() || force_update) { + if (_offboard_control_mode_sub.updated()) { const offboard_control_mode_s old = offboard_control_mode; if (_offboard_control_mode_sub.update()) { diff --git a/src/modules/logger/logged_topics.cpp b/src/modules/logger/logged_topics.cpp index 12faa20811..024ccb943f 100644 --- a/src/modules/logger/logged_topics.cpp +++ b/src/modules/logger/logged_topics.cpp @@ -36,7 +36,7 @@ #include #include -#include +#include #include @@ -297,9 +297,9 @@ bool LoggedTopics::add_topic(const orb_metadata *topic, uint16_t interval_ms, ui } RequestedSubscription &sub = _subscriptions.sub[_subscriptions.count++]; - sub.topic = topic; sub.interval_ms = interval_ms; sub.instance = instance; + sub.id = static_cast(topic->o_id); return true; } @@ -314,7 +314,7 @@ bool LoggedTopics::add_topic(const char *name, uint16_t interval_ms, uint8_t ins // check if already added: if so, only update the interval for (int j = 0; j < _subscriptions.count; ++j) { - if (_subscriptions.sub[j].topic == topics[i] && + if (_subscriptions.sub[j].id == static_cast(topics[i]->o_id) && _subscriptions.sub[j].instance == instance) { PX4_DEBUG("logging topic %s(%d), interval: %i, already added, only setting interval", diff --git a/src/modules/logger/logged_topics.h b/src/modules/logger/logged_topics.h index ddcdea34f4..be794041d6 100644 --- a/src/modules/logger/logged_topics.h +++ b/src/modules/logger/logged_topics.h @@ -36,6 +36,7 @@ #include #include +#include namespace px4 { @@ -74,9 +75,9 @@ public: static constexpr int MAX_TOPICS_NUM = 200; /**< Maximum number of logged topics */ struct RequestedSubscription { - const orb_metadata *topic; uint16_t interval_ms; uint8_t instance; + ORB_ID id{ORB_ID::INVALID}; }; struct RequestedSubscriptionArray { RequestedSubscription sub[MAX_TOPICS_NUM]; diff --git a/src/modules/logger/logger.cpp b/src/modules/logger/logger.cpp index 97bb739db7..a63ae2822e 100644 --- a/src/modules/logger/logger.cpp +++ b/src/modules/logger/logger.cpp @@ -46,7 +46,7 @@ #include #include -#include +#include #include #include #include @@ -508,7 +508,7 @@ bool Logger::initialize_topics(MissionLogType mission_log_mode) const LoggedTopics::RequestedSubscription &sub = logged_topics.subscriptions().sub[i]; // if we poll on a topic, we don't use the interval and let the polled topic define the maximum interval uint16_t interval_ms = _polling_topic_meta ? 0 : sub.interval_ms; - _subscriptions[i] = LoggerSubscription(sub.topic, interval_ms, sub.instance); + _subscriptions[i] = LoggerSubscription(sub.id, interval_ms, sub.instance); } } diff --git a/src/modules/logger/logger.h b/src/modules/logger/logger.h index a1f899eba3..f705784928 100644 --- a/src/modules/logger/logger.h +++ b/src/modules/logger/logger.h @@ -71,8 +71,8 @@ struct LoggerSubscription : public uORB::SubscriptionInterval { LoggerSubscription() = default; - LoggerSubscription(const orb_metadata *meta, uint32_t interval_ms = 0, uint8_t instance = 0) : - uORB::SubscriptionInterval(meta, interval_ms * 1000, instance) + LoggerSubscription(ORB_ID id, uint32_t interval_ms = 0, uint8_t instance = 0) : + uORB::SubscriptionInterval(id, interval_ms * 1000, instance) {} }; diff --git a/src/modules/replay/Replay.hpp b/src/modules/replay/Replay.hpp index 73e93fdbf3..c5c29565ea 100644 --- a/src/modules/replay/Replay.hpp +++ b/src/modules/replay/Replay.hpp @@ -42,7 +42,7 @@ #include "definitions.hpp" #include -#include +#include #include namespace px4 diff --git a/src/modules/uORB/CMakeLists.txt b/src/modules/uORB/CMakeLists.txt index 7d293e0206..f963d0ba2e 100644 --- a/src/modules/uORB/CMakeLists.txt +++ b/src/modules/uORB/CMakeLists.txt @@ -60,7 +60,6 @@ if(NOT "${PX4_BOARD}" MATCHES "px4_io") # TODO: fix this hack (move uORB to plat uORBMain.cpp uORBManager.cpp uORBManager.hpp - uORBTopics.h uORBUtils.cpp uORBUtils.hpp DEPENDS diff --git a/src/modules/uORB/Subscription.cpp b/src/modules/uORB/Subscription.cpp index ddbdb56ef3..49b63aa619 100644 --- a/src/modules/uORB/Subscription.cpp +++ b/src/modules/uORB/Subscription.cpp @@ -42,48 +42,49 @@ namespace uORB { -bool -Subscription::subscribe() +bool Subscription::subscribe() { - // valid ORB_ID required - if (_meta == nullptr) { - return false; - } - // check if already subscribed if (_node != nullptr) { return true; } - DeviceMaster *device_master = uORB::Manager::get_instance()->get_device_master(); + if (_orb_id != ORB_ID::INVALID) { - if (device_master != nullptr) { - uORB::DeviceNode *node = device_master->getDeviceNode(_meta, _instance); + DeviceMaster *device_master = uORB::Manager::get_instance()->get_device_master(); - if (node != nullptr) { - _node = node; - _node->add_internal_subscriber(); + if (device_master != nullptr) { - // If there were any previous publications, allow the subscriber to read them - const unsigned curr_gen = _node->published_message_count(); - const uint8_t q_size = _node->get_queue_size(); - - if (q_size < curr_gen) { - _last_generation = curr_gen - q_size; - - } else { - _last_generation = 0; + if (!device_master->deviceNodeExists(_orb_id, _instance)) { + return false; } - return true; + uORB::DeviceNode *node = device_master->getDeviceNode(get_topic(), _instance); + + if (node != nullptr) { + _node = node; + _node->add_internal_subscriber(); + + // If there were any previous publications, allow the subscriber to read them + const unsigned curr_gen = _node->published_message_count(); + const uint8_t q_size = _node->get_queue_size(); + + if (q_size < curr_gen) { + _last_generation = curr_gen - q_size; + + } else { + _last_generation = 0; + } + + return true; + } } } return false; } -void -Subscription::unsubscribe() +void Subscription::unsubscribe() { if (_node != nullptr) { _node->remove_internal_subscriber(); @@ -93,28 +94,7 @@ Subscription::unsubscribe() _last_generation = 0; } -bool -Subscription::init() -{ - if (_meta != nullptr) { - // this throttles the relatively expensive calls to getDeviceNode() - if ((_last_generation == 0) || (_last_generation < 1000) || (_last_generation % 100 == 0)) { - if (subscribe()) { - return true; - } - } - - if (_node == nullptr) { - // use generation to count attempts to subscribe - _last_generation++; - } - } - - return false; -} - -bool -Subscription::update(uint64_t *time, void *dst) +bool Subscription::update(uint64_t *time, void *dst) { if ((time != nullptr) && (dst != nullptr) && advertised()) { // always copy data to dst regardless of update diff --git a/src/modules/uORB/Subscription.hpp b/src/modules/uORB/Subscription.hpp index edd09de543..b9f5b8bf7f 100644 --- a/src/modules/uORB/Subscription.hpp +++ b/src/modules/uORB/Subscription.hpp @@ -39,6 +39,8 @@ #pragma once #include +#include + #include #include "uORBDeviceNode.hpp" @@ -55,15 +57,28 @@ class Subscription { public: + /** + * Constructor + * + * @param id The uORB ORB_ID enum for the topic. + * @param instance The instance for multi sub. + */ + Subscription(ORB_ID id, uint8_t instance = 0) : + _orb_id(id), + _instance(instance) + { + } + /** * Constructor * * @param meta The uORB metadata (usually from the ORB_ID() macro) for the topic. * @param instance The instance for multi sub. */ - Subscription(const orb_metadata *meta, uint8_t instance = 0) : _meta(meta), _instance(instance) + Subscription(const orb_metadata *meta, uint8_t instance = 0) : + _orb_id((meta == nullptr) ? ORB_ID::INVALID : static_cast(meta->o_id)), + _instance(instance) { - subscribe(); } ~Subscription() @@ -82,7 +97,7 @@ public: } // try to initialize - if (init()) { + if (subscribe()) { // check again if valid if (valid()) { return _node->is_advertised(); @@ -121,7 +136,7 @@ public: bool copy(void *dst) { return advertised() ? _node->copy(dst, _last_generation) : false; } uint8_t get_instance() const { return _instance; } - orb_id_t get_topic() const { return _meta; } + orb_id_t get_topic() const { return get_orb_meta(_orb_id); } protected: @@ -129,18 +144,12 @@ protected: DeviceNode *get_node() { return _node; } - bool init(); - DeviceNode *_node{nullptr}; - const orb_metadata *_meta{nullptr}; - /** - * Subscription's latest data generation. - * Also used to track (and rate limit) subscription - * attempts if the topic has not yet been published. - */ - unsigned _last_generation{0}; - uint8_t _instance{0}; + unsigned _last_generation{0}; /**< last generation the subscriber has seen */ + + ORB_ID _orb_id{ORB_ID::INVALID}; + uint8_t _instance{0}; }; // Subscription wrapper class with data @@ -148,6 +157,18 @@ template class SubscriptionData : public Subscription { public: + /** + * Constructor + * + * @param id The uORB metadata ORB_ID enum for the topic. + * @param instance The instance for multi sub. + */ + SubscriptionData(ORB_ID id, uint8_t instance = 0) : + Subscription(id, instance) + { + copy(&_data); + } + /** * Constructor * diff --git a/src/modules/uORB/SubscriptionInterval.hpp b/src/modules/uORB/SubscriptionInterval.hpp index 771dfe1b2a..68143d19d3 100644 --- a/src/modules/uORB/SubscriptionInterval.hpp +++ b/src/modules/uORB/SubscriptionInterval.hpp @@ -55,6 +55,18 @@ class SubscriptionInterval { public: + /** + * Constructor + * + * @param id The uORB ORB_ID enum for the topic. + * @param interval The requested maximum update interval in microseconds. + * @param instance The instance for multi sub. + */ + SubscriptionInterval(ORB_ID id, uint32_t interval_us = 0, uint8_t instance = 0) : + _subscription{id, instance}, + _interval_us(interval_us) + {} + /** * Constructor * diff --git a/src/modules/uORB/uORB.h b/src/modules/uORB/uORB.h index a75dfb1f07..5709f8d2cd 100644 --- a/src/modules/uORB/uORB.h +++ b/src/modules/uORB/uORB.h @@ -52,6 +52,7 @@ struct orb_metadata { const uint16_t o_size; /**< object size */ const uint16_t o_size_no_padding; /**< object size w/o padding at the end (for logger) */ const char *o_fields; /**< semicolon separated list of fields (with type) */ + uint8_t o_id; /**< ORB_ID enum */ }; typedef const struct orb_metadata *orb_id_t; @@ -110,13 +111,15 @@ enum ORB_PRIO { * @param _struct The structure the topic provides. * @param _size_no_padding Struct size w/o padding at the end * @param _fields All fields in a semicolon separated list e.g: "float[3] position;bool armed" + * @param _orb_id_enum ORB ID enum e.g.: ORB_ID::vehicle_status */ -#define ORB_DEFINE(_name, _struct, _size_no_padding, _fields) \ +#define ORB_DEFINE(_name, _struct, _size_no_padding, _fields, _orb_id_enum) \ const struct orb_metadata __orb_##_name = { \ #_name, \ sizeof(_struct), \ _size_no_padding, \ - _fields \ + _fields, \ + _orb_id_enum \ }; struct hack __BEGIN_DECLS diff --git a/src/modules/uORB/uORBDeviceMaster.cpp b/src/modules/uORB/uORBDeviceMaster.cpp index b118ed6e95..ce8ddf19c7 100644 --- a/src/modules/uORB/uORBDeviceMaster.cpp +++ b/src/modules/uORB/uORBDeviceMaster.cpp @@ -163,6 +163,7 @@ uORB::DeviceMaster::advertise(const struct orb_metadata *meta, bool is_advertise // add to the node map. _node_list.add(node); + _node_exists[node->get_instance()].set((uint8_t)node->id(), true); } group_tries++; @@ -452,12 +453,25 @@ uORB::DeviceNode *uORB::DeviceMaster::getDeviceNode(const char *nodepath) return nullptr; } +bool uORB::DeviceMaster::deviceNodeExists(ORB_ID id, const uint8_t instance) +{ + if ((id == ORB_ID::INVALID) || (instance > ORB_MULTI_MAX_INSTANCES - 1)) { + return false; + } + + return _node_exists[instance][(uint8_t)id]; +} + uORB::DeviceNode *uORB::DeviceMaster::getDeviceNode(const struct orb_metadata *meta, const uint8_t instance) { if (meta == nullptr) { return nullptr; } + if (!deviceNodeExists(static_cast(meta->o_id), instance)) { + return nullptr; + } + lock(); uORB::DeviceNode *node = getDeviceNodeLocked(meta, instance); unlock(); diff --git a/src/modules/uORB/uORBDeviceMaster.hpp b/src/modules/uORB/uORBDeviceMaster.hpp index b7a097f688..203513813e 100644 --- a/src/modules/uORB/uORBDeviceMaster.hpp +++ b/src/modules/uORB/uORBDeviceMaster.hpp @@ -36,6 +36,8 @@ #include #include "uORBCommon.hpp" +#include + #include namespace uORB @@ -49,6 +51,9 @@ class Manager; #include #include +#include + +using px4::AtomicBitset; /** * Master control device for ObjDev. @@ -69,6 +74,8 @@ public: uORB::DeviceNode *getDeviceNode(const char *node_name); uORB::DeviceNode *getDeviceNode(const struct orb_metadata *meta, const uint8_t instance); + bool deviceNodeExists(ORB_ID id, const uint8_t instance); + /** * Print statistics for each existing topic. * @param reset if true, reset statistics afterwards @@ -111,6 +118,7 @@ private: uORB::DeviceNode *getDeviceNodeLocked(const struct orb_metadata *meta, const uint8_t instance); List _node_list; + AtomicBitset _node_exists[ORB_MULTI_MAX_INSTANCES]; hrt_abstime _last_statistics_output; diff --git a/src/modules/uORB/uORBDeviceNode.hpp b/src/modules/uORB/uORBDeviceNode.hpp index e0efaa7ea3..4040c6e24b 100644 --- a/src/modules/uORB/uORBDeviceNode.hpp +++ b/src/modules/uORB/uORBDeviceNode.hpp @@ -192,6 +192,8 @@ public: const orb_metadata *get_meta() const { return _meta; } + ORB_ID id() const { return static_cast(_meta->o_id); } + const char *get_name() const { return _meta->o_name; } uint8_t get_instance() const { return _instance; } diff --git a/src/modules/uORB/uORB_tests/uORBTest_UnitTest.cpp b/src/modules/uORB/uORB_tests/uORBTest_UnitTest.cpp index 0d87d10637..d2c7ae97e7 100644 --- a/src/modules/uORB/uORB_tests/uORBTest_UnitTest.cpp +++ b/src/modules/uORB/uORB_tests/uORBTest_UnitTest.cpp @@ -40,21 +40,6 @@ #include #include -ORB_DEFINE(orb_test, struct orb_test, sizeof(orb_test), "ORB_TEST:int val;hrt_abstime time;"); -ORB_DEFINE(orb_multitest, struct orb_test, sizeof(orb_test), "ORB_MULTITEST:int val;hrt_abstime time;"); - -ORB_DEFINE(orb_test_medium, struct orb_test_medium, sizeof(orb_test_medium), - "ORB_TEST_MEDIUM:int val;hrt_abstime time;char[64] junk;"); -ORB_DEFINE(orb_test_medium_multi, struct orb_test_medium, sizeof(orb_test_medium), - "ORB_TEST_MEDIUM_MULTI:int val;hrt_abstime time;char[64] junk;"); -ORB_DEFINE(orb_test_medium_queue, struct orb_test_medium, sizeof(orb_test_medium), - "ORB_TEST_MEDIUM_MULTI:int val;hrt_abstime time;char[64] junk;"); -ORB_DEFINE(orb_test_medium_queue_poll, struct orb_test_medium, sizeof(orb_test_medium), - "ORB_TEST_MEDIUM_MULTI:int val;hrt_abstime time;char[64] junk;"); - -ORB_DEFINE(orb_test_large, struct orb_test_large, sizeof(orb_test_large), - "ORB_TEST_LARGE:int val;hrt_abstime time;char[512] junk;"); - uORBTest::UnitTest &uORBTest::UnitTest::instance() { static uORBTest::UnitTest t; @@ -73,7 +58,7 @@ int uORBTest::UnitTest::pubsublatency_main() int test_multi_sub_medium = orb_subscribe_multi(ORB_ID(orb_test_medium), 0); int test_multi_sub_large = orb_subscribe_multi(ORB_ID(orb_test_large), 0); - orb_test_large t{}; + orb_test_large_s t{}; /* clear all ready flags */ orb_copy(ORB_ID(orb_test), test_multi_sub, &t); @@ -121,7 +106,7 @@ int uORBTest::UnitTest::pubsublatency_main() num_missed += t.val - current_value - 1; current_value = t.val; - unsigned elt = (unsigned)hrt_elapsed_time_atomic(&t.time); + unsigned elt = (unsigned)hrt_elapsed_time_atomic(&t.timestamp); latency_integral += elt; timings[i] = elt; @@ -241,7 +226,7 @@ int uORBTest::UnitTest::test_unadvertise() //try to advertise and see whether we get the right instance int instance_test[4] {}; - orb_test t{}; + orb_test_s t{}; for (int i = 0; i < 4; ++i) { _pfd[i] = orb_advertise_multi(ORB_ID(orb_multitest), &t, &instance_test[i], ORB_PRIO_MAX); @@ -268,8 +253,8 @@ int uORBTest::UnitTest::test_single() { test_note("try single-topic support"); - orb_test t{}; - orb_test u{}; + orb_test_s t{}; + orb_test_s u{}; int sfd = -1; orb_advert_t ptopic{}; bool updated{false}; @@ -346,8 +331,8 @@ int uORBTest::UnitTest::test_multi() /* this routine tests the multi-topic support */ test_note("try multi-topic support"); - orb_test t{}; - orb_test u{}; + orb_test_s t{}; + orb_test_s u{}; int instance0; _pfd[0] = orb_advertise_multi(ORB_ID(orb_multitest), &t, &instance0, ORB_PRIO_MAX); @@ -419,7 +404,7 @@ int uORBTest::UnitTest::test_multi() return test_fail("prio: %d", prio); } - if (PX4_OK != latency_test(ORB_ID(orb_test), false)) { + if (PX4_OK != latency_test(ORB_ID(orb_test), false)) { return test_fail("latency test failed"); } @@ -440,7 +425,7 @@ int uORBTest::UnitTest::pub_test_multi2_main() int data_next_idx = 0; const int num_instances = 3; orb_advert_t orb_pub[num_instances] {}; - orb_test_medium data_topic{}; + orb_test_medium_s data_topic{}; for (int i = 0; i < num_instances; ++i) { orb_advert_t &pub = orb_pub[i]; @@ -464,7 +449,7 @@ int uORBTest::UnitTest::pub_test_multi2_main() px4_usleep(2); //make sure the timestamps are different orb_advert_t &pub = orb_pub[data_next_idx]; - data_topic.time = hrt_absolute_time(); + data_topic.timestamp = hrt_absolute_time(); data_topic.val = data_next_idx; orb_publish(ORB_ID(orb_test_medium_multi), pub, &data_topic); @@ -525,16 +510,16 @@ int uORBTest::UnitTest::test_multi2() orb_check(orb_data_cur_fd, &updated); if (updated) { - orb_test_medium msg{}; + orb_test_medium_s msg{}; orb_copy(ORB_ID(orb_test_medium_multi), orb_data_cur_fd, &msg); - if (last_time >= msg.time && last_time != 0) { - return test_fail("Timestamp not increasing! (%" PRIu64 " >= %" PRIu64 ")", last_time, msg.time); + if (last_time >= msg.timestamp && last_time != 0) { + return test_fail("Timestamp not increasing! (%" PRIu64 " >= %" PRIu64 ")", last_time, msg.timestamp); } - last_time = msg.time; + last_time = msg.timestamp; - PX4_DEBUG("got message (val=%i, idx=%i, t=%" PRIu64 ")", msg.val, orb_data_next, msg.time); + PX4_DEBUG("got message (val=%i, idx=%i, t=%" PRIu64 ")", msg.val, orb_data_next, msg.timestamp); orb_data_next = (orb_data_next + 1) % num_instances; } } @@ -559,7 +544,8 @@ int uORBTest::UnitTest::test_multi_reversed() return test_fail("sub. id2: ret: %d", sfd2); } - struct orb_test t {}, u {}; + orb_test_s t{}; + orb_test_s u{}; t.val = 0; @@ -621,18 +607,17 @@ int uORBTest::UnitTest::test_queue() { test_note("Testing orb queuing"); - struct orb_test_medium t, u; - int sfd; - orb_advert_t ptopic; - bool updated; + orb_test_medium_s t{}; + orb_test_medium_s u{}; + orb_advert_t ptopic{nullptr}; + bool updated{false}; - sfd = orb_subscribe(ORB_ID(orb_test_medium_queue)); + int sfd = orb_subscribe(ORB_ID(orb_test_medium_queue)); if (sfd < 0) { return test_fail("subscribe failed: %d", errno); } - const int queue_size = 11; t.val = 0; ptopic = orb_advertise_queue(ORB_ID(orb_test_medium_queue), &t, queue_size); @@ -738,17 +723,18 @@ int uORBTest::UnitTest::pub_test_queue_entry(int argc, char *argv[]) int uORBTest::UnitTest::pub_test_queue_main() { - struct orb_test_medium t; - orb_advert_t ptopic; + orb_test_medium_s t{}; + orb_advert_t ptopic{nullptr}; const int queue_size = 50; - t.val = 0; if ((ptopic = orb_advertise_queue(ORB_ID(orb_test_medium_queue_poll), &t, queue_size)) == nullptr) { _thread_should_exit = true; return test_fail("advertise failed: %d", errno); } - int message_counter = 0, num_messages = 20 * queue_size; + int message_counter = 0; + int num_messages = 20 * queue_size; + ++t.val; while (message_counter < num_messages) { @@ -777,7 +763,7 @@ int uORBTest::UnitTest::test_queue_poll_notify() { test_note("Testing orb queuing (poll & notify)"); - orb_test_medium t{}; + orb_test_medium_s t{}; int sfd = -1; if ((sfd = orb_subscribe(ORB_ID(orb_test_medium_queue_poll))) < 0) { diff --git a/src/modules/uORB/uORB_tests/uORBTest_UnitTest.hpp b/src/modules/uORB/uORB_tests/uORBTest_UnitTest.hpp index 90cc3e8fd0..8812f92a16 100644 --- a/src/modules/uORB/uORB_tests/uORBTest_UnitTest.hpp +++ b/src/modules/uORB/uORB_tests/uORBTest_UnitTest.hpp @@ -33,38 +33,22 @@ #ifndef _uORBTest_UnitTest_hpp_ #define _uORBTest_UnitTest_hpp_ -#include "../uORBCommon.hpp" -#include "../uORB.h" + +#include +#include +#include +#include + +#include +#include #include #include + +#include + +#include #include -struct orb_test { - int val; - hrt_abstime time; -}; -ORB_DECLARE(orb_test); -ORB_DECLARE(orb_multitest); - - -struct orb_test_medium { - int val; - hrt_abstime time; - char junk[64]; -}; -ORB_DECLARE(orb_test_medium); -ORB_DECLARE(orb_test_medium_multi); -ORB_DECLARE(orb_test_medium_queue); -ORB_DECLARE(orb_test_medium_queue_poll); - -struct orb_test_large { - int val; - hrt_abstime time; - char junk[512]; -}; -ORB_DECLARE(orb_test_large); - - namespace uORBTest { class UnitTest; @@ -125,9 +109,9 @@ template int uORBTest::UnitTest::latency_test(orb_id_t T, bool print) { test_note("---------------- LATENCY TEST ------------------"); - S t; + S t{}; t.val = 308; - t.time = hrt_absolute_time(); + t.timestamp = hrt_absolute_time(); orb_advert_t pfd0 = orb_advertise(T, &t); @@ -155,7 +139,7 @@ int uORBTest::UnitTest::latency_test(orb_id_t T, bool print) /* give the test task some data */ while (!pubsubtest_passed) { ++t.val; - t.time = hrt_absolute_time(); + t.timestamp = hrt_absolute_time(); if (PX4_OK != orb_publish(T, pfd0, &t)) { return test_fail("mult. pub0 timing fail"); diff --git a/src/modules/uORB/uORB_tests/uORB_tests_main.cpp b/src/modules/uORB/uORB_tests/uORB_tests_main.cpp index 37abf6758b..3fbc46e432 100644 --- a/src/modules/uORB/uORB_tests/uORB_tests_main.cpp +++ b/src/modules/uORB/uORB_tests/uORB_tests_main.cpp @@ -32,9 +32,6 @@ ****************************************************************************/ #include -#include "../uORBDeviceNode.hpp" -#include "../uORB.h" -#include "../uORBCommon.hpp" #include "uORBTest_UnitTest.hpp" @@ -73,13 +70,13 @@ uorb_tests_main(int argc, char *argv[]) uORBTest::UnitTest &t = uORBTest::UnitTest::instance(); if (argc > 2 && !strcmp(argv[2], "medium")) { - return t.latency_test(ORB_ID(orb_test_medium), true); + return t.latency_test(ORB_ID(orb_test_medium), true); } else if (argc > 2 && !strcmp(argv[2], "large")) { - return t.latency_test(ORB_ID(orb_test_large), true); + return t.latency_test(ORB_ID(orb_test_large), true); } else { - return t.latency_test(ORB_ID(orb_test), true); + return t.latency_test(ORB_ID(orb_test), true); } } diff --git a/src/systemcmds/tests/CMakeLists.txt b/src/systemcmds/tests/CMakeLists.txt index eb3c365b9e..4c99549462 100644 --- a/src/systemcmds/tests/CMakeLists.txt +++ b/src/systemcmds/tests/CMakeLists.txt @@ -33,8 +33,10 @@ set(srcs test_adc.c + test_atomic_bitset.cpp test_autodeclination.cpp test_bezierQuad.cpp + test_bitset.cpp test_bson.cpp test_conv.cpp test_dataman.c diff --git a/src/systemcmds/tests/test_atomic_bitset.cpp b/src/systemcmds/tests/test_atomic_bitset.cpp new file mode 100644 index 0000000000..46532eb70d --- /dev/null +++ b/src/systemcmds/tests/test_atomic_bitset.cpp @@ -0,0 +1,175 @@ +/**************************************************************************** + * + * Copyright (c) 2020 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include + +#include + +class AtomicBitsetTest : public UnitTest +{ +public: + virtual bool run_tests(); + +private: + bool constructTest(); + bool setAllTest(); + bool setRandomTest(); + +}; + +bool AtomicBitsetTest::run_tests() +{ + ut_run_test(constructTest); + ut_run_test(setAllTest); + ut_run_test(setRandomTest); + + return (_tests_failed == 0); +} + + +ut_declare_test_c(test_atomic_bitset, AtomicBitsetTest) + +bool AtomicBitsetTest::constructTest() +{ + px4::AtomicBitset<10> test_bitset1; + + ut_compare("bitset size 10", test_bitset1.size(), 10); + ut_compare("bitset init count 0", test_bitset1.count(), 0); + + for (int i = 0; i < test_bitset1.size(); i++) { + ut_compare("bitset not set by default", test_bitset1[i], false); + } + + return true; +} + +bool AtomicBitsetTest::setAllTest() +{ + px4::AtomicBitset<100> test_bitset2; + + ut_compare("bitset size 100", test_bitset2.size(), 100); + ut_compare("bitset init count 0", test_bitset2.count(), 0); + + for (int i = 0; i < test_bitset2.size(); i++) { + ut_compare("bitset not set by default", test_bitset2[i], false); + } + + // set all + for (int i = 0; i < test_bitset2.size(); i++) { + test_bitset2.set(i, true); + } + + // check count + ut_compare("bitset count", test_bitset2.count(), 100); + + // verify all set + for (int i = 0; i < test_bitset2.size(); i++) { + ut_compare("bitset not true", test_bitset2[i], true); + } + + // set all back to false + for (int i = 0; i < test_bitset2.size(); i++) { + test_bitset2.set(i, false); + } + + // check count + ut_compare("bitset count", test_bitset2.count(), 0); + + // verify all no longer set + for (int i = 0; i < test_bitset2.size(); i++) { + ut_compare("bitset not false", test_bitset2[i], false); + } + + return true; +} + +bool AtomicBitsetTest::setRandomTest() +{ + px4::AtomicBitset<999> test_bitset3; + + ut_compare("bitset size 999", test_bitset3.size(), 999); + ut_compare("bitset init count 0", test_bitset3.count(), 0); + + for (int i = 0; i < test_bitset3.size(); i++) { + ut_compare("bitset not set by default", test_bitset3[i], false); + } + + // random set and verify 100 elements + const int random_test_size = 5; + int random_array[random_test_size] = { 3, 1, 4, 5, 9 }; + + // set random elements + for (auto x : random_array) { + test_bitset3.set(x, true); + ut_less_than("invalid test element range", x, test_bitset3.size()); + } + + // check count + ut_compare("bitset count", test_bitset3.count(), random_test_size); + + // check that only random elements are set + for (int i = 0; i < test_bitset3.size(); i++) { + + // is i in the random test array + // if so it should be set + bool i_in_random = false; + + for (auto x : random_array) { + if (i == x) { + i_in_random = true; + } + } + + if (i_in_random) { + ut_compare("bitset true", test_bitset3[i], true); + + } else { + ut_compare("bitset false", test_bitset3[i], false); + } + } + + // set all back to false + for (int i = 0; i < test_bitset3.size(); i++) { + test_bitset3.set(i, false); + } + + // check count + ut_compare("bitset count", test_bitset3.count(), 0); + + // verify all no longer set + for (int i = 0; i < test_bitset3.size(); i++) { + ut_compare("bitset not false", test_bitset3[i], false); + } + + return true; +} diff --git a/src/systemcmds/tests/test_bitset.cpp b/src/systemcmds/tests/test_bitset.cpp new file mode 100644 index 0000000000..9c7568d2a4 --- /dev/null +++ b/src/systemcmds/tests/test_bitset.cpp @@ -0,0 +1,175 @@ +/**************************************************************************** + * + * Copyright (c) 2020 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include + +#include + +class BitsetTest : public UnitTest +{ +public: + virtual bool run_tests(); + +private: + bool constructTest(); + bool setAllTest(); + bool setRandomTest(); + +}; + +bool BitsetTest::run_tests() +{ + ut_run_test(constructTest); + ut_run_test(setAllTest); + ut_run_test(setRandomTest); + + return (_tests_failed == 0); +} + + +ut_declare_test_c(test_bitset, BitsetTest) + +bool BitsetTest::constructTest() +{ + px4::Bitset<10> test_bitset1; + + ut_compare("bitset size 10", test_bitset1.size(), 10); + ut_compare("bitset init count 0", test_bitset1.count(), 0); + + for (int i = 0; i < test_bitset1.size(); i++) { + ut_compare("bitset not set by default", test_bitset1[i], false); + } + + return true; +} + +bool BitsetTest::setAllTest() +{ + px4::Bitset<100> test_bitset2; + + ut_compare("bitset size 100", test_bitset2.size(), 100); + ut_compare("bitset init count 0", test_bitset2.count(), 0); + + for (int i = 0; i < test_bitset2.size(); i++) { + ut_compare("bitset not set by default", test_bitset2[i], false); + } + + // set all + for (int i = 0; i < test_bitset2.size(); i++) { + test_bitset2.set(i, true); + } + + // check count + ut_compare("bitset count", test_bitset2.count(), 100); + + // verify all set + for (int i = 0; i < test_bitset2.size(); i++) { + ut_compare("bitset not true", test_bitset2[i], true); + } + + // set all back to false + for (int i = 0; i < test_bitset2.size(); i++) { + test_bitset2.set(i, false); + } + + // check count + ut_compare("bitset count", test_bitset2.count(), 0); + + // verify all no longer set + for (int i = 0; i < test_bitset2.size(); i++) { + ut_compare("bitset not false", test_bitset2[i], false); + } + + return true; +} + +bool BitsetTest::setRandomTest() +{ + px4::Bitset<999> test_bitset3; + + ut_compare("bitset size 999", test_bitset3.size(), 999); + ut_compare("bitset init count 0", test_bitset3.count(), 0); + + for (int i = 0; i < test_bitset3.size(); i++) { + ut_compare("bitset not set by default", test_bitset3[i], false); + } + + // random set and verify 100 elements + const int random_test_size = 5; + int random_array[random_test_size] = { 3, 1, 4, 5, 9 }; + + // set random elements + for (auto x : random_array) { + test_bitset3.set(x, true); + ut_less_than("invalid test element range", x, test_bitset3.size()); + } + + // check count + ut_compare("bitset count", test_bitset3.count(), random_test_size); + + // check that only random elements are set + for (int i = 0; i < test_bitset3.size(); i++) { + + // is i in the random test array + // if so it should be set + bool i_in_random = false; + + for (auto x : random_array) { + if (i == x) { + i_in_random = true; + } + } + + if (i_in_random) { + ut_compare("bitset true", test_bitset3[i], true); + + } else { + ut_compare("bitset false", test_bitset3[i], false); + } + } + + // set all back to false + for (int i = 0; i < test_bitset3.size(); i++) { + test_bitset3.set(i, false); + } + + // check count + ut_compare("bitset count", test_bitset3.count(), 0); + + // verify all no longer set + for (int i = 0; i < test_bitset3.size(); i++) { + ut_compare("bitset not false", test_bitset3[i], false); + } + + return true; +} diff --git a/src/systemcmds/tests/test_microbench_uorb.cpp b/src/systemcmds/tests/test_microbench_uorb.cpp index 13643d95a4..9ae714233d 100644 --- a/src/systemcmds/tests/test_microbench_uorb.cpp +++ b/src/systemcmds/tests/test_microbench_uorb.cpp @@ -172,6 +172,12 @@ bool MicroBenchORB::time_px4_uorb() PERF("orb_exists sensor_accel 2", ret = orb_exists(ORB_ID(sensor_accel), 2), 100); PERF("orb_exists sensor_accel 3", ret = orb_exists(ORB_ID(sensor_accel), 3), 100); PERF("orb_exists sensor_accel 4", ret = orb_exists(ORB_ID(sensor_accel), 4), 100); + PERF("orb_exists sensor_accel 5", ret = orb_exists(ORB_ID(sensor_accel), 5), 100); + PERF("orb_exists sensor_accel 6", ret = orb_exists(ORB_ID(sensor_accel), 6), 100); + PERF("orb_exists sensor_accel 7", ret = orb_exists(ORB_ID(sensor_accel), 7), 100); + PERF("orb_exists sensor_accel 8", ret = orb_exists(ORB_ID(sensor_accel), 8), 100); + PERF("orb_exists sensor_accel 9", ret = orb_exists(ORB_ID(sensor_accel), 9), 100); + PERF("orb_exists sensor_accel 10", ret = orb_exists(ORB_ID(sensor_accel), 10), 100); orb_unsubscribe(fd_status); orb_unsubscribe(fd_lpos); @@ -196,11 +202,33 @@ bool MicroBenchORB::time_px4_uorb_direct() PERF("uORB::Subscription orb_check vehicle_local_position", ret = local_pos.updated(), 1000); PERF("uORB::Subscription orb_copy vehicle_local_position", ret = local_pos.copy(&lpos), 1000); - printf("\n"); + { + printf("\n"); + uORB::Subscription sens_gyro0{ORB_ID(sensor_gyro), 0}; + PERF("uORB::Subscription orb_check sensor_gyro:0", ret = sens_gyro0.updated(), 1000); + PERF("uORB::Subscription orb_copy sensor_gyro:0", ret = sens_gyro0.copy(&gyro), 1000); + } - uORB::Subscription sens_gyro{ORB_ID(sensor_gyro)}; - PERF("uORB::Subscription orb_check sensor_gyro", ret = sens_gyro.updated(), 1000); - PERF("uORB::Subscription orb_copy sensor_gyro", ret = sens_gyro.copy(&gyro), 1000); + { + printf("\n"); + uORB::Subscription sens_gyro1{ORB_ID(sensor_gyro), 1}; + PERF("uORB::Subscription orb_check sensor_gyro:1", ret = sens_gyro1.updated(), 1000); + PERF("uORB::Subscription orb_copy sensor_gyro:1", ret = sens_gyro1.copy(&gyro), 1000); + } + + { + printf("\n"); + uORB::Subscription sens_gyro2{ORB_ID(sensor_gyro), 2}; + PERF("uORB::Subscription orb_check sensor_gyro:2", ret = sens_gyro2.updated(), 1000); + PERF("uORB::Subscription orb_copy sensor_gyro:2", ret = sens_gyro2.copy(&gyro), 1000); + } + + { + printf("\n"); + uORB::Subscription sens_gyro3{ORB_ID(sensor_gyro), 3}; + PERF("uORB::Subscription orb_check sensor_gyro:3", ret = sens_gyro3.updated(), 1000); + PERF("uORB::Subscription orb_copy sensor_gyro:3", ret = sens_gyro3.copy(&gyro), 1000); + } return true; } diff --git a/src/systemcmds/tests/tests_main.c b/src/systemcmds/tests/tests_main.c index 2f59e20f87..e5443e723c 100644 --- a/src/systemcmds/tests/tests_main.c +++ b/src/systemcmds/tests/tests_main.c @@ -78,9 +78,10 @@ const struct { {"rc", rc_tests_main, 0}, #endif /* __PX4_NUTTX */ - + {"atomic_bitset", test_atomic_bitset, 0}, {"autodeclination", test_autodeclination, 0}, {"bezier", test_bezierQuad, 0}, + {"bitset", test_bitset, 0}, {"bson", test_bson, 0}, {"conv", test_conv, 0}, {"dataman", test_dataman, OPT_NOJIGTEST | OPT_NOALLTEST}, diff --git a/src/systemcmds/tests/tests_main.h b/src/systemcmds/tests/tests_main.h index edd5232840..14396dacc5 100644 --- a/src/systemcmds/tests/tests_main.h +++ b/src/systemcmds/tests/tests_main.h @@ -44,8 +44,10 @@ __BEGIN_DECLS extern int test_adc(int argc, char *argv[]); +extern int test_atomic_bitset(int argc, char *argv[]); extern int test_autodeclination(int argc, char *argv[]); extern int test_bezierQuad(int argc, char *argv[]); +extern int test_bitset(int argc, char *argv[]); extern int test_bson(int argc, char *argv[]); extern int test_conv(int argc, char *argv[]); extern int test_dataman(int argc, char *argv[]);