fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-17 19:23:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

Added uORB tiny pub/sub for usage without struct on stack.

Browse Source
This commit is contained in:
James Goppert
2014-06-05 10:49:31 -04:00
parent 5624c1406a
commit c4ed97f3c1
9 changed files with 236 additions and 243 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/roboclaw/RoboClaw.cpp
+1 -1
View File
@@ -65,7 +65,7 @@ RoboClaw::RoboClaw(const char *deviceName, uint16_t address,
_pulsesPerRev(pulsesPerRev),
_uart(0),
_controlPoll(),
_actuators(NULL, ORB_ID(actuator_controls_0), 20),
_actuators(ORB_ID(actuator_controls_0), 20),
_motor1Position(0),
_motor1Speed(0),
_motor1Overflow(0),
src/modules/controllib/block/Block.cpp
+2 -2
View File
@@ -101,7 +101,7 @@ void Block::updateParams()
void Block::updateSubscriptions()
{
uORB::SubscriptionBase *sub = getSubscriptions().getHead();
uORB::SubscriptionNode *sub = getSubscriptions().getHead();
int count = 0;
while (sub != NULL) {
@@ -119,7 +119,7 @@ void Block::updateSubscriptions()
void Block::updatePublications()
{
uORB::PublicationBase *pub = getPublications().getHead();
uORB::PublicationNode *pub = getPublications().getHead();
int count = 0;
while (pub != NULL) {
src/modules/controllib/block/Block.hpp
+6 -6
View File
@@ -46,8 +46,8 @@
// forward declaration
namespace uORB {
class SubscriptionBase;
class PublicationBase;
class SubscriptionNode;
class PublicationNode;
}
namespace control
@@ -83,15 +83,15 @@ public:
protected:
// accessors
SuperBlock *getParent() { return _parent; }
List<uORB::SubscriptionBase *> & getSubscriptions() { return _subscriptions; }
List<uORB::PublicationBase *> & getPublications() { return _publications; }
List<uORB::SubscriptionNode *> & getSubscriptions() { return _subscriptions; }
List<uORB::PublicationNode *> & getPublications() { return _publications; }
List<BlockParamBase *> & getParams() { return _params; }
// attributes
const char *_name;
SuperBlock *_parent;
float _dt;
List<uORB::SubscriptionBase *> _subscriptions;
List<uORB::PublicationBase *> _publications;
List<uORB::SubscriptionNode *> _subscriptions;
List<uORB::PublicationNode *> _publications;
List<BlockParamBase *> _params;
};
src/modules/controllib/uorb/blocks.cpp
+9 -9
View File
@@ -82,16 +82,16 @@ void BlockWaypointGuidance::update(vehicle_global_position_s &pos,
BlockUorbEnabledAutopilot::BlockUorbEnabledAutopilot(SuperBlock *parent, const char *name) :
SuperBlock(parent, name),
// subscriptions
_att(&getSubscriptions(), ORB_ID(vehicle_attitude), 20),
_attCmd(&getSubscriptions(), ORB_ID(vehicle_attitude_setpoint), 20),
_ratesCmd(&getSubscriptions(), ORB_ID(vehicle_rates_setpoint), 20),
_pos(&getSubscriptions() , ORB_ID(vehicle_global_position), 20),
_missionCmd(&getSubscriptions(), ORB_ID(position_setpoint_triplet), 20),
_manual(&getSubscriptions(), ORB_ID(manual_control_setpoint), 20),
_status(&getSubscriptions(), ORB_ID(vehicle_status), 20),
_param_update(&getSubscriptions(), ORB_ID(parameter_update), 1000), // limit to 1 Hz
_att(ORB_ID(vehicle_attitude), 20, &getSubscriptions()),
_attCmd(ORB_ID(vehicle_attitude_setpoint), 20, &getSubscriptions()),
_ratesCmd(ORB_ID(vehicle_rates_setpoint), 20, &getSubscriptions()),
_pos(ORB_ID(vehicle_global_position), 20, &getSubscriptions()),
_missionCmd(ORB_ID(position_setpoint_triplet), 20, &getSubscriptions()),
_manual(ORB_ID(manual_control_setpoint), 20, &getSubscriptions()),
_status(ORB_ID(vehicle_status), 20, &getSubscriptions()),
_param_update(ORB_ID(parameter_update), 1000, &getSubscriptions()), // limit to 1 Hz
// publications
_actuators(&getPublications(), ORB_ID(actuator_controls_0))
_actuators(ORB_ID(actuator_controls_0), &getPublications())
{
}
src/modules/mc_att_control/mc_att_control_main.cpp
+37 -170
View File
@@ -60,6 +60,8 @@
#include <drivers/drv_hrt.h>
#include <arch/board/board.h>
#include <uORB/uORB.h>
#include <uORB/Subscription.hpp>
#include <uORB/Publication.hpp>
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/actuator_controls.h>
@@ -111,17 +113,17 @@ private:
bool _task_should_exit; /**< if true, sensor task should exit */
int _control_task; /**< task handle for sensor task */
int _v_att_sub; /**< vehicle attitude subscription */
int _v_att_sp_sub; /**< vehicle attitude setpoint subscription */
int _v_rates_sp_sub; /**< vehicle rates setpoint subscription */
int _v_control_mode_sub; /**< vehicle control mode subscription */
int _params_sub; /**< parameter updates subscription */
int _manual_control_sp_sub; /**< manual control setpoint subscription */
int _armed_sub; /**< arming status subscription */
uORB::SubscriptionTiny _v_att_sub; /**< vehicle attitude subscription */
uORB::SubscriptionTiny _v_att_sp_sub; /**< vehicle attitude setpoint subscription */
uORB::SubscriptionTiny _v_rates_sp_sub; /**< vehicle rates setpoint subscription */
uORB::SubscriptionTiny _v_control_mode_sub; /**< vehicle control mode subscription */
uORB::SubscriptionTiny _params_sub; /**< parameter updates subscription */
uORB::SubscriptionTiny _manual_control_sp_sub; /**< manual control setpoint subscription */
uORB::SubscriptionTiny _armed_sub; /**< arming status subscription */
orb_advert_t _att_sp_pub; /**< attitude setpoint publication */
orb_advert_t _v_rates_sp_pub; /**< rate setpoint publication */
orb_advert_t _actuators_0_pub; /**< attitude actuator controls publication */
uORB::PublicationTiny _att_sp_pub; /**< attitude setpoint publication */
uORB::PublicationTiny _v_rates_sp_pub; /**< rate setpoint publication */
uORB::PublicationTiny _actuators_0_pub; /**< attitude actuator controls publication */
struct vehicle_attitude_s _v_att; /**< vehicle attitude */
struct vehicle_attitude_setpoint_s _v_att_sp; /**< vehicle attitude setpoint */
@@ -186,36 +188,6 @@ private:
*/
int parameters_update();
/**
* Check for parameter update and handle it.
*/
void parameter_update_poll();
/**
* Check for changes in vehicle control mode.
*/
void vehicle_control_mode_poll();
/**
* Check for changes in manual inputs.
*/
void vehicle_manual_poll();
/**
* Check for attitude setpoint updates.
*/
void vehicle_attitude_setpoint_poll();
/**
* Check for rates setpoint updates.
*/
void vehicle_rates_setpoint_poll();
/**
* Check for arming status updates.
*/
void arming_status_poll();
/**
* Attitude controller.
*/
@@ -255,17 +227,18 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() :
_control_task(-1),
/* subscriptions */
_v_att_sub(-1),
_v_att_sp_sub(-1),
_v_control_mode_sub(-1),
_params_sub(-1),
_manual_control_sp_sub(-1),
_armed_sub(-1),
_v_att_sub(ORB_ID(vehicle_attitude)),
_v_att_sp_sub(ORB_ID(vehicle_attitude_setpoint)),
_v_rates_sp_sub(ORB_ID(vehicle_rates_setpoint)),
_v_control_mode_sub(ORB_ID(vehicle_control_mode)),
_params_sub(ORB_ID(parameter_update)),
_manual_control_sp_sub(ORB_ID(manual_control_setpoint)),
_armed_sub(ORB_ID(actuator_armed)),
/* publications */
_att_sp_pub(-1),
_v_rates_sp_pub(-1),
_actuators_0_pub(-1),
_att_sp_pub(ORB_ID(vehicle_attitude_setpoint)),
_v_rates_sp_pub(ORB_ID(vehicle_rates_setpoint)),
_actuators_0_pub(ORB_ID_VEHICLE_ATTITUDE_CONTROLS),
/* performance counters */
_loop_perf(perf_alloc(PC_ELAPSED, "mc_att_control"))
@@ -405,83 +378,6 @@ MulticopterAttitudeControl::parameters_update()
return OK;
}
void
MulticopterAttitudeControl::parameter_update_poll()
{
bool updated;
/* Check HIL state if vehicle status has changed */
orb_check(_params_sub, &updated);
if (updated) {
struct parameter_update_s param_update;
orb_copy(ORB_ID(parameter_update), _params_sub, &param_update);
parameters_update();
}
}
void
MulticopterAttitudeControl::vehicle_control_mode_poll()
{
bool updated;
/* Check HIL state if vehicle status has changed */
orb_check(_v_control_mode_sub, &updated);
if (updated) {
orb_copy(ORB_ID(vehicle_control_mode), _v_control_mode_sub, &_v_control_mode);
}
}
void
MulticopterAttitudeControl::vehicle_manual_poll()
{
bool updated;
/* get pilots inputs */
orb_check(_manual_control_sp_sub, &updated);
if (updated) {
orb_copy(ORB_ID(manual_control_setpoint), _manual_control_sp_sub, &_manual_control_sp);
}
}
void
MulticopterAttitudeControl::vehicle_attitude_setpoint_poll()
{
/* check if there is a new setpoint */
bool updated;
orb_check(_v_att_sp_sub, &updated);
if (updated) {
orb_copy(ORB_ID(vehicle_attitude_setpoint), _v_att_sp_sub, &_v_att_sp);
}
}
void
MulticopterAttitudeControl::vehicle_rates_setpoint_poll()
{
/* check if there is a new setpoint */
bool updated;
orb_check(_v_rates_sp_sub, &updated);
if (updated) {
orb_copy(ORB_ID(vehicle_rates_setpoint), _v_rates_sp_sub, &_v_rates_sp);
}
}
void
MulticopterAttitudeControl::arming_status_poll()
{
/* check if there is a new setpoint */
bool updated;
orb_check(_armed_sub, &updated);
if (updated) {
orb_copy(ORB_ID(actuator_armed), _armed_sub, &_armed);
}
}
/*
* Attitude controller.
* Input: 'manual_control_setpoint' and 'vehicle_attitude_setpoint' topics (depending on mode)
@@ -498,7 +394,7 @@ MulticopterAttitudeControl::control_attitude(float dt)
if (_v_control_mode.flag_control_velocity_enabled || _v_control_mode.flag_control_climb_rate_enabled) {
/* in assisted modes poll 'vehicle_attitude_setpoint' topic and modify it */
vehicle_attitude_setpoint_poll();
(void) _v_att_sp_sub.update(&_v_att_sp);
}
if (!_v_control_mode.flag_control_climb_rate_enabled) {
@@ -553,7 +449,7 @@ MulticopterAttitudeControl::control_attitude(float dt)
} else {
/* in non-manual mode use 'vehicle_attitude_setpoint' topic */
vehicle_attitude_setpoint_poll();
(void) _v_att_sp_sub.update(&_v_att_sp);
/* reset yaw setpoint after non-manual control mode */
_reset_yaw_sp = true;
@@ -581,12 +477,7 @@ MulticopterAttitudeControl::control_attitude(float dt)
if (publish_att_sp) {
_v_att_sp.timestamp = hrt_absolute_time();
if (_att_sp_pub > 0) {
orb_publish(ORB_ID(vehicle_attitude_setpoint), _att_sp_pub, &_v_att_sp);
} else {
_att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &_v_att_sp);
}
_att_sp_pub.update(&_v_att_sp);
}
/* rotation matrix for current state */
@@ -717,24 +608,13 @@ MulticopterAttitudeControl::task_main()
warnx("started");
fflush(stdout);
/*
* do subscriptions
*/
_v_att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
_v_rates_sp_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint));
_v_att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
_v_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
_params_sub = orb_subscribe(ORB_ID(parameter_update));
_manual_control_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
_armed_sub = orb_subscribe(ORB_ID(actuator_armed));
/* initialize parameters cache */
parameters_update();
/* wakeup source: vehicle attitude */
struct pollfd fds[1];
fds[0].fd = _v_att_sub;
fds[0].fd = _v_att_sub.getHandle();
fds[0].events = POLLIN;
while (!_task_should_exit) {
@@ -771,13 +651,15 @@ MulticopterAttitudeControl::task_main()
}
/* copy attitude topic */
orb_copy(ORB_ID(vehicle_attitude), _v_att_sub, &_v_att);
_v_att_sub.update(&_v_att);
/* check for updates in other topics */
parameter_update_poll();
vehicle_control_mode_poll();
arming_status_poll();
vehicle_manual_poll();
if (_params_sub.updated()) {
parameters_update();
}
(void) _v_control_mode_sub.update(&_v_control_mode);
(void) _armed_sub.update(&_armed);
(void) _manual_control_sp_sub.update(&_manual_control_sp);
if (_v_control_mode.flag_control_attitude_enabled) {
control_attitude(dt);
@@ -789,12 +671,7 @@ MulticopterAttitudeControl::task_main()
_v_rates_sp.thrust = _thrust_sp;
_v_rates_sp.timestamp = hrt_absolute_time();
if (_v_rates_sp_pub > 0) {
orb_publish(ORB_ID(vehicle_rates_setpoint), _v_rates_sp_pub, &_v_rates_sp);
} else {
_v_rates_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &_v_rates_sp);
}
_v_rates_sp_pub.update(&_v_rates_sp);
} else {
/* attitude controller disabled, poll rates setpoint topic */
@@ -813,16 +690,11 @@ MulticopterAttitudeControl::task_main()
_v_rates_sp.thrust = _thrust_sp;
_v_rates_sp.timestamp = hrt_absolute_time();
if (_v_rates_sp_pub > 0) {
orb_publish(ORB_ID(vehicle_rates_setpoint), _v_rates_sp_pub, &_v_rates_sp);
} else {
_v_rates_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &_v_rates_sp);
}
_v_rates_sp_pub.update(&_v_rates_sp);
} else {
/* attitude controller disabled, poll rates setpoint topic */
vehicle_rates_setpoint_poll();
_v_rates_sp_sub.update(&_v_rates_sp);
_rates_sp(0) = _v_rates_sp.roll;
_rates_sp(1) = _v_rates_sp.pitch;
_rates_sp(2) = _v_rates_sp.yaw;
@@ -840,12 +712,7 @@ MulticopterAttitudeControl::task_main()
_actuators.control[3] = (isfinite(_thrust_sp)) ? _thrust_sp : 0.0f;
_actuators.timestamp = hrt_absolute_time();
if (_actuators_0_pub > 0) {
orb_publish(ORB_ID(actuator_controls_0), _actuators_0_pub, &_actuators);
} else {
_actuators_0_pub = orb_advertise(ORB_ID(actuator_controls_0), &_actuators);
}
_actuators_0_pub.update(&_actuators);
}
}
src/modules/uORB/Publication.cpp
+3 -3
View File
@@ -52,10 +52,10 @@ namespace uORB {
template<class T>
Publication<T>::Publication(
List<PublicationBase *> * list,
const struct orb_metadata *meta) :
const struct orb_metadata *meta,
List<PublicationNode *> * list) :
T(), // initialize data structure to zero
PublicationBase(list, meta) {
PublicationNode(meta, list) {
}
template<class T>
src/modules/uORB/Publication.hpp
+81 -15
View File
@@ -38,6 +38,8 @@
#pragma once
#include <assert.h>
#include <uORB/uORB.h>
#include <containers/List.hpp>
@@ -49,55 +51,112 @@ namespace uORB
* Base publication warapper class, used in list traversal
* of various publications.
*/
class __EXPORT PublicationBase : public ListNode<uORB::PublicationBase *>
class __EXPORT PublicationBase
{
public:
PublicationBase(
List<PublicationBase *> * list,
const struct orb_metadata *meta) :
/**
* Constructor
*
*
* @param meta The uORB metadata (usually from the ORB_ID()
* macro) for the topic.
*/
PublicationBase(const struct orb_metadata *meta) :
_meta(meta),
_handle(-1) {
if (list != NULL) list->add(this);
}
void update() {
/**
* Update the struct
* @param data The uORB message struct we are updating.
*/
void update(void * data) {
if (_handle > 0) {
orb_publish(getMeta(), getHandle(), getDataVoidPtr());
orb_publish(getMeta(), getHandle(), data);
} else {
setHandle(orb_advertise(getMeta(), getDataVoidPtr()));
setHandle(orb_advertise(getMeta(), data));
}
}
virtual void *getDataVoidPtr() = 0;
/**
* Deconstructor
*/
virtual ~PublicationBase() {
orb_unsubscribe(getHandle());
}
// accessors
const struct orb_metadata *getMeta() { return _meta; }
int getHandle() { return _handle; }
protected:
// accessors
void setHandle(orb_advert_t handle) { _handle = handle; }
// attributes
const struct orb_metadata *_meta;
orb_advert_t _handle;
};
/**
* alias class name so it is clear that the base class
* can be used by itself if desired
*/
typedef PublicationBase PublicationTiny;
/**
* The publication base class as a list node.
*/
class __EXPORT PublicationNode :
public PublicationBase,
public ListNode<PublicationNode *>
{
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.
*/
PublicationNode(const struct orb_metadata *meta,
List<PublicationNode *> * list=nullptr) :
PublicationBase(meta) {
if (list != nullptr) list->add(this);
}
/**
* This function is the callback for list traversal
* updates, a child class must implement it.
*/
virtual void update() = 0;
};
/**
* Publication wrapper class
*/
template<class T>
class Publication :
public T, // this must be first!
public PublicationBase
public PublicationNode
{
public:
/**
* Constructor
*
* @param list A list interface for adding to list during construction
* @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 list A list interface for adding to
* list during construction
*/
Publication(List<PublicationBase *> * list,
const struct orb_metadata *meta);
Publication(const struct orb_metadata *meta,
List<PublicationNode *> * list=nullptr);
/**
* Deconstructor
**/
virtual ~Publication();
/*
* XXX
* This function gets the T struct, assuming
@@ -106,6 +165,13 @@ public:
* seem to be available
*/
void *getDataVoidPtr();
/**
* Create an update function that uses the embedded struct.
*/
void update() {
PublicationBase::update(getDataVoidPtr());
}
};
} // namespace uORB
src/modules/uORB/Subscription.cpp
+4 -12
View File
@@ -55,21 +55,13 @@
namespace uORB
{
bool __EXPORT SubscriptionBase::updated()
{
bool isUpdated = false;
orb_check(_handle, &isUpdated);
return isUpdated;
}
template<class T>
Subscription<T>::Subscription(
List<SubscriptionBase *> * list,
const struct orb_metadata *meta, unsigned interval) :
const struct orb_metadata *meta,
unsigned interval,
List<SubscriptionNode *> * list) :
T(), // initialize data structure to zero
SubscriptionBase(list, meta) {
setHandle(orb_subscribe(getMeta()));
orb_set_interval(getHandle(), interval);
SubscriptionNode(meta, interval, list) {
}
template<class T>
src/modules/uORB/Subscription.hpp
+93 -25
View File
@@ -38,10 +38,11 @@
#pragma once
#include <assert.h>
#include <uORB/uORB.h>
#include <containers/List.hpp>
namespace uORB
{
@@ -49,8 +50,7 @@ namespace uORB
* Base subscription warapper class, used in list traversal
* of various subscriptions.
*/
class __EXPORT SubscriptionBase :
public ListNode<SubscriptionBase *>
class __EXPORT SubscriptionBase
{
public:
// methods
@@ -58,23 +58,42 @@ 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 interval The minimum interval in milliseconds
* between updates
*/
SubscriptionBase(
List<SubscriptionBase *> * list,
const struct orb_metadata *meta) :
SubscriptionBase(const struct orb_metadata *meta,
unsigned interval=0) :
_meta(meta),
_handle() {
if (list != NULL) list->add(this);
setHandle(orb_subscribe(getMeta()));
orb_set_interval(getHandle(), interval);
}
bool updated();
void update() {
/**
* Check if there is a new update.
* */
bool updated() {
bool isUpdated = false;
orb_check(_handle, &isUpdated);
return isUpdated;
}
/**
* Update the struct
* @param data The uORB message struct we are updating.
*/
void update(void * data) {
if (updated()) {
orb_copy(_meta, _handle, getDataVoidPtr());
orb_copy(_meta, _handle, data);
}
}
virtual void *getDataVoidPtr() = 0;
/**
* Deconstructor
*/
virtual ~SubscriptionBase() {
orb_unsubscribe(_handle);
}
@@ -90,30 +109,79 @@ protected:
};
/**
* Subscription wrapper class
* alias class name so it is clear that the base class
*/
template<class T>
class __EXPORT Subscription :
public T, // this must be first!
public SubscriptionBase
typedef SubscriptionBase SubscriptionTiny;
/**
* The publication base class as a list node.
*/
class __EXPORT SubscriptionNode :
public SubscriptionBase,
public ListNode<SubscriptionNode *>
{
public:
/**
* Constructor
*
* @param list A list interface for adding to list during construction
* @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 list A pointer to a list of subscriptions
* that this should be appended to.
*/
Subscription(
List<SubscriptionBase *> * list,
const struct orb_metadata *meta, unsigned interval);
SubscriptionNode(const struct orb_metadata *meta,
unsigned interval=0,
List<SubscriptionNode *> * list=nullptr) :
SubscriptionBase(meta, interval) {
if (list != nullptr) list->add(this);
}
/**
* This function is the callback for list traversal
* updates, a child class must implement it.
*/
virtual void update() = 0;
};
/**
* Subscription wrapper class
*/
template<class T>
class __EXPORT Subscription :
public T, // this must be first!
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,
List<SubscriptionNode *> * list=nullptr);
/**
* Deconstructor
*/
virtual ~Subscription();
/**
* Create an update function that uses the embedded struct.
*/
void update() {
SubscriptionBase::update(getDataVoidPtr());
}
/*
* XXX
* This function gets the T struct, assuming