mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-22 14:03:34 +08:00
uORB::Subscription subscribe directly to uORB device node object
This commit is contained in:
Daniel Agar
+150
-105
@@ -1,6 +1,6 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2012-2017 PX4 Development Team. All rights reserved.
|
||||
* Copyright (c) 2012-2019 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
|
||||
@@ -39,159 +39,204 @@
|
||||
#pragma once
|
||||
|
||||
#include <uORB/uORB.h>
|
||||
#include <containers/List.hpp>
|
||||
#include <systemlib/err.h>
|
||||
#include <px4_defines.h>
|
||||
|
||||
#include "uORBDeviceNode.hpp"
|
||||
#include "uORBManager.hpp"
|
||||
#include "uORBUtils.hpp"
|
||||
|
||||
namespace uORB
|
||||
{
|
||||
|
||||
/**
|
||||
* Base subscription wrapper class, used in list traversal
|
||||
* of various subscriptions.
|
||||
*/
|
||||
class __EXPORT SubscriptionBase
|
||||
// Base subscription wrapper class
|
||||
class Subscription
|
||||
{
|
||||
public:
|
||||
|
||||
/**
|
||||
* Constructor
|
||||
*
|
||||
* @param meta The uORB metadata (usually from the ORB_ID()
|
||||
* macro) for the topic.
|
||||
* @param interval The minimum interval in milliseconds
|
||||
* between updates
|
||||
* @param meta The uORB metadata (usually from the ORB_ID() macro) for the topic.
|
||||
* @param instance The instance for multi sub.
|
||||
*/
|
||||
SubscriptionBase(const struct orb_metadata *meta, unsigned interval = 0, unsigned instance = 0);
|
||||
virtual ~SubscriptionBase();
|
||||
Subscription(const orb_metadata *meta, uint8_t instance = 0) : _meta(meta), _instance(instance)
|
||||
{
|
||||
init();
|
||||
}
|
||||
|
||||
// no copy, assignment, move, move assignment
|
||||
SubscriptionBase(const SubscriptionBase &) = delete;
|
||||
SubscriptionBase &operator=(const SubscriptionBase &) = delete;
|
||||
SubscriptionBase(SubscriptionBase &&) = delete;
|
||||
SubscriptionBase &operator=(SubscriptionBase &&) = delete;
|
||||
virtual ~Subscription() { unsubscribe(); }
|
||||
|
||||
bool init();
|
||||
bool forceInit();
|
||||
|
||||
bool valid() const { return _node != nullptr; }
|
||||
bool published() { return valid() ? _node->is_published() : init(); }
|
||||
|
||||
/**
|
||||
* Check if there is a new update.
|
||||
* */
|
||||
bool updated();
|
||||
virtual bool updated() { return published() ? (_node->published_message_count() != _last_generation) : false; }
|
||||
|
||||
/**
|
||||
* Update the struct
|
||||
* @param data The uORB message struct we are updating.
|
||||
*/
|
||||
bool update(void *data);
|
||||
virtual bool update(void *dst) { return updated() ? copy(dst) : false; }
|
||||
|
||||
int getHandle() const { return _handle; }
|
||||
/**
|
||||
* Check if subscription updated based on timestamp.
|
||||
*
|
||||
* @return true only if topic was updated based on a timestamp and
|
||||
* copied to buffer successfully.
|
||||
* If topic was not updated since last check it will return false but
|
||||
* still copy the data.
|
||||
* If no data available data buffer will be filled with zeros.
|
||||
*/
|
||||
bool update(uint64_t *time, void *dst);
|
||||
|
||||
const orb_metadata *getMeta() const { return _meta; }
|
||||
/**
|
||||
* Copy the struct
|
||||
* @param data The uORB message struct we are updating.
|
||||
*/
|
||||
bool copy(void *dst) { return published() ? _node->copy(dst, _last_generation) : false; }
|
||||
|
||||
unsigned getInstance() const { return _instance; }
|
||||
hrt_abstime last_update() { return published() ? _node->last_update() : 0; }
|
||||
|
||||
uint8_t get_instance() const { return _instance; }
|
||||
orb_id_t get_topic() const { return _meta; }
|
||||
|
||||
protected:
|
||||
const struct orb_metadata *_meta;
|
||||
|
||||
unsigned _instance;
|
||||
bool subscribe();
|
||||
void unsubscribe();
|
||||
|
||||
int _handle{-1};
|
||||
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};
|
||||
};
|
||||
|
||||
/**
|
||||
* alias class name so it is clear that the base class
|
||||
*/
|
||||
typedef SubscriptionBase SubscriptionTiny;
|
||||
|
||||
/**
|
||||
* The subscription base class as a list node.
|
||||
*/
|
||||
class __EXPORT SubscriptionNode : public SubscriptionBase, public ListNode<SubscriptionNode *>
|
||||
// Subscription wrapper class with configured interval
|
||||
class SubscriptionInterval : public Subscription
|
||||
{
|
||||
public:
|
||||
/**
|
||||
* Constructor
|
||||
*
|
||||
* @param meta The uORB metadata (usually from the ORB_ID()
|
||||
* macro) for the topic.
|
||||
* @param interval The minimum interval in milliseconds
|
||||
* between updates
|
||||
* @param meta The uORB metadata (usually from the ORB_ID() macro) for the topic.
|
||||
* @param interval The minimum interval in milliseconds between updates
|
||||
* @param instance The instance for multi sub.
|
||||
* @param list A pointer to a list of subscriptions
|
||||
* that this should be appended to.
|
||||
*/
|
||||
SubscriptionNode(const struct orb_metadata *meta, unsigned interval = 0, unsigned instance = 0,
|
||||
List<SubscriptionNode *> *list = nullptr);
|
||||
SubscriptionInterval(const orb_metadata *meta, unsigned interval = 0, uint8_t instance = 0) :
|
||||
Subscription(meta, instance),
|
||||
_interval(interval)
|
||||
{}
|
||||
|
||||
virtual ~SubscriptionNode() override = default;
|
||||
virtual ~SubscriptionInterval() = default;
|
||||
|
||||
/**
|
||||
* This function is the callback for list traversal
|
||||
* updates, a child class must implement it.
|
||||
*/
|
||||
virtual bool update() = 0;
|
||||
|
||||
/**
|
||||
* Like update(), but does not check first if there is data available
|
||||
*/
|
||||
virtual bool forcedUpdate() = 0;
|
||||
|
||||
};
|
||||
|
||||
/**
|
||||
* Subscription wrapper class
|
||||
*/
|
||||
template<class T>
|
||||
class __EXPORT Subscription : public SubscriptionNode
|
||||
{
|
||||
public:
|
||||
/**
|
||||
* Constructor
|
||||
*
|
||||
* @param meta The uORB metadata (usually from
|
||||
* the ORB_ID() macro) for the topic.
|
||||
* @param interval The minimum interval in milliseconds
|
||||
* between updates
|
||||
* @param list A list interface for adding to
|
||||
* list during construction
|
||||
*/
|
||||
Subscription(const struct orb_metadata *meta, unsigned interval = 0, unsigned instance = 0,
|
||||
List<SubscriptionNode *> *list = nullptr):
|
||||
SubscriptionNode(meta, interval, instance, list),
|
||||
_data() // initialize data structure to zero
|
||||
bool updated() override
|
||||
{
|
||||
forcedUpdate();
|
||||
if (hrt_elapsed_time(&_last_update) >= (_interval * 1000)) {
|
||||
return Subscription::updated();
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
~Subscription() override = default;
|
||||
bool update(void *dst) override
|
||||
{
|
||||
if (updated()) {
|
||||
if (copy(dst)) {
|
||||
_last_update = hrt_absolute_time();
|
||||
return true;
|
||||
}
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
int get_interval() const { return _interval; }
|
||||
void set_interval(unsigned interval) { _interval = interval; }
|
||||
|
||||
protected:
|
||||
uint64_t _last_update{0}; // last update in microseconds
|
||||
unsigned _interval{0}; // interval in milliseconds
|
||||
};
|
||||
|
||||
// Subscription wrapper class with data
|
||||
template<class T>
|
||||
class SubscriptionData : public Subscription
|
||||
{
|
||||
public:
|
||||
/**
|
||||
* Constructor
|
||||
*
|
||||
* @param meta The uORB metadata (usually from the ORB_ID() macro) for the topic.
|
||||
* @param instance The instance for multi sub.
|
||||
*/
|
||||
SubscriptionData(const orb_metadata *meta, uint8_t instance = 0) :
|
||||
Subscription(meta, instance)
|
||||
{
|
||||
copy(&_data);
|
||||
}
|
||||
|
||||
virtual ~SubscriptionData() = default;
|
||||
|
||||
// no copy, assignment, move, move assignment
|
||||
Subscription(const Subscription &) = delete;
|
||||
Subscription &operator=(const Subscription &) = delete;
|
||||
Subscription(Subscription &&) = delete;
|
||||
Subscription &operator=(Subscription &&) = delete;
|
||||
SubscriptionData(const SubscriptionData &) = delete;
|
||||
SubscriptionData &operator=(const SubscriptionData &) = delete;
|
||||
SubscriptionData(SubscriptionData &&) = delete;
|
||||
SubscriptionData &operator=(SubscriptionData &&) = delete;
|
||||
|
||||
/**
|
||||
* Create an update function that uses the embedded struct.
|
||||
*/
|
||||
bool update() override
|
||||
{
|
||||
return SubscriptionBase::update((void *)(&_data));
|
||||
}
|
||||
// update the embedded struct.
|
||||
bool update() { return Subscription::update((void *)(&_data)); }
|
||||
|
||||
bool forcedUpdate() override
|
||||
{
|
||||
return orb_copy(_meta, _handle, &_data) == PX4_OK;
|
||||
}
|
||||
|
||||
/*
|
||||
* This function gets the T struct data
|
||||
* */
|
||||
const T &get() const
|
||||
{
|
||||
return _data;
|
||||
}
|
||||
const T &get() const { return _data; }
|
||||
|
||||
private:
|
||||
T _data;
|
||||
|
||||
T _data{};
|
||||
};
|
||||
|
||||
// Subscription wrapper class with data and configured interval
|
||||
template<class T>
|
||||
class SubscriptionIntervalData : public SubscriptionInterval
|
||||
{
|
||||
public:
|
||||
/**
|
||||
* Constructor
|
||||
*
|
||||
* @param meta The uORB metadata (usually from the ORB_ID() macro) for the topic.
|
||||
* @param interval The minimum interval in milliseconds between updates
|
||||
* @param instance The instance for multi sub.
|
||||
*/
|
||||
SubscriptionIntervalData(const orb_metadata *meta, unsigned interval = 0, uint8_t instance = 0) :
|
||||
SubscriptionInterval(meta, interval, instance)
|
||||
{
|
||||
copy(&_data);
|
||||
}
|
||||
|
||||
~SubscriptionIntervalData() override = default;
|
||||
|
||||
// no copy, assignment, move, move assignment
|
||||
SubscriptionIntervalData(const SubscriptionIntervalData &) = delete;
|
||||
SubscriptionIntervalData &operator=(const SubscriptionIntervalData &) = delete;
|
||||
SubscriptionIntervalData(SubscriptionIntervalData &&) = delete;
|
||||
SubscriptionIntervalData &operator=(SubscriptionIntervalData &&) = delete;
|
||||
|
||||
// update the embedded struct.
|
||||
bool update() { return SubscriptionInterval::update((void *)(&_data)); }
|
||||
|
||||
const T &get() const { return _data; }
|
||||
|
||||
private:
|
||||
T _data{};
|
||||
};
|
||||
|
||||
} // namespace uORB
|
||||
|
||||
Reference in New Issue
Block a user