fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

Inline uORBDeviceNode::copy for performance improvement

Browse Source 
Signed-off-by: Jukka Laitinen <jukkax@ssrc.tii.ae>
This commit is contained in:
Jukka Laitinen 2021-11-12 16:01:35 +02:00 committed by Beat Küng
parent 3f884c5d24
commit b66270f8a8
2 changed files with 51 additions and 52 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

51
platforms/common/uORB/uORBDeviceNode.cpp
View File

@ -44,17 +44,6 @@
static uORB::SubscriptionInterval *filp_to_subscription(cdev::file_t *filp) { return static_cast<uORB::SubscriptionInterval *>(filp->f_priv); }
// Determine the data range
static inline bool is_in_range(unsigned left, unsigned value, unsigned right)
{
if (right > left) {
return (left <= value) && (value <= right);
} else { // Maybe the data overflowed and a wraparound occurred
return (left <= value) || (value <= right);
}
}
// round up to nearest power of two
// Such as 0 => 1, 1 => 1, 2 => 2 ,3 => 4, 10 => 16, 60 => 64, 65...255 => 128
// Note: When the input value > 128, the output is always 128
@ -151,46 +140,6 @@ uORB::DeviceNode::close(cdev::file_t *filp)
return CDev::close(filp);
}
bool
uORB::DeviceNode::copy(void *dst, unsigned &generation)
{
if ((dst != nullptr) && (_data != nullptr)) {
if (_queue_size == 1) {
ATOMIC_ENTER;
memcpy(dst, _data, _meta->o_size);
generation = _generation.load();
ATOMIC_LEAVE;
return true;
} else {
ATOMIC_ENTER;
const unsigned current_generation = _generation.load();
if (current_generation == generation) {
/* The subscriber already read the latest message, but nothing new was published yet.
* Return the previous message
*/
--generation;
}
// Compatible with normal and overflow conditions
if (!is_in_range(current_generation - _queue_size, generation, current_generation - 1)) {
// Reader is too far behind: some messages are lost
generation = current_generation - _queue_size;
}
memcpy(dst, _data + (_meta->o_size * (generation % _queue_size)), _meta->o_size);
ATOMIC_LEAVE;
++generation;
return true;
}
}
return false;
}
ssize_t
uORB::DeviceNode::read(cdev::file_t *filp, char *buffer, size_t buflen)
{

52
platforms/common/uORB/uORBDeviceNode.hpp
View File

@ -225,7 +225,45 @@ public:
* @return bool
* Returns true if the data was copied.
*/
bool copy(void *dst, unsigned &generation);
bool copy(void *dst, unsigned &generation)
{
if ((dst != nullptr) && (_data != nullptr)) {
if (_queue_size == 1) {
ATOMIC_ENTER;
memcpy(dst, _data, _meta->o_size);
generation = _generation.load();
ATOMIC_LEAVE;
return true;
} else {
ATOMIC_ENTER;
const unsigned current_generation = _generation.load();
if (current_generation == generation) {
/* The subscriber already read the latest message, but nothing new was published yet.
* Return the previous message
*/
--generation;
}
// Compatible with normal and overflow conditions
if (!is_in_range(current_generation - _queue_size, generation, current_generation - 1)) {
// Reader is too far behind: some messages are lost
generation = current_generation - _queue_size;
}
memcpy(dst, _data + (_meta->o_size * (generation % _queue_size)), _meta->o_size);
ATOMIC_LEAVE;
++generation;
return true;
}
}
return false;
}
// add item to list of work items to schedule on node update
bool register_callback(SubscriptionCallback *callback_sub);
@ -253,4 +291,16 @@ private:
bool _advertised{false}; /**< has ever been advertised (not necessarily published data yet) */
uint8_t _queue_size; /**< maximum number of elements in the queue */
int8_t _subscriber_count{0};
// Determine the data range
static inline bool is_in_range(unsigned left, unsigned value, unsigned right)
{
if (right > left) {
return (left <= value) && (value <= right);
} else { // Maybe the data overflowed and a wraparound occurred
return (left <= value) || (value <= right);
}
}
};