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

uORB::Subscription subscribe directly to uORB device node object

Browse Source
This commit is contained in:
Daniel Agar
2019-05-18 11:47:17 -04:00
parent 2d1c60bc85
commit 2c63e335e9
37 changed files with 766 additions and 270 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/md25/md25.hpp
+1 -1
View File
@@ -270,7 +270,7 @@ private:
struct pollfd _controlPoll;
/** actuator controls subscription */
uORB::Subscription<actuator_controls_s> _actuators;
uORB::SubscriptionData<actuator_controls_s> _actuators;
// local copy of data from i2c device
uint8_t _version;
src/drivers/roboclaw/RoboClaw.hpp
+2 -2
View File
@@ -45,7 +45,7 @@
#include <poll.h>
#include <stdio.h>
#include <uORB/Subscription.hpp>
#include <uORB/SubscriptionPollable.hpp>
#include <uORB/topics/actuator_controls.h>
#include <drivers/device/i2c.h>
@@ -169,7 +169,7 @@ private:
struct pollfd _controlPoll;
/** actuator controls subscription */
uORB::Subscription<actuator_controls_s> _actuators;
uORB::SubscriptionPollable<actuator_controls_s> _actuators;
// private data
float _motor1Position;
src/examples/segway/blocks.hpp
+9 -9
View File
@@ -48,7 +48,7 @@
#include <controllib/blocks.hpp>
#include <drivers/drv_hrt.h>
#include <uORB/Subscription.hpp>
#include <uORB/SubscriptionPollable.hpp>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/position_setpoint_triplet.h>
@@ -92,14 +92,14 @@ class __EXPORT BlockUorbEnabledAutopilot : public SuperBlock
{
protected:
// subscriptions
uORB::Subscription<manual_control_setpoint_s> _manual;
uORB::Subscription<parameter_update_s> _param_update;
uORB::Subscription<position_setpoint_triplet_s> _missionCmd;
uORB::Subscription<vehicle_attitude_s> _att;
uORB::Subscription<vehicle_attitude_setpoint_s> _attCmd;
uORB::Subscription<vehicle_global_position_s> _pos;
uORB::Subscription<vehicle_rates_setpoint_s> _ratesCmd;
uORB::Subscription<vehicle_status_s> _status;
uORB::SubscriptionPollable<manual_control_setpoint_s> _manual;
uORB::SubscriptionPollable<parameter_update_s> _param_update;
uORB::SubscriptionPollable<position_setpoint_triplet_s> _missionCmd;
uORB::SubscriptionPollable<vehicle_attitude_s> _att;
uORB::SubscriptionPollable<vehicle_attitude_setpoint_s> _attCmd;
uORB::SubscriptionPollable<vehicle_global_position_s> _pos;
uORB::SubscriptionPollable<vehicle_rates_setpoint_s> _ratesCmd;
uORB::SubscriptionPollable<vehicle_status_s> _status;
// publications
uORB::Publication<actuator_controls_s> _actuators;
src/lib/CollisionPrevention/CollisionPrevention.hpp
+1 -1
View File
@@ -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;
src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.hpp
+3 -3
View File
@@ -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. */
src/lib/FlightTasks/tasks/FlightTask/FlightTask.hpp
+3 -3
View File
@@ -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 */
src/lib/FlightTasks/tasks/FlightTask/SubscriptionArray.cpp
+2 -2
View File
@@ -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;
}
src/lib/FlightTasks/tasks/FlightTask/SubscriptionArray.hpp
+7 -6
View File
@@ -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;
src/lib/FlightTasks/tasks/Manual/FlightTaskManual.hpp
+1 -1
View File
@@ -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 */
src/lib/FlightTasks/tasks/ManualAltitude/FlightTaskManualAltitude.hpp
+1 -1
View File
@@ -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;
src/lib/FlightTasks/tasks/Offboard/FlightTaskOffboard.hpp
+1 -1
View File
@@ -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{};
src/lib/FlightTasks/tasks/Utility/ObstacleAvoidance.hpp
+2 -2
View File
@@ -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 */
src/lib/controllib/block/Block.cpp
+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 *>;
src/lib/controllib/block/Block.hpp
+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;
};
src/modules/commander/Commander.hpp
+8 -9
View File
@@ -62,8 +62,6 @@
#include <uORB/topics/vehicle_local_position.h>
using math::constrain;
using uORB::Publication;
using uORB::Subscription;
using namespace time_literals;
@@ -233,14 +231,15 @@ private:
bool _print_avoidance_msg_once{false};
// Subscriptions
Subscription<airspeed_s> _airspeed_sub{ORB_ID(airspeed)};
Subscription<estimator_status_s> _estimator_status_sub{ORB_ID(estimator_status)};
Subscription<mission_result_s> _mission_result_sub{ORB_ID(mission_result)};
Subscription<sensor_bias_s> _sensor_bias_sub{ORB_ID(sensor_bias)};
Subscription<vehicle_global_position_s> _global_position_sub{ORB_ID(vehicle_global_position)};
Subscription<vehicle_local_position_s> _local_position_sub{ORB_ID(vehicle_local_position)};
uORB::SubscriptionData<airspeed_s> _airspeed_sub{ORB_ID(airspeed)};
uORB::SubscriptionData<estimator_status_s> _estimator_status_sub{ORB_ID(estimator_status)};
uORB::SubscriptionData<mission_result_s> _mission_result_sub{ORB_ID(mission_result)};
uORB::SubscriptionData<sensor_bias_s> _sensor_bias_sub{ORB_ID(sensor_bias)};
uORB::SubscriptionData<vehicle_global_position_s> _global_position_sub{ORB_ID(vehicle_global_position)};
uORB::SubscriptionData<vehicle_local_position_s> _local_position_sub{ORB_ID(vehicle_local_position)};
Publication<home_position_s> _home_pub{ORB_ID(home_position)};
// Publications
uORB::Publication<home_position_s> _home_pub{ORB_ID(home_position)};
orb_advert_t _status_pub{nullptr};
};
src/modules/commander/PreflightCheck.cpp
+8 -8
View File
@@ -100,7 +100,7 @@ static bool check_calibration(const char *param_template, int32_t device_id)
return calibration_found;
}
static bool magnometerCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, unsigned instance, bool optional,
static bool magnometerCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, uint8_t instance, bool optional,
int32_t &device_id, bool report_fail)
{
const bool exists = (orb_exists(ORB_ID(sensor_mag), instance) == PX4_OK);
@@ -109,7 +109,7 @@ static bool magnometerCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &sta
if (exists) {
uORB::Subscription<sensor_mag_s> magnetometer{ORB_ID(sensor_mag), 0, instance};
uORB::SubscriptionData<sensor_mag_s> magnetometer{ORB_ID(sensor_mag), instance};
mag_valid = (hrt_elapsed_time(&magnetometer.get().timestamp) < 1_s);
@@ -236,7 +236,7 @@ static bool magConsistencyCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s
return true;
}
static bool accelerometerCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, unsigned instance,
static bool accelerometerCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, uint8_t instance,
bool optional, bool dynamic, int32_t &device_id, bool report_fail)
{
const bool exists = (orb_exists(ORB_ID(sensor_accel), instance) == PX4_OK);
@@ -245,7 +245,7 @@ static bool accelerometerCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &
if (exists) {
uORB::Subscription<sensor_accel_s> accel{ORB_ID(sensor_accel), 0, instance};
uORB::SubscriptionData<sensor_accel_s> accel{ORB_ID(sensor_accel), instance};
accel_valid = (hrt_elapsed_time(&accel.get().timestamp) < 1_s);
@@ -300,7 +300,7 @@ static bool accelerometerCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &
return success;
}
static bool gyroCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, unsigned instance, bool optional,
static bool gyroCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, uint8_t instance, bool optional,
int32_t &device_id, bool report_fail)
{
const bool exists = (orb_exists(ORB_ID(sensor_gyro), instance) == PX4_OK);
@@ -309,7 +309,7 @@ static bool gyroCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, u
if (exists) {
uORB::Subscription<sensor_gyro_s> gyro{ORB_ID(sensor_gyro), 0, instance};
uORB::SubscriptionData<sensor_gyro_s> gyro{ORB_ID(sensor_gyro), instance};
gyro_valid = (hrt_elapsed_time(&gyro.get().timestamp) < 1_s);
@@ -345,14 +345,14 @@ static bool gyroCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, u
return calibration_valid && gyro_valid;
}
static bool baroCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, unsigned instance, bool optional,
static bool baroCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, uint8_t instance, bool optional,
int32_t &device_id, bool report_fail)
{
const bool exists = (orb_exists(ORB_ID(sensor_baro), instance) == PX4_OK);
bool baro_valid = false;
if (exists) {
uORB::Subscription<sensor_baro_s> baro{ORB_ID(sensor_baro), 0, instance};
uORB::SubscriptionData<sensor_baro_s> baro{ORB_ID(sensor_baro), instance};
baro_valid = (hrt_elapsed_time(&baro.get().timestamp) < 1_s);
src/modules/commander/failure_detector/FailureDetector.hpp
+3 -3
View File
@@ -60,7 +60,7 @@ typedef enum {
FAILURE_ALT = vehicle_status_s::FAILURE_ALT,
} failure_detector_bitmak;
using uORB::Subscription;
using uORB::SubscriptionData;
class FailureDetector : public ModuleParams
{
@@ -79,8 +79,8 @@ private:
)
// Subscriptions
Subscription<vehicle_attitude_s> _sub_vehicle_attitude_setpoint;
Subscription<vehicle_attitude_s> _sub_vehicule_attitude;
SubscriptionData<vehicle_attitude_s> _sub_vehicle_attitude_setpoint;
SubscriptionData<vehicle_attitude_s> _sub_vehicule_attitude;
uint8_t _status{FAILURE_NONE};
src/modules/fw_att_control/FixedwingAttitudeControl.hpp
+2 -2
View File
@@ -64,7 +64,7 @@
using matrix::Eulerf;
using matrix::Quatf;
using uORB::Subscription;
using uORB::SubscriptionData;
class FixedwingAttitudeControl final : public ModuleBase<FixedwingAttitudeControl>
{
@@ -122,7 +122,7 @@ private:
vehicle_rates_setpoint_s _rates_sp {}; /* attitude rates setpoint */
vehicle_status_s _vehicle_status {}; /**< vehicle status */
Subscription<airspeed_s> _airspeed_sub;
SubscriptionData<airspeed_s> _airspeed_sub;
perf_counter_t _loop_perf; /**< loop performance counter */
perf_counter_t _nonfinite_input_perf; /**< performance counter for non finite input */
src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp
+3 -3
View File
@@ -99,7 +99,7 @@ using matrix::Vector2f;
using matrix::Vector3f;
using matrix::wrap_pi;
using uORB::Subscription;
using uORB::SubscriptionData;
using namespace launchdetection;
using namespace runwaytakeoff;
@@ -181,8 +181,8 @@ private:
vehicle_land_detected_s _vehicle_land_detected {}; ///< vehicle land detected */
vehicle_status_s _vehicle_status {}; ///< vehicle status */
Subscription<airspeed_s> _sub_airspeed;
Subscription<sensor_bias_s> _sub_sensors;
SubscriptionData<airspeed_s> _sub_airspeed;
SubscriptionData<sensor_bias_s> _sub_sensors;
perf_counter_t _loop_perf; ///< loop performance counter */
src/modules/gnd_pos_control/GroundRoverPositionControl.cpp
+2 -2
View File
@@ -65,8 +65,8 @@ GroundRoverPositionControl *g_control = nullptr;
GroundRoverPositionControl::GroundRoverPositionControl() :
/* performance counters */
_sub_attitude(ORB_ID(vehicle_attitude), 0, 0, nullptr),
_sub_sensors(ORB_ID(sensor_bias), 0, 0, nullptr),
_sub_attitude(ORB_ID(vehicle_attitude)),
_sub_sensors(ORB_ID(sensor_bias)),
_loop_perf(perf_alloc(PC_ELAPSED, "rover position control")) // TODO : do we even need these perf counters
{
_parameter_handles.l1_period = param_find("GND_L1_PERIOD");
src/modules/gnd_pos_control/GroundRoverPositionControl.hpp
+3 -3
View File
@@ -67,7 +67,7 @@
using matrix::Dcmf;
using uORB::Subscription;
using uORB::SubscriptionData;
class GroundRoverPositionControl
{
@@ -110,8 +110,8 @@ private:
vehicle_control_mode_s _control_mode{}; /**< control mode */
vehicle_global_position_s _global_pos{}; /**< global vehicle position */
Subscription<vehicle_attitude_s> _sub_attitude;
Subscription<sensor_bias_s> _sub_sensors;
SubscriptionData<vehicle_attitude_s> _sub_attitude;
SubscriptionData<sensor_bias_s> _sub_sensors;
perf_counter_t _loop_perf; /**< loop performance counter */
src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp
+1 -1
View File
@@ -210,7 +210,7 @@ void BlockLocalPositionEstimator::update()
if (!armedState && (_sub_lidar == nullptr || _sub_sonar == nullptr)) {
// detect distance sensors
for (size_t i = 0; i < N_DIST_SUBS; i++) {
uORB::Subscription<distance_sensor_s> *s = _dist_subs[i];
uORB::SubscriptionPollable<distance_sensor_s> *s = _dist_subs[i];
if (s == _sub_lidar || s == _sub_sonar) { continue; }
src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp
+19 -19
View File
@@ -9,7 +9,7 @@
#include <matrix/Matrix.hpp>
// uORB Subscriptions
#include <uORB/Subscription.hpp>
#include <uORB/SubscriptionPollable.hpp>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/actuator_armed.h>
#include <uORB/topics/vehicle_land_detected.h>
@@ -246,24 +246,24 @@ private:
// ----------------------------
// subscriptions
uORB::Subscription<actuator_armed_s> _sub_armed;
uORB::Subscription<vehicle_land_detected_s> _sub_land;
uORB::Subscription<vehicle_attitude_s> _sub_att;
uORB::Subscription<optical_flow_s> _sub_flow;
uORB::Subscription<sensor_combined_s> _sub_sensor;
uORB::Subscription<parameter_update_s> _sub_param_update;
uORB::Subscription<vehicle_gps_position_s> _sub_gps;
uORB::Subscription<vehicle_odometry_s> _sub_visual_odom;
uORB::Subscription<vehicle_odometry_s> _sub_mocap_odom;
uORB::Subscription<distance_sensor_s> _sub_dist0;
uORB::Subscription<distance_sensor_s> _sub_dist1;
uORB::Subscription<distance_sensor_s> _sub_dist2;
uORB::Subscription<distance_sensor_s> _sub_dist3;
uORB::Subscription<distance_sensor_s> *_dist_subs[N_DIST_SUBS];
uORB::Subscription<distance_sensor_s> *_sub_lidar;
uORB::Subscription<distance_sensor_s> *_sub_sonar;
uORB::Subscription<landing_target_pose_s> _sub_landing_target_pose;
uORB::Subscription<vehicle_air_data_s> _sub_airdata;
uORB::SubscriptionPollable<actuator_armed_s> _sub_armed;
uORB::SubscriptionPollable<vehicle_land_detected_s> _sub_land;
uORB::SubscriptionPollable<vehicle_attitude_s> _sub_att;
uORB::SubscriptionPollable<optical_flow_s> _sub_flow;
uORB::SubscriptionPollable<sensor_combined_s> _sub_sensor;
uORB::SubscriptionPollable<parameter_update_s> _sub_param_update;
uORB::SubscriptionPollable<vehicle_gps_position_s> _sub_gps;
uORB::SubscriptionPollable<vehicle_odometry_s> _sub_visual_odom;
uORB::SubscriptionPollable<vehicle_odometry_s> _sub_mocap_odom;
uORB::SubscriptionPollable<distance_sensor_s> _sub_dist0;
uORB::SubscriptionPollable<distance_sensor_s> _sub_dist1;
uORB::SubscriptionPollable<distance_sensor_s> _sub_dist2;
uORB::SubscriptionPollable<distance_sensor_s> _sub_dist3;
uORB::SubscriptionPollable<distance_sensor_s> *_dist_subs[N_DIST_SUBS];
uORB::SubscriptionPollable<distance_sensor_s> *_sub_lidar;
uORB::SubscriptionPollable<distance_sensor_s> *_sub_sonar;
uORB::SubscriptionPollable<landing_target_pose_s> _sub_landing_target_pose;
uORB::SubscriptionPollable<vehicle_air_data_s> _sub_airdata;
// publications
uORB::Publication<vehicle_local_position_s> _pub_lpos;
src/modules/logger/logger.cpp
+1 -1
View File
@@ -934,7 +934,7 @@ void Logger::run()
}
int vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
uORB::Subscription<parameter_update_s> parameter_update_sub(ORB_ID(parameter_update));
uORB::SubscriptionData<parameter_update_s> parameter_update_sub(ORB_ID(parameter_update));
int log_message_sub = orb_subscribe(ORB_ID(log_message));
orb_set_interval(log_message_sub, 20);
src/modules/navigator/geofence.h
+1 -1
View File
@@ -166,7 +166,7 @@ private:
(ParamFloat<px4::params::GF_MAX_VER_DIST>) _param_gf_max_ver_dist
)
uORB::Subscription<vehicle_air_data_s> _sub_airdata;
uORB::SubscriptionData<vehicle_air_data_s> _sub_airdata;
int _outside_counter{0};
uint16_t _update_counter{0}; ///< dataman update counter: if it does not match, we polygon data was updated
src/modules/navigator/mission_feasibility_checker.cpp
+1 -2
View File
@@ -450,8 +450,7 @@ MissionFeasibilityChecker::checkFixedWingLanding(const mission_s &mission, bool
if (MissionBlock::item_contains_position(missionitem_previous)) {
uORB::Subscription<position_controller_landing_status_s> landing_status{ORB_ID(position_controller_landing_status)};
landing_status.forcedUpdate();
uORB::SubscriptionData<position_controller_landing_status_s> landing_status{ORB_ID(position_controller_landing_status)};
const bool landing_status_valid = (landing_status.get().timestamp > 0);
const float wp_distance = get_distance_to_next_waypoint(missionitem_previous.lat, missionitem_previous.lon,
src/modules/navigator/navigator.h
+1 -1
View File
@@ -341,7 +341,7 @@ private:
vehicle_local_position_s _local_pos{}; /**< local vehicle position */
vehicle_status_s _vstatus{}; /**< vehicle status */
uORB::Subscription<position_controller_status_s> _position_controller_status_sub{ORB_ID(position_controller_status)};
uORB::SubscriptionData<position_controller_status_s> _position_controller_status_sub{ORB_ID(position_controller_status)};
uint8_t _previous_nav_state{}; /**< nav_state of the previous iteration*/
src/modules/sensors/voted_sensors_update.cpp
+6 -6
View File
@@ -152,9 +152,9 @@ void VotedSensorsUpdate::parameters_update()
_temperature_compensation.parameters_update(_hil_enabled);
/* gyro */
for (int topic_instance = 0; topic_instance < _gyro.subscription_count; ++topic_instance) {
for (uint8_t topic_instance = 0; topic_instance < _gyro.subscription_count; ++topic_instance) {
uORB::Subscription<sensor_gyro_s> report{ORB_ID(sensor_gyro), 0, (unsigned)topic_instance};
uORB::SubscriptionData<sensor_gyro_s> report{ORB_ID(sensor_gyro), topic_instance};
int temp = _temperature_compensation.set_sensor_id_gyro(report.get().device_id, topic_instance);
@@ -172,9 +172,9 @@ void VotedSensorsUpdate::parameters_update()
/* accel */
for (int topic_instance = 0; topic_instance < _accel.subscription_count; ++topic_instance) {
for (uint8_t topic_instance = 0; topic_instance < _accel.subscription_count; ++topic_instance) {
uORB::Subscription<sensor_accel_s> report{ORB_ID(sensor_accel), 0, (unsigned)topic_instance};
uORB::SubscriptionData<sensor_accel_s> report{ORB_ID(sensor_accel), topic_instance};
int temp = _temperature_compensation.set_sensor_id_accel(report.get().device_id, topic_instance);
@@ -192,9 +192,9 @@ void VotedSensorsUpdate::parameters_update()
/* baro */
for (int topic_instance = 0; topic_instance < _baro.subscription_count; ++topic_instance) {
for (uint8_t topic_instance = 0; topic_instance < _baro.subscription_count; ++topic_instance) {
uORB::Subscription<sensor_baro_s> report{ORB_ID(sensor_baro), 0, (unsigned)topic_instance};
uORB::SubscriptionData<sensor_baro_s> report{ORB_ID(sensor_baro), topic_instance};
int temp = _temperature_compensation.set_sensor_id_baro(report.get().device_id, topic_instance);
src/modules/uORB/CMakeLists.txt
+4 -3
View File
@@ -32,10 +32,10 @@
############################################################################
if(NOT "${PX4_BOARD}" MATCHES "px4_io") # TODO: fix this hack (move uORB to platform layer)
# this includes the generated topics directory
include_directories(${CMAKE_CURRENT_BINARY_DIR})
px4_add_module(
MODULE modules__uORB
MAIN uorb
@@ -43,6 +43,7 @@ if(NOT "${PX4_BOARD}" MATCHES "px4_io") # TODO: fix this hack (move uORB to plat
SRCS
Publication.cpp
Subscription.cpp
SubscriptionPollable.cpp
uORB.cpp
uORBDeviceMaster.cpp
uORBDeviceNode.cpp
@@ -53,7 +54,7 @@ if(NOT "${PX4_BOARD}" MATCHES "px4_io") # TODO: fix this hack (move uORB to plat
cdev
uorb_msgs
)
if(PX4_TESTING)
add_subdirectory(uORB_tests)
endif()
src/modules/uORB/Subscription.cpp
+49 -38
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2012-2017 PX4 Development Team. All rights reserved.
* Copyright (c) 2012-2019 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -42,67 +42,78 @@
namespace uORB
{
SubscriptionBase::SubscriptionBase(const struct orb_metadata *meta, unsigned interval, unsigned instance) :
_meta(meta),
_instance(instance)
bool Subscription::subscribe()
{
if (instance > 0) {
_handle = orb_subscribe_multi(_meta, instance);
DeviceMaster *device_master = uORB::Manager::get_instance()->get_device_master();
_node = device_master->getDeviceNode(_meta, _instance);
} else {
_handle = orb_subscribe(_meta);
if (_node != nullptr) {
_node->add_internal_subscriber();
// If there were any previous publications, allow the subscriber to read them
const unsigned curr_gen = _node->published_message_count();
const unsigned q_size = _node->get_queue_size();
_last_generation = curr_gen - (q_size < curr_gen ? q_size : curr_gen);
return true;
}
if (_handle < 0) {
PX4_ERR("%s sub failed", _meta->o_name);
}
if (interval > 0) {
orb_set_interval(_handle, interval);
}
return false;
}
bool SubscriptionBase::updated()
void Subscription::unsubscribe()
{
bool isUpdated = false;
if (orb_check(_handle, &isUpdated) != PX4_OK) {
PX4_ERR("%s check failed", _meta->o_name);
if (_node != nullptr) {
_node->remove_internal_subscriber();
}
return isUpdated;
_last_generation = 0;
}
bool SubscriptionBase::update(void *data)
bool Subscription::init()
{
bool orb_updated = false;
if (_meta != nullptr) {
// this throttles the relatively expensive calls to getDeviceNode()
if ((_last_generation == 0) || (_last_generation < 1000) || (_last_generation % 100 == 0)) {
if (subscribe()) {
return true;
}
}
if (updated()) {
if (orb_copy(_meta, _handle, data) != PX4_OK) {
PX4_ERR("%s copy failed", _meta->o_name);
} else {
orb_updated = true;
if (_node == nullptr) {
// use generation to count attempts to subscribe
_last_generation++;
}
}
return orb_updated;
return false;
}
SubscriptionBase::~SubscriptionBase()
bool Subscription::forceInit()
{
if (orb_unsubscribe(_handle) != PX4_OK) {
PX4_ERR("%s unsubscribe failed", _meta->o_name);
if (_node == nullptr) {
// reset generation to force subscription attempt
_last_generation = 0;
return subscribe();
}
return false;
}
SubscriptionNode::SubscriptionNode(const struct orb_metadata *meta, unsigned interval, unsigned instance,
List<SubscriptionNode *> *list)
: SubscriptionBase(meta, interval, instance)
bool Subscription::update(uint64_t *time, void *dst)
{
if (list != nullptr) {
list->add(this);
if ((time != nullptr) && (dst != nullptr) && published()) {
// always copy data to dst regardless of update
const uint64_t t = _node->copy_and_get_timestamp(dst, _last_generation);
if (*time == 0 || *time != t) {
*time = t;
return true;
}
}
return false;
}
} // namespace uORB
src/modules/uORB/Subscription.hpp
+150 -105
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2012-2017 PX4 Development Team. All rights reserved.
* Copyright (c) 2012-2019 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -39,159 +39,204 @@
#pragma once
#include <uORB/uORB.h>
#include <containers/List.hpp>
#include <systemlib/err.h>
#include <px4_defines.h>
#include "uORBDeviceNode.hpp"
#include "uORBManager.hpp"
#include "uORBUtils.hpp"
namespace uORB
{
/**
* Base subscription wrapper class, used in list traversal
* of various subscriptions.
*/
class __EXPORT SubscriptionBase
// Base subscription wrapper class
class Subscription
{
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 meta The uORB metadata (usually from the ORB_ID() macro) for the topic.
* @param instance The instance for multi sub.
*/
SubscriptionBase(const struct orb_metadata *meta, unsigned interval = 0, unsigned instance = 0);
virtual ~SubscriptionBase();
Subscription(const orb_metadata *meta, uint8_t instance = 0) : _meta(meta), _instance(instance)
{
init();
}
// no copy, assignment, move, move assignment
SubscriptionBase(const SubscriptionBase &) = delete;
SubscriptionBase &operator=(const SubscriptionBase &) = delete;
SubscriptionBase(SubscriptionBase &&) = delete;
SubscriptionBase &operator=(SubscriptionBase &&) = delete;
virtual ~Subscription() { unsubscribe(); }
bool init();
bool forceInit();
bool valid() const { return _node != nullptr; }
bool published() { return valid() ? _node->is_published() : init(); }
/**
* Check if there is a new update.
* */
bool updated();
virtual bool updated() { return published() ? (_node->published_message_count() != _last_generation) : false; }
/**
* Update the struct
* @param data The uORB message struct we are updating.
*/
bool update(void *data);
virtual bool update(void *dst) { return updated() ? copy(dst) : false; }
int getHandle() const { return _handle; }
/**
* Check if subscription updated based on timestamp.
*
* @return true only if topic was updated based on a timestamp and
* copied to buffer successfully.
* If topic was not updated since last check it will return false but
* still copy the data.
* If no data available data buffer will be filled with zeros.
*/
bool update(uint64_t *time, void *dst);
const orb_metadata *getMeta() const { return _meta; }
/**
* Copy the struct
* @param data The uORB message struct we are updating.
*/
bool copy(void *dst) { return published() ? _node->copy(dst, _last_generation) : false; }
unsigned getInstance() const { return _instance; }
hrt_abstime last_update() { return published() ? _node->last_update() : 0; }
uint8_t get_instance() const { return _instance; }
orb_id_t get_topic() const { return _meta; }
protected:
const struct orb_metadata *_meta;
unsigned _instance;
bool subscribe();
void unsubscribe();
int _handle{-1};
DeviceNode *_node{nullptr};
const orb_metadata *_meta{nullptr};
/**
* Subscription's latest data generation.
* Also used to track (and rate limit) subscription
* attempts if the topic has not yet been published.
*/
unsigned _last_generation{0};
uint8_t _instance{0};
};
/**
* alias class name so it is clear that the base class
*/
typedef SubscriptionBase SubscriptionTiny;
/**
* The subscription base class as a list node.
*/
class __EXPORT SubscriptionNode : public SubscriptionBase, public ListNode<SubscriptionNode *>
// Subscription wrapper class with configured interval
class SubscriptionInterval : public Subscription
{
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 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, unsigned instance = 0,
List<SubscriptionNode *> *list = nullptr);
SubscriptionInterval(const orb_metadata *meta, unsigned interval = 0, uint8_t instance = 0) :
Subscription(meta, instance),
_interval(interval)
{}
virtual ~SubscriptionNode() override = default;
virtual ~SubscriptionInterval() = default;
/**
* This function is the callback for list traversal
* updates, a child class must implement it.
*/
virtual bool update() = 0;
/**
* Like update(), but does not check first if there is data available
*/
virtual bool forcedUpdate() = 0;
};
/**
* Subscription wrapper class
*/
template<class T>
class __EXPORT Subscription : 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, unsigned instance = 0,
List<SubscriptionNode *> *list = nullptr):
SubscriptionNode(meta, interval, instance, list),
_data() // initialize data structure to zero
bool updated() override
{
forcedUpdate();
if (hrt_elapsed_time(&_last_update) >= (_interval * 1000)) {
return Subscription::updated();
}
return false;
}
~Subscription() override = default;
bool update(void *dst) override
{
if (updated()) {
if (copy(dst)) {
_last_update = hrt_absolute_time();
return true;
}
}
return false;
}
int get_interval() const { return _interval; }
void set_interval(unsigned interval) { _interval = interval; }
protected:
uint64_t _last_update{0}; // last update in microseconds
unsigned _interval{0}; // interval in milliseconds
};
// Subscription wrapper class with data
template<class T>
class SubscriptionData : public Subscription
{
public:
/**
* Constructor
*
* @param meta The uORB metadata (usually from the ORB_ID() macro) for the topic.
* @param instance The instance for multi sub.
*/
SubscriptionData(const orb_metadata *meta, uint8_t instance = 0) :
Subscription(meta, instance)
{
copy(&_data);
}
virtual ~SubscriptionData() = default;
// no copy, assignment, move, move assignment
Subscription(const Subscription &) = delete;
Subscription &operator=(const Subscription &) = delete;
Subscription(Subscription &&) = delete;
Subscription &operator=(Subscription &&) = delete;
SubscriptionData(const SubscriptionData &) = delete;
SubscriptionData &operator=(const SubscriptionData &) = delete;
SubscriptionData(SubscriptionData &&) = delete;
SubscriptionData &operator=(SubscriptionData &&) = delete;
/**
* Create an update function that uses the embedded struct.
*/
bool update() override
{
return SubscriptionBase::update((void *)(&_data));
}
// update the embedded struct.
bool update() { return Subscription::update((void *)(&_data)); }
bool forcedUpdate() override
{
return orb_copy(_meta, _handle, &_data) == PX4_OK;
}
/*
* This function gets the T struct data
* */
const T &get() const
{
return _data;
}
const T &get() const { return _data; }
private:
T _data;
T _data{};
};
// Subscription wrapper class with data and configured interval
template<class T>
class SubscriptionIntervalData : public SubscriptionInterval
{
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.
*/
SubscriptionIntervalData(const orb_metadata *meta, unsigned interval = 0, uint8_t instance = 0) :
SubscriptionInterval(meta, interval, instance)
{
copy(&_data);
}
~SubscriptionIntervalData() override = default;
// no copy, assignment, move, move assignment
SubscriptionIntervalData(const SubscriptionIntervalData &) = delete;
SubscriptionIntervalData &operator=(const SubscriptionIntervalData &) = delete;
SubscriptionIntervalData(SubscriptionIntervalData &&) = delete;
SubscriptionIntervalData &operator=(SubscriptionIntervalData &&) = delete;
// update the embedded struct.
bool update() { return SubscriptionInterval::update((void *)(&_data)); }
const T &get() const { return _data; }
private:
T _data{};
};
} // namespace uORB
src/modules/uORB/SubscriptionPollable.cpp
+110
View File
@@ -0,0 +1,110 @@
/****************************************************************************
*
* Copyright (c) 2012-2017 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file SubscriptionPollable.cpp
*
*/
#include "SubscriptionPollable.hpp"
#include <px4_defines.h>
namespace uORB
{
SubscriptionPollableBase::SubscriptionPollableBase(const struct orb_metadata *meta, unsigned interval,
unsigned instance) :
_meta(meta),
_instance(instance)
{
if (instance > 0) {
_handle = orb_subscribe_multi(_meta, instance);
} else {
_handle = orb_subscribe(_meta);
}
if (_handle < 0) {
PX4_ERR("%s sub failed", _meta->o_name);
}
if (interval > 0) {
orb_set_interval(_handle, interval);
}
}
bool SubscriptionPollableBase::updated()
{
bool isUpdated = false;
if (orb_check(_handle, &isUpdated) != PX4_OK) {
PX4_ERR("%s check failed", _meta->o_name);
}
return isUpdated;
}
bool SubscriptionPollableBase::update(void *data)
{
bool orb_updated = false;
if (updated()) {
if (orb_copy(_meta, _handle, data) != PX4_OK) {
PX4_ERR("%s copy failed", _meta->o_name);
} else {
orb_updated = true;
}
}
return orb_updated;
}
SubscriptionPollableBase::~SubscriptionPollableBase()
{
if (orb_unsubscribe(_handle) != PX4_OK) {
PX4_ERR("%s unsubscribe failed", _meta->o_name);
}
}
SubscriptionPollableNode::SubscriptionPollableNode(const struct orb_metadata *meta, unsigned interval,
unsigned instance,
List<SubscriptionPollableNode *> *list)
: SubscriptionPollableBase(meta, interval, instance)
{
if (list != nullptr) {
list->add(this);
}
}
} // namespace uORB
src/modules/uORB/SubscriptionPollable.hpp
+197
View File
@@ -0,0 +1,197 @@
/****************************************************************************
*
* Copyright (c) 2012-2019 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file SubscriptionPollable.hpp
*
*/
#pragma once
#include <uORB/uORB.h>
#include <containers/List.hpp>
#include <systemlib/err.h>
#include <px4_defines.h>
namespace uORB
{
/**
* Base subscription wrapper class, used in list traversal
* of various subscriptions.
*/
class __EXPORT SubscriptionPollableBase
{
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.
*/
SubscriptionPollableBase(const struct orb_metadata *meta, unsigned interval = 0, unsigned instance = 0);
virtual ~SubscriptionPollableBase();
// no copy, assignment, move, move assignment
SubscriptionPollableBase(const SubscriptionPollableBase &) = delete;
SubscriptionPollableBase &operator=(const SubscriptionPollableBase &) = delete;
SubscriptionPollableBase(SubscriptionPollableBase &&) = delete;
SubscriptionPollableBase &operator=(SubscriptionPollableBase &&) = delete;
/**
* Check if there is a new update.
* */
bool updated();
/**
* Update the struct
* @param data The uORB message struct we are updating.
*/
bool update(void *data);
int getHandle() const { return _handle; }
const orb_metadata *getMeta() const { return _meta; }
unsigned getInstance() const { return _instance; }
protected:
const struct orb_metadata *_meta;
unsigned _instance;
int _handle{-1};
};
/**
* alias class name so it is clear that the base class
*/
typedef SubscriptionPollableBase SubscriptionPollableTiny;
/**
* The subscription base class as a list node.
*/
class __EXPORT SubscriptionPollableNode : public SubscriptionPollableBase, public ListNode<SubscriptionPollableNode *>
{
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.
*/
SubscriptionPollableNode(const struct orb_metadata *meta, unsigned interval = 0, unsigned instance = 0,
List<SubscriptionPollableNode *> *list = nullptr);
virtual ~SubscriptionPollableNode() override = default;
/**
* This function is the callback for list traversal
* updates, a child class must implement it.
*/
virtual bool update() = 0;
/**
* Like update(), but does not check first if there is data available
*/
virtual bool forcedUpdate() = 0;
};
/**
* SubscriptionPollable wrapper class
*/
template<class T>
class __EXPORT SubscriptionPollable : public SubscriptionPollableNode
{
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
*/
SubscriptionPollable(const struct orb_metadata *meta, unsigned interval = 0, unsigned instance = 0,
List<SubscriptionPollableNode *> *list = nullptr):
SubscriptionPollableNode(meta, interval, instance, list),
_data() // initialize data structure to zero
{
forcedUpdate();
}
~SubscriptionPollable() override = default;
// no copy, assignment, move, move assignment
SubscriptionPollable(const SubscriptionPollable &) = delete;
SubscriptionPollable &operator=(const SubscriptionPollable &) = delete;
SubscriptionPollable(SubscriptionPollable &&) = delete;
SubscriptionPollable &operator=(SubscriptionPollable &&) = delete;
/**
* Create an update function that uses the embedded struct.
*/
bool update() override
{
return SubscriptionPollableBase::update((void *)(&_data));
}
bool forcedUpdate() override
{
return orb_copy(_meta, _handle, &_data) == PX4_OK;
}
/*
* This function gets the T struct data
* */
const T &get() const
{
return _data;
}
private:
T _data;
};
} // namespace uORB
src/modules/uORB/uORBDeviceNode.cpp
+70 -23
View File
@@ -161,6 +161,75 @@ uORB::DeviceNode::close(cdev::file_t *filp)
return CDev::close(filp);
}
bool
uORB::DeviceNode::copy_locked(void *dst, unsigned &generation)
{
bool updated = false;
if ((dst != nullptr) && (_data != nullptr)) {
if (_generation > generation + _queue_size) {
// Reader is too far behind: some messages are lost
_lost_messages += _generation - (generation + _queue_size);
generation = _generation - _queue_size;
}
if ((_generation == generation) && (generation > 0)) {
/* The subscriber already read the latest message, but nothing new was published yet.
* Return the previous message
*/
--generation;
}
memcpy(dst, _data + (_meta->o_size * (generation % _queue_size)), _meta->o_size);
if (generation < _generation) {
++generation;
}
updated = true;
}
return updated;
}
bool
uORB::DeviceNode::copy(void *dst, unsigned &generation)
{
ATOMIC_ENTER;
bool updated = copy_locked(dst, generation);
ATOMIC_LEAVE;
return updated;
}
uint64_t
uORB::DeviceNode::copy_and_get_timestamp(void *dst, unsigned &generation)
{
ATOMIC_ENTER;
const hrt_abstime update_time = _last_update;
copy_locked(dst, generation);
ATOMIC_LEAVE;
return update_time;
}
hrt_abstime
uORB::DeviceNode::last_update()
{
ATOMIC_ENTER;
const hrt_abstime update_time = _last_update;
ATOMIC_LEAVE;
return update_time;
}
ssize_t
uORB::DeviceNode::read(cdev::file_t *filp, char *buffer, size_t buflen)
{
@@ -181,29 +250,7 @@ uORB::DeviceNode::read(cdev::file_t *filp, char *buffer, size_t buflen)
*/
ATOMIC_ENTER;
const unsigned gen = published_message_count();
if (gen > sd->generation + _queue_size) {
/* Reader is too far behind: some messages are lost */
_lost_messages += gen - (sd->generation + _queue_size);
sd->generation = gen - _queue_size;
}
if (gen == sd->generation && sd->generation > 0) {
/* The subscriber already read the latest message, but nothing new was published yet.
* Return the previous message
*/
--sd->generation;
}
/* if the caller doesn't want the data, don't give it to them */
if (nullptr != buffer) {
memcpy(buffer, _data + (_meta->o_size * (sd->generation % _queue_size)), _meta->o_size);
}
if (sd->generation < gen) {
++sd->generation;
}
copy_locked(buffer, sd->generation);
// if subscriber has an interval track the last update time
if (sd->update_interval) {
src/modules/uORB/uORBDeviceNode.hpp
+49
View File
@@ -195,6 +195,42 @@ public:
int get_priority() const { return _priority; }
void set_priority(uint8_t priority) { _priority = priority; }
/**
* Copies the timestamp of the last update atomically.
*
* @return uint64_t
* Returns the timestamp of the most recent data.
*/
hrt_abstime last_update();
/**
* Copies data and the corresponding generation
* from a node to the buffer provided.
*
* @param dst
* The buffer into which the data is copied.
* @param generation
* The generation that was copied.
* @return bool
* Returns true if the data was copied.
*/
bool copy(void *dst, unsigned &generation);
/**
* Copies data and the corresponding generation
* from a node to the buffer provided.
*
* @param dst
* The buffer into which the data is copied.
* If topic was not updated since last check it will return false but
* still copy the data.
* @param generation
* The generation that was copied.
* @return uint64_t
* Returns the timestamp of the copied data.
*/
uint64_t copy_and_get_timestamp(void *dst, unsigned &generation);
protected:
pollevent_t poll_state(cdev::file_t *filp) override;
@@ -203,6 +239,19 @@ protected:
private:
/**
* Copies data and the corresponding generation
* from a node to the buffer provided. Caller handles locking.
*
* @param dst
* The buffer into which the data is copied.
* @param generation
* The generation that was copied.
* @return bool
* Returns true if the data was copied.
*/
bool copy_locked(void *dst, unsigned &generation);
struct UpdateIntervalData {
uint64_t last_update{0}; /**< time at which the last update was provided, used when update_interval is nonzero */
unsigned interval{0}; /**< if nonzero minimum interval between updates */
src/modules/wind_estimator/wind_estimator_main.cpp
+1 -1
View File
@@ -324,7 +324,7 @@ int WindEstimatorModule::print_status()
perf_print_counter(_perf_interval);
if (_instance > -1) {
uORB::Subscription<wind_estimate_s> est{ORB_ID(wind_estimate), (unsigned)_instance};
uORB::SubscriptionData<wind_estimate_s> est{ORB_ID(wind_estimate), (uint8_t)_instance};
est.update();
print_message(est.get());
src/systemcmds/tests/test_microbench_uorb.cpp
+38 -1
View File
@@ -47,6 +47,7 @@
#include <px4_config.h>
#include <px4_micro_hal.h>
#include <uORB/Subscription.hpp>
#include <uORB/topics/sensor_accel.h>
#include <uORB/topics/sensor_gyro.h>
#include <uORB/topics/vehicle_local_position.h>
@@ -98,6 +99,7 @@ public:
private:
bool time_px4_uorb();
bool time_px4_uorb_direct();
void reset();
@@ -109,6 +111,7 @@ private:
bool MicroBenchORB::run_tests()
{
ut_run_test(time_px4_uorb);
ut_run_test(time_px4_uorb_direct);
return (_tests_failed == 0);
}
@@ -150,13 +153,19 @@ bool MicroBenchORB::time_px4_uorb()
PERF("orb_stat vehicle_status", ret = orb_stat(fd_status, &time), 1000);
PERF("orb_copy vehicle_status", ret = orb_copy(ORB_ID(vehicle_status), fd_status, &status), 1000);
printf("\n");
PERF("orb_check vehicle_local_position", ret = orb_check(fd_lpos, &updated), 1000);
PERF("orb_stat vehicle_local_position", ret = orb_stat(fd_lpos, &time), 1000);
PERF("orb_copy vehicle_local_position", ret = orb_copy(ORB_ID(vehicle_local_position), fd_lpos, &lpos), 1000);
printf("\n");
PERF("orb_check sensor_gyro", ret = orb_check(fd_gyro, &updated), 1000);
PERF("orb_stat sensor_gyro", ret = orb_stat(fd_gyro, &time), 1000);
PERF("orb_copy sensor_gyro", ret = orb_copy(ORB_ID(sensor_gyro), fd_gyro, &lpos), 1000);
PERF("orb_copy sensor_gyro", ret = orb_copy(ORB_ID(sensor_gyro), fd_gyro, &gyro), 1000);
printf("\n");
PERF("orb_exists sensor_accel 0", ret = orb_exists(ORB_ID(sensor_accel), 0), 100);
PERF("orb_exists sensor_accel 1", ret = orb_exists(ORB_ID(sensor_accel), 1), 100);
@@ -171,4 +180,32 @@ bool MicroBenchORB::time_px4_uorb()
return true;
}
bool MicroBenchORB::time_px4_uorb_direct()
{
bool ret = false;
bool updated = false;
uint64_t time = 0;
uORB::Subscription vstatus{ORB_ID(vehicle_status)};
PERF("uORB::Subscription orb_check vehicle_status", ret = vstatus.updated(), 1000);
PERF("uORB::Subscription orb_stat vehicle_status", time = vstatus.last_update(), 1000);
PERF("uORB::Subscription orb_copy vehicle_status", ret = vstatus.copy(&status), 1000);
printf("\n");
uORB::Subscription local_pos{ORB_ID(vehicle_local_position)};
PERF("uORB::Subscription orb_check vehicle_local_position", ret = local_pos.updated(), 1000);
PERF("uORB::Subscription orb_stat vehicle_local_position", time = local_pos.last_update(), 1000);
PERF("uORB::Subscription orb_copy vehicle_local_position", ret = local_pos.copy(&lpos), 1000);
printf("\n");
uORB::Subscription sens_gyro{ORB_ID(sensor_gyro)};
PERF("uORB::Subscription orb_check sensor_gyro", ret = sens_gyro.updated(), 1000);
PERF("uORB::Subscription orb_stat sensor_gyro", time = sens_gyro.last_update(), 1000);
PERF("uORB::Subscription orb_copy sensor_gyro", ret = sens_gyro.copy(&gyro), 1000);
return true;
}
} // namespace MicroBenchORB