fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-06-28 22:50:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merge pull request #3030 from dronecrew/control-update

Browse Source 
uORB/ control wrapper update
This commit is contained in:
Lorenz Meier
2015-10-22 00:22:58 +02:00
parent 8fa49d8ac5 f0ad724fda
commit c6b41b4e5e
13 changed files with 314 additions and 222 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/uORB/Publication.cpp
+41 -14
View File
@@ -50,37 +50,63 @@
#include "topics/encoders.h"
#include "topics/tecs_status.h"
#include "topics/rc_channels.h"
#include "topics/filtered_bottom_flow.h"
#include <px4_defines.h>
namespace uORB
{
template<class T>
Publication<T>::Publication(
const struct orb_metadata *meta,
List<PublicationNode *> *list) :
T(), // initialize data structure to zero
PublicationNode(meta, list)
PublicationBase::PublicationBase(const struct orb_metadata *meta,
int priority) :
_meta(meta),
_priority(priority),
_instance(),
_handle(nullptr)
{
}
template<class T>
Publication<T>::~Publication() {}
template<class T>
void *Publication<T>::getDataVoidPtr()
void PublicationBase::update(void *data)
{
return (void *)(T *)(this);
if (_handle != nullptr) {
int ret = orb_publish(getMeta(), getHandle(), data);
if (ret != PX4_OK) { warnx("publish fail"); }
} else {
orb_advert_t handle;
if (_priority > 0) {
handle = orb_advertise_multi(
getMeta(), data,
&_instance, _priority);
} else {
handle = orb_advertise(getMeta(), data);
}
if (int64_t(handle) != PX4_ERROR) {
setHandle(handle);
} else {
warnx("advert fail");
}
}
}
PublicationBase::~PublicationBase()
{
}
PublicationNode::PublicationNode(const struct orb_metadata *meta,
int priority,
List<PublicationNode *> *list) :
PublicationBase(meta)
PublicationBase(meta, priority)
{
if (list != nullptr) { list->add(this); }
}
// explicit template instantiation
template class __EXPORT Publication<vehicle_attitude_s>;
template class __EXPORT Publication<vehicle_local_position_s>;
template class __EXPORT Publication<vehicle_global_position_s>;
@@ -94,5 +120,6 @@ template class __EXPORT Publication<actuator_direct_s>;
template class __EXPORT Publication<encoders_s>;
template class __EXPORT Publication<tecs_status_s>;
template class __EXPORT Publication<rc_channels_s>;
template class __EXPORT Publication<filtered_bottom_flow_s>;
}
src/modules/uORB/Publication.hpp
+37 -43
View File
@@ -42,13 +42,13 @@
#include <uORB/uORB.h>
#include <containers/List.hpp>
#include <systemlib/err.h>
namespace uORB
{
/**
* Base publication warapper class, used in list traversal
* Base publication wrapper class, used in list traversal
* of various publications.
*/
class __EXPORT PublicationBase
@@ -58,50 +58,40 @@ public:
/**
* Constructor
*
*
* @param meta The uORB metadata (usually from the ORB_ID()
* macro) for the topic.
* @param meta The uORB metadata (usually from
* the ORB_ID() macro) for the topic.
* @param priority The priority for multi pub/sub, 0-based, -1 means
* don't publish as multi
*/
PublicationBase(const struct orb_metadata *meta) :
_meta(meta),
_handle(nullptr)
{
}
PublicationBase(const struct orb_metadata *meta,
int priority = -1);
/**
* Update the struct
* @param data The uORB message struct we are updating.
*/
void update(void *data)
{
if (_handle != nullptr) {
orb_publish(getMeta(), getHandle(), data);
} else {
setHandle(orb_advertise(getMeta(), data));
}
}
void update(void *data);
/**
* Deconstructor
*/
virtual ~PublicationBase()
{
}
virtual ~PublicationBase();
// accessors
const struct orb_metadata *getMeta() { return _meta; }
orb_advert_t getHandle() { return _handle; }
protected:
// disallow copy
PublicationBase(const PublicationBase &other);
// disallow assignment
PublicationBase &operator=(const PublicationBase &other);
// accessors
void setHandle(orb_advert_t handle) { _handle = handle; }
// attributes
const struct orb_metadata *_meta;
int _priority;
int _instance;
orb_advert_t _handle;
private:
// forbid copy
PublicationBase(const PublicationBase &) : _meta(), _handle() {};
// forbid assignment
PublicationBase &operator = (const PublicationBase &);
};
/**
@@ -121,13 +111,14 @@ public:
/**
* Constructor
*
*
* @param meta The uORB metadata (usually from the ORB_ID()
* macro) for the topic.
* @param list A pointer to a list of subscriptions
* that this should be appended to.
* @param meta The uORB metadata (usually from
* the ORB_ID() macro) for the topic.
* @param priority The priority for multi pub, 0-based.
* @param list A list interface for adding to
* list during construction
*/
PublicationNode(const struct orb_metadata *meta,
int priority = -1,
List<PublicationNode *> *list = nullptr);
/**
@@ -142,7 +133,6 @@ public:
*/
template<class T>
class __EXPORT Publication :
public T, // this must be first!
public PublicationNode
{
public:
@@ -151,33 +141,37 @@ public:
*
* @param meta The uORB metadata (usually from
* the ORB_ID() macro) for the topic.
* @param priority The priority for multi pub, 0-based.
* @param list A list interface for adding to
* list during construction
*/
Publication(const struct orb_metadata *meta,
List<PublicationNode *> *list = nullptr);
int priority = -1,
List<PublicationNode *> *list = nullptr) :
PublicationNode(meta, priority, list),
_data()
{
}
/**
* Deconstructor
**/
virtual ~Publication();
virtual ~Publication() {};
/*
* XXX
* This function gets the T struct, assuming
* the struct is the first base class, this
* should use dynamic cast, but doesn't
* seem to be available
*/
void *getDataVoidPtr();
* This function gets the T struct
* */
T &get() { return _data; }
/**
* Create an update function that uses the embedded struct.
*/
void update()
{
PublicationBase::update(getDataVoidPtr());
PublicationBase::update((void *)(&_data));
}
private:
T _data;
};
} // namespace uORB
src/modules/uORB/Subscription.cpp
+74 -16
View File
@@ -54,37 +54,89 @@
#include "topics/vehicle_attitude_setpoint.h"
#include "topics/vehicle_rates_setpoint.h"
#include "topics/rc_channels.h"
#include "topics/battery_status.h"
#include "topics/optical_flow.h"
#include "topics/distance_sensor.h"
#include "topics/home_position.h"
#include "topics/vehicle_control_mode.h"
#include "topics/actuator_armed.h"
#include "topics/att_pos_mocap.h"
#include "topics/vision_position_estimate.h"
#include <px4_defines.h>
namespace uORB
{
template<class T>
Subscription<T>::Subscription(
const struct orb_metadata *meta,
unsigned interval,
List<SubscriptionNode *> *list) :
T(), // initialize data structure to zero
SubscriptionNode(meta, interval, list)
SubscriptionBase::SubscriptionBase(const struct orb_metadata *meta,
unsigned interval, unsigned instance) :
_meta(meta),
_instance(instance),
_handle()
{
if (_instance > 0) {
_handle = orb_subscribe_multi(
getMeta(), instance);
} else {
_handle = orb_subscribe(getMeta());
}
if (_handle < 0) { warnx("sub failed"); }
orb_set_interval(getHandle(), interval);
}
bool SubscriptionBase::updated()
{
bool isUpdated = false;
int ret = orb_check(_handle, &isUpdated);
if (ret != PX4_OK) { warnx("orb check failed"); }
return isUpdated;
}
void SubscriptionBase::update(void *data)
{
if (updated()) {
int ret = orb_copy(_meta, _handle, data);
if (ret != PX4_OK) { warnx("orb copy failed"); }
}
}
SubscriptionBase::~SubscriptionBase()
{
int ret = orb_unsubscribe(_handle);
if (ret != PX4_OK) { warnx("orb unsubscribe failed"); }
}
template <class T>
Subscription<T>::Subscription(const struct orb_metadata *meta,
unsigned interval,
int instance,
List<SubscriptionNode *> *list) :
SubscriptionNode(meta, interval, instance, list),
_data() // initialize data structure to zero
{
}
template<class T>
Subscription<T>::~Subscription() {}
template<class T>
void *Subscription<T>::getDataVoidPtr()
template <class T>
Subscription<T>::~Subscription()
{
return (void *)(T *)(this);
}
template<class T>
T Subscription<T>::getData()
template <class T>
void Subscription<T>::update()
{
return T(*this);
SubscriptionBase::update((void *)(&_data));
}
template <class T>
const T &Subscription<T>::get() { return _data; }
template class __EXPORT Subscription<parameter_update_s>;
template class __EXPORT Subscription<actuator_controls_s>;
template class __EXPORT Subscription<vehicle_gps_position_s>;
@@ -104,5 +156,11 @@ template class __EXPORT Subscription<vehicle_rates_setpoint_s>;
template class __EXPORT Subscription<rc_channels_s>;
template class __EXPORT Subscription<vehicle_control_mode_s>;
template class __EXPORT Subscription<actuator_armed_s>;
template class __EXPORT Subscription<battery_status_s>;
template class __EXPORT Subscription<home_position_s>;
template class __EXPORT Subscription<optical_flow_s>;
template class __EXPORT Subscription<distance_sensor_s>;
template class __EXPORT Subscription<att_pos_mocap_s>;
template class __EXPORT Subscription<vision_position_estimate_s>;
} // namespace uORB
src/modules/uORB/Subscription.hpp
+23 -43
View File
@@ -42,6 +42,7 @@
#include <uORB/uORB.h>
#include <containers/List.hpp>
#include <systemlib/err.h>
namespace uORB
{
@@ -60,47 +61,29 @@ public:
*
* @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.
*/
SubscriptionBase(const struct orb_metadata *meta,
unsigned interval = 0) :
_meta(meta),
_handle()
{
setHandle(orb_subscribe(getMeta()));
orb_set_interval(getHandle(), interval);
}
unsigned interval = 0, unsigned instance = 0);
/**
* Check if there is a new update.
* */
bool updated()
{
bool isUpdated = false;
orb_check(_handle, &isUpdated);
return isUpdated;
}
bool updated();
/**
* Update the struct
* @param data The uORB message struct we are updating.
*/
void update(void *data)
{
if (updated()) {
orb_copy(_meta, _handle, data);
}
}
void update(void *data);
/**
* Deconstructor
*/
virtual ~SubscriptionBase()
{
orb_unsubscribe(_handle);
}
virtual ~SubscriptionBase();
// accessors
const struct orb_metadata *getMeta() { return _meta; }
int getHandle() { return _handle; }
@@ -109,12 +92,13 @@ protected:
void setHandle(int handle) { _handle = handle; }
// attributes
const struct orb_metadata *_meta;
int _instance;
int _handle;
private:
// forbid copy
// disallow copy
SubscriptionBase(const SubscriptionBase &other);
// forbid assignment
SubscriptionBase &operator = (const SubscriptionBase &);
// disallow assignment
SubscriptionBase &operator=(const SubscriptionBase &other);
};
/**
@@ -123,7 +107,7 @@ private:
typedef SubscriptionBase SubscriptionTiny;
/**
* The publication base class as a list node.
* The subscription base class as a list node.
*/
class __EXPORT SubscriptionNode :
@@ -134,18 +118,19 @@ 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.
* @param list A pointer to a list of subscriptions
* that this should be appended to.
*/
SubscriptionNode(const struct orb_metadata *meta,
unsigned interval = 0,
int instance = 0,
List<SubscriptionNode *> *list = nullptr) :
SubscriptionBase(meta, interval),
SubscriptionBase(meta, interval, instance),
_interval(interval)
{
if (list != nullptr) { list->add(this); }
@@ -169,7 +154,6 @@ protected:
*/
template<class T>
class __EXPORT Subscription :
public T, // this must be first!
public SubscriptionNode
{
public:
@@ -185,7 +169,9 @@ public:
*/
Subscription(const struct orb_metadata *meta,
unsigned interval = 0,
int instance = 0,
List<SubscriptionNode *> *list = nullptr);
/**
* Deconstructor
*/
@@ -195,20 +181,14 @@ public:
/**
* Create an update function that uses the embedded struct.
*/
void update()
{
SubscriptionBase::update(getDataVoidPtr());
}
void update();
/*
* XXX
* This function gets the T struct, assuming
* the struct is the first base class, this
* should use dynamic cast, but doesn't
* seem to be available
*/
void *getDataVoidPtr();
T getData();
* This function gets the T struct data
* */
const T &get();
private:
T _data;
};
} // namespace uORB