mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-18 00:03:33 +08:00
uORB::Subscription subscribe directly to uORB device node object
This commit is contained in:
@@ -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 */
|
||||
|
||||
@@ -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 *>;
|
||||
|
||||
@@ -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;
|
||||
};
|
||||
|
||||
Reference in New Issue
Block a user