From c4ed97f3c16311601b4983360d51f662729e81d4 Mon Sep 17 00:00:00 2001 From: James Goppert Date: Thu, 5 Jun 2014 07:06:48 -0400 Subject: Added uORB tiny pub/sub for usage without struct on stack. --- src/drivers/roboclaw/RoboClaw.cpp | 2 +- src/modules/controllib/block/Block.cpp | 4 +- src/modules/controllib/block/Block.hpp | 12 +- src/modules/controllib/uorb/blocks.cpp | 18 +- src/modules/mc_att_control/mc_att_control_main.cpp | 207 ++++----------------- src/modules/uORB/Publication.cpp | 6 +- src/modules/uORB/Publication.hpp | 96 ++++++++-- src/modules/uORB/Subscription.cpp | 16 +- src/modules/uORB/Subscription.hpp | 110 ++++++++--- 9 files changed, 232 insertions(+), 239 deletions(-) diff --git a/src/drivers/roboclaw/RoboClaw.cpp b/src/drivers/roboclaw/RoboClaw.cpp index dd5e4d3e0..280f6c831 100644 --- a/src/drivers/roboclaw/RoboClaw.cpp +++ b/src/drivers/roboclaw/RoboClaw.cpp @@ -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), diff --git a/src/modules/controllib/block/Block.cpp b/src/modules/controllib/block/Block.cpp index 6768bfa7e..f3cd87728 100644 --- a/src/modules/controllib/block/Block.cpp +++ b/src/modules/controllib/block/Block.cpp @@ -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) { diff --git a/src/modules/controllib/block/Block.hpp b/src/modules/controllib/block/Block.hpp index 736698e21..42ca3a742 100644 --- a/src/modules/controllib/block/Block.hpp +++ b/src/modules/controllib/block/Block.hpp @@ -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 & getSubscriptions() { return _subscriptions; } - List & getPublications() { return _publications; } + List & getSubscriptions() { return _subscriptions; } + List & getPublications() { return _publications; } List & getParams() { return _params; } // attributes const char *_name; SuperBlock *_parent; float _dt; - List _subscriptions; - List _publications; + List _subscriptions; + List _publications; List _params; }; diff --git a/src/modules/controllib/uorb/blocks.cpp b/src/modules/controllib/uorb/blocks.cpp index e8fecef0d..454d0db19 100644 --- a/src/modules/controllib/uorb/blocks.cpp +++ b/src/modules/controllib/uorb/blocks.cpp @@ -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()) { } diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp index 20e016da3..b024589a6 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -60,6 +60,8 @@ #include #include #include +#include +#include #include #include #include @@ -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, ¶m_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); } } diff --git a/src/modules/uORB/Publication.cpp b/src/modules/uORB/Publication.cpp index 5a5981617..e9398a625 100644 --- a/src/modules/uORB/Publication.cpp +++ b/src/modules/uORB/Publication.cpp @@ -52,10 +52,10 @@ namespace uORB { template Publication::Publication( - List * list, - const struct orb_metadata *meta) : + const struct orb_metadata *meta, + List * list) : T(), // initialize data structure to zero - PublicationBase(list, meta) { + PublicationNode(meta, list) { } template diff --git a/src/modules/uORB/Publication.hpp b/src/modules/uORB/Publication.hpp index 8650b3df8..1c48929e4 100644 --- a/src/modules/uORB/Publication.hpp +++ b/src/modules/uORB/Publication.hpp @@ -38,6 +38,8 @@ #pragma once +#include + #include #include @@ -49,55 +51,112 @@ namespace uORB * Base publication warapper class, used in list traversal * of various publications. */ -class __EXPORT PublicationBase : public ListNode +class __EXPORT PublicationBase { public: - PublicationBase( - List * 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 +{ +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 * 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 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 * list, - const struct orb_metadata *meta); + Publication(const struct orb_metadata *meta, + List * 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 diff --git a/src/modules/uORB/Subscription.cpp b/src/modules/uORB/Subscription.cpp index c1d1a938f..1a4867c85 100644 --- a/src/modules/uORB/Subscription.cpp +++ b/src/modules/uORB/Subscription.cpp @@ -55,21 +55,13 @@ namespace uORB { -bool __EXPORT SubscriptionBase::updated() -{ - bool isUpdated = false; - orb_check(_handle, &isUpdated); - return isUpdated; -} - template Subscription::Subscription( - List * list, - const struct orb_metadata *meta, unsigned interval) : + const struct orb_metadata *meta, + unsigned interval, + List * list) : T(), // initialize data structure to zero - SubscriptionBase(list, meta) { - setHandle(orb_subscribe(getMeta())); - orb_set_interval(getHandle(), interval); + SubscriptionNode(meta, interval, list) { } template diff --git a/src/modules/uORB/Subscription.hpp b/src/modules/uORB/Subscription.hpp index 34e9a83e0..bab1ef2ea 100644 --- a/src/modules/uORB/Subscription.hpp +++ b/src/modules/uORB/Subscription.hpp @@ -38,10 +38,11 @@ #pragma once +#include + #include #include - namespace uORB { @@ -49,8 +50,7 @@ namespace uORB * Base subscription warapper class, used in list traversal * of various subscriptions. */ -class __EXPORT SubscriptionBase : - public ListNode +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 * 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); } @@ -89,31 +108,80 @@ protected: int _handle; }; +/** + * alias class name so it is clear that the base class + */ +typedef SubscriptionBase SubscriptionTiny; + +/** + * The publication base class as a list node. + */ +class __EXPORT SubscriptionNode : + + public SubscriptionBase, + public ListNode +{ +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 pointer to a list of subscriptions + * that this should be appended to. + */ + SubscriptionNode(const struct orb_metadata *meta, + unsigned interval=0, + List * 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 __EXPORT Subscription : public T, // this must be first! - public SubscriptionBase + public 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 list interface for adding to + * list during construction */ - Subscription( - List * list, - const struct orb_metadata *meta, unsigned interval); + Subscription(const struct orb_metadata *meta, + unsigned interval=0, + List * 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 -- cgit v1.2.3 From 579ec36c28d4cbf08d93531853b2b8fd3f3461ac Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 25 Nov 2014 15:56:05 +0100 Subject: fix order of arguments (merge fix) --- src/modules/fw_pos_control_l1/mtecs/mTecs.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp index bffeefc1f..ecdab2936 100644 --- a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp +++ b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp @@ -52,7 +52,7 @@ mTecs::mTecs() : _mTecsEnabled(this, "ENABLED"), _airspeedMin(this, "FW_AIRSPD_MIN", false), /* Publications */ - _status(&getPublications(), ORB_ID(tecs_status)), + _status(ORB_ID(tecs_status), &getPublications()), /* control blocks */ _controlTotalEnergy(this, "THR"), _controlEnergyDistribution(this, "PIT", true), -- cgit v1.2.3