From 2c63e335e92f2cc3f35be061ceb6103ca13407b7 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Sat, 18 May 2019 11:47:17 -0400 Subject: [PATCH] uORB::Subscription subscribe directly to uORB device node object --- src/drivers/md25/md25.hpp | 2 +- src/drivers/roboclaw/RoboClaw.hpp | 4 +- src/examples/segway/blocks.hpp | 18 +- .../CollisionPrevention.hpp | 2 +- .../FlightTasks/tasks/Auto/FlightTaskAuto.hpp | 6 +- .../tasks/FlightTask/FlightTask.hpp | 6 +- .../tasks/FlightTask/SubscriptionArray.cpp | 4 +- .../tasks/FlightTask/SubscriptionArray.hpp | 13 +- .../tasks/Manual/FlightTaskManual.hpp | 2 +- .../FlightTaskManualAltitude.hpp | 2 +- .../tasks/Offboard/FlightTaskOffboard.hpp | 2 +- .../tasks/Utility/ObstacleAvoidance.hpp | 4 +- src/lib/controllib/block/Block.cpp | 6 +- src/lib/controllib/block/Block.hpp | 6 +- src/modules/commander/Commander.hpp | 17 +- src/modules/commander/PreflightCheck.cpp | 16 +- .../failure_detector/FailureDetector.hpp | 6 +- .../FixedwingAttitudeControl.hpp | 4 +- .../FixedwingPositionControl.hpp | 6 +- .../GroundRoverPositionControl.cpp | 4 +- .../GroundRoverPositionControl.hpp | 6 +- .../BlockLocalPositionEstimator.cpp | 2 +- .../BlockLocalPositionEstimator.hpp | 38 +-- src/modules/logger/logger.cpp | 2 +- src/modules/navigator/geofence.h | 2 +- .../navigator/mission_feasibility_checker.cpp | 3 +- src/modules/navigator/navigator.h | 2 +- src/modules/sensors/voted_sensors_update.cpp | 12 +- src/modules/uORB/CMakeLists.txt | 7 +- src/modules/uORB/Subscription.cpp | 87 +++--- src/modules/uORB/Subscription.hpp | 255 ++++++++++-------- src/modules/uORB/SubscriptionPollable.cpp | 110 ++++++++ src/modules/uORB/SubscriptionPollable.hpp | 197 ++++++++++++++ src/modules/uORB/uORBDeviceNode.cpp | 93 +++++-- src/modules/uORB/uORBDeviceNode.hpp | 49 ++++ .../wind_estimator/wind_estimator_main.cpp | 2 +- src/systemcmds/tests/test_microbench_uorb.cpp | 39 ++- 37 files changed, 766 insertions(+), 270 deletions(-) create mode 100644 src/modules/uORB/SubscriptionPollable.cpp create mode 100644 src/modules/uORB/SubscriptionPollable.hpp diff --git a/src/drivers/md25/md25.hpp b/src/drivers/md25/md25.hpp index c59b97a972..f672ae9958 100644 --- a/src/drivers/md25/md25.hpp +++ b/src/drivers/md25/md25.hpp @@ -270,7 +270,7 @@ private: struct pollfd _controlPoll; /** actuator controls subscription */ - uORB::Subscription _actuators; + uORB::SubscriptionData _actuators; // local copy of data from i2c device uint8_t _version; diff --git a/src/drivers/roboclaw/RoboClaw.hpp b/src/drivers/roboclaw/RoboClaw.hpp index f2096088b9..8fe4f5337c 100644 --- a/src/drivers/roboclaw/RoboClaw.hpp +++ b/src/drivers/roboclaw/RoboClaw.hpp @@ -45,7 +45,7 @@ #include #include -#include +#include #include #include @@ -169,7 +169,7 @@ private: struct pollfd _controlPoll; /** actuator controls subscription */ - uORB::Subscription _actuators; + uORB::SubscriptionPollable _actuators; // private data float _motor1Position; diff --git a/src/examples/segway/blocks.hpp b/src/examples/segway/blocks.hpp index f5b95b36b6..7d7d0a7f52 100644 --- a/src/examples/segway/blocks.hpp +++ b/src/examples/segway/blocks.hpp @@ -48,7 +48,7 @@ #include #include -#include +#include #include #include #include @@ -92,14 +92,14 @@ class __EXPORT BlockUorbEnabledAutopilot : public SuperBlock { protected: // subscriptions - uORB::Subscription _manual; - uORB::Subscription _param_update; - uORB::Subscription _missionCmd; - uORB::Subscription _att; - uORB::Subscription _attCmd; - uORB::Subscription _pos; - uORB::Subscription _ratesCmd; - uORB::Subscription _status; + uORB::SubscriptionPollable _manual; + uORB::SubscriptionPollable _param_update; + uORB::SubscriptionPollable _missionCmd; + uORB::SubscriptionPollable _att; + uORB::SubscriptionPollable _attCmd; + uORB::SubscriptionPollable _pos; + uORB::SubscriptionPollable _ratesCmd; + uORB::SubscriptionPollable _status; // publications uORB::Publication _actuators; diff --git a/src/lib/CollisionPrevention/CollisionPrevention.hpp b/src/lib/CollisionPrevention/CollisionPrevention.hpp index 7689fdd845..19fab2e1b7 100644 --- a/src/lib/CollisionPrevention/CollisionPrevention.hpp +++ b/src/lib/CollisionPrevention/CollisionPrevention.hpp @@ -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 *_sub_obstacle_distance{nullptr}; /**< obstacle distances received form a range sensor */ + uORB::SubscriptionPollable *_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; diff --git a/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.hpp b/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.hpp index 0af1e4c679..d9cc18ce79 100644 --- a/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.hpp +++ b/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.hpp @@ -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 *_sub_home_position{nullptr}; + uORB::SubscriptionPollable *_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 *_sub_triplet_setpoint{nullptr}; - uORB::Subscription *_sub_vehicle_status{nullptr}; + uORB::SubscriptionPollable *_sub_triplet_setpoint{nullptr}; + uORB::SubscriptionPollable *_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. */ diff --git a/src/lib/FlightTasks/tasks/FlightTask/FlightTask.hpp b/src/lib/FlightTasks/tasks/FlightTask/FlightTask.hpp index 19984514b8..b6fd32ae0b 100644 --- a/src/lib/FlightTasks/tasks/FlightTask/FlightTask.hpp +++ b/src/lib/FlightTasks/tasks/FlightTask/FlightTask.hpp @@ -44,7 +44,7 @@ #include #include #include -#include +#include #include #include #include @@ -181,8 +181,8 @@ public: protected: - uORB::Subscription *_sub_vehicle_local_position{nullptr}; - uORB::Subscription *_sub_attitude{nullptr}; + uORB::SubscriptionPollable *_sub_vehicle_local_position{nullptr}; + uORB::SubscriptionPollable *_sub_attitude{nullptr}; uint8_t _heading_reset_counter{0}; /**< estimator heading reset */ /** Reset all setpoints to NAN */ diff --git a/src/lib/FlightTasks/tasks/FlightTask/SubscriptionArray.cpp b/src/lib/FlightTasks/tasks/FlightTask/SubscriptionArray.cpp index f56e47fb2f..5ff7503435 100644 --- a/src/lib/FlightTasks/tasks/FlightTask/SubscriptionArray.cpp +++ b/src/lib/FlightTasks/tasks/FlightTask/SubscriptionArray.cpp @@ -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; } diff --git a/src/lib/FlightTasks/tasks/FlightTask/SubscriptionArray.hpp b/src/lib/FlightTasks/tasks/FlightTask/SubscriptionArray.hpp index b5bc0586d8..276ec4cfc0 100644 --- a/src/lib/FlightTasks/tasks/FlightTask/SubscriptionArray.hpp +++ b/src/lib/FlightTasks/tasks/FlightTask/SubscriptionArray.hpp @@ -41,7 +41,7 @@ #pragma once -#include +#include class SubscriptionArray { @@ -57,7 +57,7 @@ public: * @return true on success, false otherwise (subscription set to nullptr) */ template - bool get(const struct orb_metadata *meta, uORB::Subscription *&subscription, unsigned instance = 0); + bool get(const struct orb_metadata *meta, uORB::SubscriptionPollable *&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 -bool SubscriptionArray::get(const struct orb_metadata *meta, uORB::Subscription *&subscription, unsigned instance) +bool SubscriptionArray::get(const struct orb_metadata *meta, uORB::SubscriptionPollable *&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*>(_subscriptions[i]); + subscription = reinterpret_cast*>(_subscriptions[i]); return true; } } @@ -100,7 +101,7 @@ bool SubscriptionArray::get(const struct orb_metadata *meta, uORB::Subscription< } } - subscription = new uORB::Subscription(meta, 0, instance); + subscription = new uORB::SubscriptionPollable(meta, 0, instance); if (!subscription) { return false; diff --git a/src/lib/FlightTasks/tasks/Manual/FlightTaskManual.hpp b/src/lib/FlightTasks/tasks/Manual/FlightTaskManual.hpp index 7fb49a75bb..2ee0a37f7b 100644 --- a/src/lib/FlightTasks/tasks/Manual/FlightTaskManual.hpp +++ b/src/lib/FlightTasks/tasks/Manual/FlightTaskManual.hpp @@ -68,7 +68,7 @@ private: bool _evaluateSticks(); /**< checks and sets stick inputs */ void _applyGearSwitch(uint8_t gswitch); /**< Sets gears according to switch */ - uORB::Subscription *_sub_manual_control_setpoint{nullptr}; + uORB::SubscriptionPollable *_sub_manual_control_setpoint{nullptr}; DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTask, (ParamFloat) _param_mpc_hold_dz, /**< 0-deadzone around the center for the sticks */ diff --git a/src/lib/FlightTasks/tasks/ManualAltitude/FlightTaskManualAltitude.hpp b/src/lib/FlightTasks/tasks/ManualAltitude/FlightTaskManualAltitude.hpp index a0ea75424c..13a808074d 100644 --- a/src/lib/FlightTasks/tasks/ManualAltitude/FlightTaskManualAltitude.hpp +++ b/src/lib/FlightTasks/tasks/ManualAltitude/FlightTaskManualAltitude.hpp @@ -110,7 +110,7 @@ private: */ void _respectGroundSlowdown(); - uORB::Subscription *_sub_home_position{nullptr}; + uORB::SubscriptionPollable *_sub_home_position{nullptr}; uint8_t _reset_counter = 0; /**< counter for estimator resets in z-direction */ float _max_speed_up = 10.0f; diff --git a/src/lib/FlightTasks/tasks/Offboard/FlightTaskOffboard.hpp b/src/lib/FlightTasks/tasks/Offboard/FlightTaskOffboard.hpp index c1a1e81791..fd42e6298f 100644 --- a/src/lib/FlightTasks/tasks/Offboard/FlightTaskOffboard.hpp +++ b/src/lib/FlightTasks/tasks/Offboard/FlightTaskOffboard.hpp @@ -53,7 +53,7 @@ public: bool updateInitialize() override; protected: - uORB::Subscription *_sub_triplet_setpoint{nullptr}; + uORB::SubscriptionPollable *_sub_triplet_setpoint{nullptr}; private: matrix::Vector3f _position_lock{}; diff --git a/src/lib/FlightTasks/tasks/Utility/ObstacleAvoidance.hpp b/src/lib/FlightTasks/tasks/Utility/ObstacleAvoidance.hpp index 903d8f35eb..3e0849c30e 100644 --- a/src/lib/FlightTasks/tasks/Utility/ObstacleAvoidance.hpp +++ b/src/lib/FlightTasks/tasks/Utility/ObstacleAvoidance.hpp @@ -105,8 +105,8 @@ public: private: - uORB::Subscription *_sub_vehicle_trajectory_waypoint{nullptr}; /**< vehicle trajectory waypoint subscription */ - uORB::Subscription *_sub_vehicle_status{nullptr}; /**< vehicle status subscription */ + uORB::SubscriptionPollable *_sub_vehicle_trajectory_waypoint{nullptr}; /**< vehicle trajectory waypoint subscription */ + uORB::SubscriptionPollable *_sub_vehicle_status{nullptr}; /**< vehicle status subscription */ DEFINE_PARAMETERS( (ParamFloat) _param_nav_mc_alt_rad /**< Acceptance radius for multicopter altitude */ diff --git a/src/lib/controllib/block/Block.cpp b/src/lib/controllib/block/Block.cpp index 43171a0dfb..17454c4e33 100644 --- a/src/lib/controllib/block/Block.cpp +++ b/src/lib/controllib/block/Block.cpp @@ -42,7 +42,7 @@ #include -#include +#include #include 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; +template class List; template class List; template class List; diff --git a/src/lib/controllib/block/Block.hpp b/src/lib/controllib/block/Block.hpp index 3d12098244..d39a24ed9c 100644 --- a/src/lib/controllib/block/Block.hpp +++ b/src/lib/controllib/block/Block.hpp @@ -41,7 +41,7 @@ #include #include -#include +#include #include namespace control @@ -87,7 +87,7 @@ protected: virtual void updateParamsSubclass() {} SuperBlock *getParent() { return _parent; } - List &getSubscriptions() { return _subscriptions; } + List &getSubscriptions() { return _subscriptions; } List &getPublications() { return _publications; } List &getParams() { return _params; } @@ -95,7 +95,7 @@ protected: SuperBlock *_parent; float _dt{0.0f}; - List _subscriptions; + List _subscriptions; List _publications; List _params; }; diff --git a/src/modules/commander/Commander.hpp b/src/modules/commander/Commander.hpp index 428ca56abb..ba389b6e5a 100644 --- a/src/modules/commander/Commander.hpp +++ b/src/modules/commander/Commander.hpp @@ -62,8 +62,6 @@ #include 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_sub{ORB_ID(airspeed)}; - Subscription _estimator_status_sub{ORB_ID(estimator_status)}; - Subscription _mission_result_sub{ORB_ID(mission_result)}; - Subscription _sensor_bias_sub{ORB_ID(sensor_bias)}; - Subscription _global_position_sub{ORB_ID(vehicle_global_position)}; - Subscription _local_position_sub{ORB_ID(vehicle_local_position)}; + uORB::SubscriptionData _airspeed_sub{ORB_ID(airspeed)}; + uORB::SubscriptionData _estimator_status_sub{ORB_ID(estimator_status)}; + uORB::SubscriptionData _mission_result_sub{ORB_ID(mission_result)}; + uORB::SubscriptionData _sensor_bias_sub{ORB_ID(sensor_bias)}; + uORB::SubscriptionData _global_position_sub{ORB_ID(vehicle_global_position)}; + uORB::SubscriptionData _local_position_sub{ORB_ID(vehicle_local_position)}; - Publication _home_pub{ORB_ID(home_position)}; + // Publications + uORB::Publication _home_pub{ORB_ID(home_position)}; orb_advert_t _status_pub{nullptr}; }; diff --git a/src/modules/commander/PreflightCheck.cpp b/src/modules/commander/PreflightCheck.cpp index 9952e8ec9b..e9fa2279e3 100644 --- a/src/modules/commander/PreflightCheck.cpp +++ b/src/modules/commander/PreflightCheck.cpp @@ -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 magnetometer{ORB_ID(sensor_mag), 0, instance}; + uORB::SubscriptionData 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 accel{ORB_ID(sensor_accel), 0, instance}; + uORB::SubscriptionData 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 gyro{ORB_ID(sensor_gyro), 0, instance}; + uORB::SubscriptionData 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 baro{ORB_ID(sensor_baro), 0, instance}; + uORB::SubscriptionData baro{ORB_ID(sensor_baro), instance}; baro_valid = (hrt_elapsed_time(&baro.get().timestamp) < 1_s); diff --git a/src/modules/commander/failure_detector/FailureDetector.hpp b/src/modules/commander/failure_detector/FailureDetector.hpp index c354effde6..fb5a9eb682 100644 --- a/src/modules/commander/failure_detector/FailureDetector.hpp +++ b/src/modules/commander/failure_detector/FailureDetector.hpp @@ -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 _sub_vehicle_attitude_setpoint; - Subscription _sub_vehicule_attitude; + SubscriptionData _sub_vehicle_attitude_setpoint; + SubscriptionData _sub_vehicule_attitude; uint8_t _status{FAILURE_NONE}; diff --git a/src/modules/fw_att_control/FixedwingAttitudeControl.hpp b/src/modules/fw_att_control/FixedwingAttitudeControl.hpp index 53fe29f03d..2991c2960c 100644 --- a/src/modules/fw_att_control/FixedwingAttitudeControl.hpp +++ b/src/modules/fw_att_control/FixedwingAttitudeControl.hpp @@ -64,7 +64,7 @@ using matrix::Eulerf; using matrix::Quatf; -using uORB::Subscription; +using uORB::SubscriptionData; class FixedwingAttitudeControl final : public ModuleBase { @@ -122,7 +122,7 @@ private: vehicle_rates_setpoint_s _rates_sp {}; /* attitude rates setpoint */ vehicle_status_s _vehicle_status {}; /**< vehicle status */ - Subscription _airspeed_sub; + SubscriptionData _airspeed_sub; perf_counter_t _loop_perf; /**< loop performance counter */ perf_counter_t _nonfinite_input_perf; /**< performance counter for non finite input */ diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp index 44a9d53784..3229e4a673 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp @@ -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 _sub_airspeed; - Subscription _sub_sensors; + SubscriptionData _sub_airspeed; + SubscriptionData _sub_sensors; perf_counter_t _loop_perf; ///< loop performance counter */ diff --git a/src/modules/gnd_pos_control/GroundRoverPositionControl.cpp b/src/modules/gnd_pos_control/GroundRoverPositionControl.cpp index 10cf8e40af..e5aaf392a1 100644 --- a/src/modules/gnd_pos_control/GroundRoverPositionControl.cpp +++ b/src/modules/gnd_pos_control/GroundRoverPositionControl.cpp @@ -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"); diff --git a/src/modules/gnd_pos_control/GroundRoverPositionControl.hpp b/src/modules/gnd_pos_control/GroundRoverPositionControl.hpp index cd910422a0..d1b6b60e93 100644 --- a/src/modules/gnd_pos_control/GroundRoverPositionControl.hpp +++ b/src/modules/gnd_pos_control/GroundRoverPositionControl.hpp @@ -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 _sub_attitude; - Subscription _sub_sensors; + SubscriptionData _sub_attitude; + SubscriptionData _sub_sensors; perf_counter_t _loop_perf; /**< loop performance counter */ diff --git a/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp b/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp index de0620b9f0..b59b4c766d 100644 --- a/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp +++ b/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp @@ -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 *s = _dist_subs[i]; + uORB::SubscriptionPollable *s = _dist_subs[i]; if (s == _sub_lidar || s == _sub_sonar) { continue; } diff --git a/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp b/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp index 7c816533ee..62e559e228 100644 --- a/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp +++ b/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp @@ -9,7 +9,7 @@ #include // uORB Subscriptions -#include +#include #include #include #include @@ -246,24 +246,24 @@ private: // ---------------------------- // subscriptions - uORB::Subscription _sub_armed; - uORB::Subscription _sub_land; - uORB::Subscription _sub_att; - uORB::Subscription _sub_flow; - uORB::Subscription _sub_sensor; - uORB::Subscription _sub_param_update; - uORB::Subscription _sub_gps; - uORB::Subscription _sub_visual_odom; - uORB::Subscription _sub_mocap_odom; - uORB::Subscription _sub_dist0; - uORB::Subscription _sub_dist1; - uORB::Subscription _sub_dist2; - uORB::Subscription _sub_dist3; - uORB::Subscription *_dist_subs[N_DIST_SUBS]; - uORB::Subscription *_sub_lidar; - uORB::Subscription *_sub_sonar; - uORB::Subscription _sub_landing_target_pose; - uORB::Subscription _sub_airdata; + uORB::SubscriptionPollable _sub_armed; + uORB::SubscriptionPollable _sub_land; + uORB::SubscriptionPollable _sub_att; + uORB::SubscriptionPollable _sub_flow; + uORB::SubscriptionPollable _sub_sensor; + uORB::SubscriptionPollable _sub_param_update; + uORB::SubscriptionPollable _sub_gps; + uORB::SubscriptionPollable _sub_visual_odom; + uORB::SubscriptionPollable _sub_mocap_odom; + uORB::SubscriptionPollable _sub_dist0; + uORB::SubscriptionPollable _sub_dist1; + uORB::SubscriptionPollable _sub_dist2; + uORB::SubscriptionPollable _sub_dist3; + uORB::SubscriptionPollable *_dist_subs[N_DIST_SUBS]; + uORB::SubscriptionPollable *_sub_lidar; + uORB::SubscriptionPollable *_sub_sonar; + uORB::SubscriptionPollable _sub_landing_target_pose; + uORB::SubscriptionPollable _sub_airdata; // publications uORB::Publication _pub_lpos; diff --git a/src/modules/logger/logger.cpp b/src/modules/logger/logger.cpp index 877725b85e..cd4a9f3760 100644 --- a/src/modules/logger/logger.cpp +++ b/src/modules/logger/logger.cpp @@ -934,7 +934,7 @@ void Logger::run() } int vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status)); - uORB::Subscription parameter_update_sub(ORB_ID(parameter_update)); + uORB::SubscriptionData parameter_update_sub(ORB_ID(parameter_update)); int log_message_sub = orb_subscribe(ORB_ID(log_message)); orb_set_interval(log_message_sub, 20); diff --git a/src/modules/navigator/geofence.h b/src/modules/navigator/geofence.h index f9ed6f8ac4..35d20f9b2d 100644 --- a/src/modules/navigator/geofence.h +++ b/src/modules/navigator/geofence.h @@ -166,7 +166,7 @@ private: (ParamFloat) _param_gf_max_ver_dist ) - uORB::Subscription _sub_airdata; + uORB::SubscriptionData _sub_airdata; int _outside_counter{0}; uint16_t _update_counter{0}; ///< dataman update counter: if it does not match, we polygon data was updated diff --git a/src/modules/navigator/mission_feasibility_checker.cpp b/src/modules/navigator/mission_feasibility_checker.cpp index f4f084321c..c2b748f175 100644 --- a/src/modules/navigator/mission_feasibility_checker.cpp +++ b/src/modules/navigator/mission_feasibility_checker.cpp @@ -450,8 +450,7 @@ MissionFeasibilityChecker::checkFixedWingLanding(const mission_s &mission, bool if (MissionBlock::item_contains_position(missionitem_previous)) { - uORB::Subscription landing_status{ORB_ID(position_controller_landing_status)}; - landing_status.forcedUpdate(); + uORB::SubscriptionData 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, diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index c62148d990..468364646f 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -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_sub{ORB_ID(position_controller_status)}; + uORB::SubscriptionData _position_controller_status_sub{ORB_ID(position_controller_status)}; uint8_t _previous_nav_state{}; /**< nav_state of the previous iteration*/ diff --git a/src/modules/sensors/voted_sensors_update.cpp b/src/modules/sensors/voted_sensors_update.cpp index 597313ebba..4ddc4a3ed2 100644 --- a/src/modules/sensors/voted_sensors_update.cpp +++ b/src/modules/sensors/voted_sensors_update.cpp @@ -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 report{ORB_ID(sensor_gyro), 0, (unsigned)topic_instance}; + uORB::SubscriptionData 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 report{ORB_ID(sensor_accel), 0, (unsigned)topic_instance}; + uORB::SubscriptionData 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 report{ORB_ID(sensor_baro), 0, (unsigned)topic_instance}; + uORB::SubscriptionData report{ORB_ID(sensor_baro), topic_instance}; int temp = _temperature_compensation.set_sensor_id_baro(report.get().device_id, topic_instance); diff --git a/src/modules/uORB/CMakeLists.txt b/src/modules/uORB/CMakeLists.txt index 319717777c..4b181265df 100644 --- a/src/modules/uORB/CMakeLists.txt +++ b/src/modules/uORB/CMakeLists.txt @@ -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() diff --git a/src/modules/uORB/Subscription.cpp b/src/modules/uORB/Subscription.cpp index 82ccfcc9f5..bc4987c895 100644 --- a/src/modules/uORB/Subscription.cpp +++ b/src/modules/uORB/Subscription.cpp @@ -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 *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 diff --git a/src/modules/uORB/Subscription.hpp b/src/modules/uORB/Subscription.hpp index c0137df574..393bbedee5 100644 --- a/src/modules/uORB/Subscription.hpp +++ b/src/modules/uORB/Subscription.hpp @@ -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 -#include -#include #include +#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 +// 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 *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 __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 *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 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 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 diff --git a/src/modules/uORB/SubscriptionPollable.cpp b/src/modules/uORB/SubscriptionPollable.cpp new file mode 100644 index 0000000000..cfeac7ef56 --- /dev/null +++ b/src/modules/uORB/SubscriptionPollable.cpp @@ -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 + +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 *list) + : SubscriptionPollableBase(meta, interval, instance) +{ + if (list != nullptr) { + list->add(this); + } +} + +} // namespace uORB diff --git a/src/modules/uORB/SubscriptionPollable.hpp b/src/modules/uORB/SubscriptionPollable.hpp new file mode 100644 index 0000000000..21de9095b9 --- /dev/null +++ b/src/modules/uORB/SubscriptionPollable.hpp @@ -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 +#include +#include +#include + +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 +{ +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 *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 __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 *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 diff --git a/src/modules/uORB/uORBDeviceNode.cpp b/src/modules/uORB/uORBDeviceNode.cpp index cfcf696787..484ae4ab24 100644 --- a/src/modules/uORB/uORBDeviceNode.cpp +++ b/src/modules/uORB/uORBDeviceNode.cpp @@ -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) { diff --git a/src/modules/uORB/uORBDeviceNode.hpp b/src/modules/uORB/uORBDeviceNode.hpp index 4fc4680984..ee0ec759dc 100644 --- a/src/modules/uORB/uORBDeviceNode.hpp +++ b/src/modules/uORB/uORBDeviceNode.hpp @@ -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 */ diff --git a/src/modules/wind_estimator/wind_estimator_main.cpp b/src/modules/wind_estimator/wind_estimator_main.cpp index 8cd71beb27..2a70b40730 100644 --- a/src/modules/wind_estimator/wind_estimator_main.cpp +++ b/src/modules/wind_estimator/wind_estimator_main.cpp @@ -324,7 +324,7 @@ int WindEstimatorModule::print_status() perf_print_counter(_perf_interval); if (_instance > -1) { - uORB::Subscription est{ORB_ID(wind_estimate), (unsigned)_instance}; + uORB::SubscriptionData est{ORB_ID(wind_estimate), (uint8_t)_instance}; est.update(); print_message(est.get()); diff --git a/src/systemcmds/tests/test_microbench_uorb.cpp b/src/systemcmds/tests/test_microbench_uorb.cpp index 96da81dc94..8d324347e5 100644 --- a/src/systemcmds/tests/test_microbench_uorb.cpp +++ b/src/systemcmds/tests/test_microbench_uorb.cpp @@ -47,6 +47,7 @@ #include #include +#include #include #include #include @@ -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