dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

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/
This commit is contained in:
Daniel Agar 2020-03-11 09:06:33 -04:00 committed by GitHub
parent 88c9761f1f
commit 9585055e9e
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
38 changed files with 919 additions and 200 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
msg/CMakeLists.txt
View File

@ -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}

5
msg/orb_test.msg Normal file
View File

@ -0,0 +1,5 @@
uint64 timestamp # time since system start (microseconds)
int32 val
# TOPICS orb_test orb_multitest

5
msg/orb_test_large.msg Normal file
View File

@ -0,0 +1,5 @@
uint64 timestamp # time since system start (microseconds)
int32 val
uint8[512] junk

7
msg/orb_test_medium.msg Normal file
View File

@ -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

5
msg/templates/uorb/msg.cpp.em
View File

@ -70,6 +70,7 @@ topic_fields = ["%s %s" % (convert_type(field.type), field.name) for field in so
#include <px4_platform_common/log.h>
#include <px4_platform_common/defines.h>
#include <uORB/topics/@(topic_name).h>
#include <uORB/topics/uORBTopics.hpp>
#include <drivers/drv_hrt.h>
#include <lib/drivers/device/Device.hpp>
@ -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<uint8_t>(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;

22
msg/templates/uorb/uORBTopics.cpp.em
View File

@ -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 <uORB/uORBTopics.h>
#include <uORB/topics/uORBTopics.hpp>
#include <uORB/uORB.h>
@{
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 <uORB/topics/@(msg_name).h>
@[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<uint8_t>(id)];
}

76
msg/templates/uorb/uORBTopics.hpp.em Normal file
View File

@ -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 <stddef.h>
#include <uORB/uORB.h>
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);

49
msg/templates/uorb_microcdr/uORBTopics.hpp.em Normal file
View File

@ -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 <uORB/uORB.h>

26
msg/tools/px_generate_uorb_topic_files.py
View File

@ -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,

6
msg/tools/uorb_rtps_message_ids.yaml
View File

@ -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

6
platforms/common/include/px4_platform_common/atomic.h
View File

@ -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
};

95
platforms/common/include/px4_platform_common/atomic_bitset.h Normal file
View File

@ -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 <size_t N>
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<uint32_t> _data[ARRAY_SIZE];
};
} // namespace px4

2
platforms/posix/cmake/sitl_tests.cmake
View File

@ -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

2
src/drivers/power_monitor/ina226/ina226.h
View File

@ -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)};
/**

88
src/include/containers/Bitset.hpp Normal file
View File

@ -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 <size_t N>
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

12
src/modules/commander/Commander.cpp
View File

@ -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()) {

6
src/modules/logger/logged_topics.cpp
View File

@ -36,7 +36,7 @@
#include <px4_platform_common/log.h>
#include <px4_platform_common/px4_config.h>
#include <uORB/uORBTopics.h>
#include <uORB/topics/uORBTopics.hpp>
#include <string.h>
@ -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<ORB_ID>(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<ORB_ID>(topics[i]->o_id) &&
_subscriptions.sub[j].instance == instance) {
PX4_DEBUG("logging topic %s(%d), interval: %i, already added, only setting interval",

3
src/modules/logger/logged_topics.h
View File

@ -36,6 +36,7 @@
#include <stdint.h>
#include <uORB/uORB.h>
#include <uORB/topics/uORBTopics.hpp>
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];

4
src/modules/logger/logger.cpp
View File

@ -46,7 +46,7 @@
#include <time.h>
#include <uORB/PublicationQueued.hpp>
#include <uORB/uORBTopics.h>
#include <uORB/topics/uORBTopics.hpp>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/vehicle_command_ack.h>
#include <uORB/topics/battery_status.h>
@ -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);
}
}

4
src/modules/logger/logger.h
View File

@ -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)
{}
};

2
src/modules/replay/Replay.hpp
View File

@ -42,7 +42,7 @@
#include "definitions.hpp"
#include <px4_platform_common/module.h>
#include <uORB/uORBTopics.h>
#include <uORB/topics/uORBTopics.hpp>
#include <uORB/topics/ekf2_timestamps.h>
namespace px4

1
src/modules/uORB/CMakeLists.txt
View File

@ -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

74
src/modules/uORB/Subscription.cpp
View File

@ -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

49
src/modules/uORB/Subscription.hpp
View File

@ -39,6 +39,8 @@
#pragma once
#include <uORB/uORB.h>
#include <uORB/topics/uORBTopics.hpp>
#include <px4_platform_common/defines.h>
#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<ORB_ID>(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 T>
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
*

12
src/modules/uORB/SubscriptionInterval.hpp
View File

@ -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
*

7
src/modules/uORB/uORB.h
View File

@ -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

14
src/modules/uORB/uORBDeviceMaster.cpp
View File

@ -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<ORB_ID>(meta->o_id), instance)) {
return nullptr;
}
lock();
uORB::DeviceNode *node = getDeviceNodeLocked(meta, instance);
unlock();

8
src/modules/uORB/uORBDeviceMaster.hpp
View File

@ -36,6 +36,8 @@
#include <stdint.h>
#include "uORBCommon.hpp"
#include <uORB/topics/uORBTopics.hpp>
#include <px4_platform_common/posix.h>
namespace uORB
@ -49,6 +51,9 @@ class Manager;
#include <stdlib.h>
#include <containers/List.hpp>
#include <px4_platform_common/atomic_bitset.h>
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<uORB::DeviceNode *> _node_list;
AtomicBitset<ORB_TOPICS_COUNT> _node_exists[ORB_MULTI_MAX_INSTANCES];
hrt_abstime _last_statistics_output;

2
src/modules/uORB/uORBDeviceNode.hpp
View File

@ -192,6 +192,8 @@ public:
const orb_metadata *get_meta() const { return _meta; }
ORB_ID id() const { return static_cast<ORB_ID>(_meta->o_id); }
const char *get_name() const { return _meta->o_name; }
uint8_t get_instance() const { return _instance; }

70
src/modules/uORB/uORB_tests/uORBTest_UnitTest.cpp
View File

@ -40,21 +40,6 @@
#include <math.h>
#include <lib/cdev/CDev.hpp>
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<struct orb_test>(ORB_ID(orb_test), false)) {
if (PX4_OK != latency_test<orb_test_s>(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) {

46
src/modules/uORB/uORB_tests/uORBTest_UnitTest.hpp
View File

@ -33,38 +33,22 @@
#ifndef _uORBTest_UnitTest_hpp_
#define _uORBTest_UnitTest_hpp_
#include "../uORBCommon.hpp"
#include "../uORB.h"
#include <uORB/uORB.h>
#include <uORB/topics/orb_test.h>
#include <uORB/topics/orb_test_medium.h>
#include <uORB/topics/orb_test_large.h>
#include <px4_platform_common/defines.h>
#include <px4_platform_common/posix.h>
#include <px4_platform_common/time.h>
#include <px4_platform_common/tasks.h>
#include <drivers/drv_hrt.h>
#include <errno.h>
#include <unistd.h>
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<typename S>
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");

9
src/modules/uORB/uORB_tests/uORB_tests_main.cpp
View File

@ -32,9 +32,6 @@
****************************************************************************/
#include <string.h>
#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<struct orb_test_medium>(ORB_ID(orb_test_medium), true);
return t.latency_test<orb_test_medium_s>(ORB_ID(orb_test_medium), true);
} else if (argc > 2 && !strcmp(argv[2], "large")) {
return t.latency_test<struct orb_test_large>(ORB_ID(orb_test_large), true);
return t.latency_test<orb_test_large_s>(ORB_ID(orb_test_large), true);
} else {
return t.latency_test<struct orb_test>(ORB_ID(orb_test), true);
return t.latency_test<orb_test_s>(ORB_ID(orb_test), true);
}
}

2
src/systemcmds/tests/CMakeLists.txt
View File

@ -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

175
src/systemcmds/tests/test_atomic_bitset.cpp Normal file
View File

@ -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 <unit_test.h>
#include <px4_platform_common/atomic_bitset.h>
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;
}

175
src/systemcmds/tests/test_bitset.cpp Normal file
View File

@ -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 <unit_test.h>
#include <include/containers/Bitset.hpp>
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;
}

36
src/systemcmds/tests/test_microbench_uorb.cpp
View File

@ -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;
}

3
src/systemcmds/tests/tests_main.c
View File

@ -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},

2
src/systemcmds/tests/tests_main.h
View File

@ -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[]);