uORB::Subscription subscribe directly to uORB device node object

This commit is contained in:
Daniel Agar
2019-06-03 17:06:21 -04:00
parent 2d1c60bc85
commit 2c63e335e9
37 changed files with 766 additions and 270 deletions
@@ -77,7 +77,7 @@ private:
orb_advert_t _constraints_pub{nullptr}; /**< constraints publication */
orb_advert_t _mavlink_log_pub{nullptr}; /**< Mavlink log uORB handle */
uORB::Subscription<obstacle_distance_s> *_sub_obstacle_distance{nullptr}; /**< obstacle distances received form a range sensor */
uORB::SubscriptionPollable<obstacle_distance_s> *_sub_obstacle_distance{nullptr}; /**< obstacle distances received form a range sensor */
static constexpr uint64_t RANGE_STREAM_TIMEOUT_US = 500000;
static constexpr uint64_t MESSAGE_THROTTLE_US = 5000000;
@@ -98,7 +98,7 @@ protected:
matrix::Vector3f _next_wp{}; /**< The next waypoint after target (local frame). If no next setpoint is available, next is set to target. */
float _mc_cruise_speed{0.0f}; /**< Requested cruise speed. If not valid, default cruise speed is used. */
WaypointType _type{WaypointType::idle}; /**< Type of current target triplet. */
uORB::Subscription<home_position_s> *_sub_home_position{nullptr};
uORB::SubscriptionPollable<home_position_s> *_sub_home_position{nullptr};
State _current_state{State::none};
float _target_acceptance_radius = 0.0f; /**< Acceptances radius of the target */
@@ -121,8 +121,8 @@ protected:
private:
matrix::Vector2f _lock_position_xy{NAN, NAN}; /**< if no valid triplet is received, lock positition to current position */
bool _yaw_lock = false; /**< if within acceptance radius, lock yaw to current yaw */
uORB::Subscription<position_setpoint_triplet_s> *_sub_triplet_setpoint{nullptr};
uORB::Subscription<vehicle_status_s> *_sub_vehicle_status{nullptr};
uORB::SubscriptionPollable<position_setpoint_triplet_s> *_sub_triplet_setpoint{nullptr};
uORB::SubscriptionPollable<vehicle_status_s> *_sub_vehicle_status{nullptr};
matrix::Vector3f
_triplet_target; /**< current triplet from navigator which may differ from the intenal one (_target) depending on the vehicle state. */
@@ -44,7 +44,7 @@
#include <px4_module_params.h>
#include <drivers/drv_hrt.h>
#include <matrix/matrix/math.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/SubscriptionPollable.hpp>
#include <uORB/topics/landing_gear.h>
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/vehicle_local_position_setpoint.h>
@@ -181,8 +181,8 @@ public:
protected:
uORB::Subscription<vehicle_local_position_s> *_sub_vehicle_local_position{nullptr};
uORB::Subscription<vehicle_attitude_s> *_sub_attitude{nullptr};
uORB::SubscriptionPollable<vehicle_local_position_s> *_sub_vehicle_local_position{nullptr};
uORB::SubscriptionPollable<vehicle_attitude_s> *_sub_attitude{nullptr};
uint8_t _heading_reset_counter{0}; /**< estimator heading reset */
/** Reset all setpoints to NAN */
@@ -53,14 +53,14 @@ void SubscriptionArray::cleanup()
bool SubscriptionArray::resizeSubscriptions()
{
const int new_size = _subscriptions_size == 0 ? 4 : _subscriptions_size * 2;
uORB::SubscriptionNode **new_array = new uORB::SubscriptionNode*[new_size];
uORB::SubscriptionPollableNode **new_array = new uORB::SubscriptionPollableNode*[new_size];
if (!new_array) {
return false;
}
if (_subscriptions) {
memcpy(new_array, _subscriptions, sizeof(uORB::SubscriptionNode *)*_subscriptions_count);
memcpy(new_array, _subscriptions, sizeof(uORB::SubscriptionPollableNode *)*_subscriptions_count);
delete[] _subscriptions;
}
@@ -41,7 +41,7 @@
#pragma once
#include <uORB/Subscription.hpp>
#include <uORB/SubscriptionPollable.hpp>
class SubscriptionArray
{
@@ -57,7 +57,7 @@ public:
* @return true on success, false otherwise (subscription set to nullptr)
*/
template<class T>
bool get(const struct orb_metadata *meta, uORB::Subscription<T> *&subscription, unsigned instance = 0);
bool get(const struct orb_metadata *meta, uORB::SubscriptionPollable<T> *&subscription, unsigned instance = 0);
/**
* update all subscriptions (if new data is available)
@@ -74,20 +74,21 @@ private:
bool resizeSubscriptions();
uORB::SubscriptionNode **_subscriptions{nullptr};
uORB::SubscriptionPollableNode **_subscriptions{nullptr};
int _subscriptions_count{0}; ///< number of valid subscriptions
int _subscriptions_size{0}; ///< actual size of the _subscriptions array
};
template<class T>
bool SubscriptionArray::get(const struct orb_metadata *meta, uORB::Subscription<T> *&subscription, unsigned instance)
bool SubscriptionArray::get(const struct orb_metadata *meta, uORB::SubscriptionPollable<T> *&subscription,
unsigned instance)
{
// does it already exist?
for (int i = 0; i < _subscriptions_count; ++i) {
if (_subscriptions[i]->getMeta() == meta && _subscriptions[i]->getInstance() == instance) {
// we know the type must be correct, so we can use reinterpret_cast (dynamic_cast is not available)
subscription = reinterpret_cast<uORB::Subscription<T>*>(_subscriptions[i]);
subscription = reinterpret_cast<uORB::SubscriptionPollable<T>*>(_subscriptions[i]);
return true;
}
}
@@ -100,7 +101,7 @@ bool SubscriptionArray::get(const struct orb_metadata *meta, uORB::Subscription<
}
}
subscription = new uORB::Subscription<T>(meta, 0, instance);
subscription = new uORB::SubscriptionPollable<T>(meta, 0, instance);
if (!subscription) {
return false;
@@ -68,7 +68,7 @@ private:
bool _evaluateSticks(); /**< checks and sets stick inputs */
void _applyGearSwitch(uint8_t gswitch); /**< Sets gears according to switch */
uORB::Subscription<manual_control_setpoint_s> *_sub_manual_control_setpoint{nullptr};
uORB::SubscriptionPollable<manual_control_setpoint_s> *_sub_manual_control_setpoint{nullptr};
DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTask,
(ParamFloat<px4::params::MPC_HOLD_DZ>) _param_mpc_hold_dz, /**< 0-deadzone around the center for the sticks */
@@ -110,7 +110,7 @@ private:
*/
void _respectGroundSlowdown();
uORB::Subscription<home_position_s> *_sub_home_position{nullptr};
uORB::SubscriptionPollable<home_position_s> *_sub_home_position{nullptr};
uint8_t _reset_counter = 0; /**< counter for estimator resets in z-direction */
float _max_speed_up = 10.0f;
@@ -53,7 +53,7 @@ public:
bool updateInitialize() override;
protected:
uORB::Subscription<position_setpoint_triplet_s> *_sub_triplet_setpoint{nullptr};
uORB::SubscriptionPollable<position_setpoint_triplet_s> *_sub_triplet_setpoint{nullptr};
private:
matrix::Vector3f _position_lock{};
@@ -105,8 +105,8 @@ public:
private:
uORB::Subscription<vehicle_trajectory_waypoint_s> *_sub_vehicle_trajectory_waypoint{nullptr}; /**< vehicle trajectory waypoint subscription */
uORB::Subscription<vehicle_status_s> *_sub_vehicle_status{nullptr}; /**< vehicle status subscription */
uORB::SubscriptionPollable<vehicle_trajectory_waypoint_s> *_sub_vehicle_trajectory_waypoint{nullptr}; /**< vehicle trajectory waypoint subscription */
uORB::SubscriptionPollable<vehicle_status_s> *_sub_vehicle_status{nullptr}; /**< vehicle status subscription */
DEFINE_PARAMETERS(
(ParamFloat<px4::params::NAV_MC_ALT_RAD>) _param_nav_mc_alt_rad /**< Acceptance radius for multicopter altitude */
+3 -3
View File
@@ -42,7 +42,7 @@
#include <cstring>
#include <uORB/Subscription.hpp>
#include <uORB/SubscriptionPollable.hpp>
#include <uORB/Publication.hpp>
namespace control
@@ -102,7 +102,7 @@ void Block::updateParams()
void Block::updateSubscriptions()
{
uORB::SubscriptionNode *sub = getSubscriptions().getHead();
uORB::SubscriptionPollableNode *sub = getSubscriptions().getHead();
int count = 0;
while (sub != nullptr) {
@@ -211,6 +211,6 @@ void SuperBlock::updateChildPublications()
} // namespace control
template class List<uORB::SubscriptionNode *>;
template class List<uORB::SubscriptionPollableNode *>;
template class List<uORB::PublicationNode *>;
template class List<control::BlockParamBase *>;
+3 -3
View File
@@ -41,7 +41,7 @@
#include <containers/List.hpp>
#include <uORB/Publication.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/SubscriptionPollable.hpp>
#include <controllib/block/BlockParam.hpp>
namespace control
@@ -87,7 +87,7 @@ protected:
virtual void updateParamsSubclass() {}
SuperBlock *getParent() { return _parent; }
List<uORB::SubscriptionNode *> &getSubscriptions() { return _subscriptions; }
List<uORB::SubscriptionPollableNode *> &getSubscriptions() { return _subscriptions; }
List<uORB::PublicationNode *> &getPublications() { return _publications; }
List<BlockParamBase *> &getParams() { return _params; }
@@ -95,7 +95,7 @@ protected:
SuperBlock *_parent;
float _dt{0.0f};
List<uORB::SubscriptionNode *> _subscriptions;
List<uORB::SubscriptionPollableNode *> _subscriptions;
List<uORB::PublicationNode *> _publications;
List<BlockParamBase *> _params;
};