diff --git a/src/drivers/md25/md25.cpp b/src/drivers/md25/md25.cpp index 5cbcece21b..59f6a8a4d5 100644 --- a/src/drivers/md25/md25.cpp +++ b/src/drivers/md25/md25.cpp @@ -583,8 +583,7 @@ int md25Sine(const char *deviceName, uint8_t bus, uint8_t address, float amplitu float prev_revolution = md25.getRevolutions1(); // debug publication - uORB::Publication debug_msg(NULL, - ORB_ID(debug_key_value)); + uORB::PublicationData debug_msg{ORB_ID(debug_key_value)}; // sine wave for motor 1 md25.resetEncoders(); diff --git a/src/examples/segway/BlockSegwayController.cpp b/src/examples/segway/BlockSegwayController.cpp index 54aca0c793..e3fefe171c 100644 --- a/src/examples/segway/BlockSegwayController.cpp +++ b/src/examples/segway/BlockSegwayController.cpp @@ -61,6 +61,6 @@ void BlockSegwayController::update() actuators.control[CH_RIGHT] = -_manual.get().x; } - // update all publications - updatePublications(); + // publish + _actuators.update(); } diff --git a/src/examples/segway/blocks.cpp b/src/examples/segway/blocks.cpp index 695cdec2d0..2112ddb0db 100644 --- a/src/examples/segway/blocks.cpp +++ b/src/examples/segway/blocks.cpp @@ -95,7 +95,7 @@ BlockUorbEnabledAutopilot::BlockUorbEnabledAutopilot(SuperBlock *parent, const c _status(ORB_ID(vehicle_status), 20, 0, &getSubscriptions()), // publications - _actuators(ORB_ID(actuator_controls_0), -1, &getPublications()) + _actuators(ORB_ID(actuator_controls_0)) { } diff --git a/src/examples/segway/blocks.hpp b/src/examples/segway/blocks.hpp index 7d7d0a7f52..4557a76479 100644 --- a/src/examples/segway/blocks.hpp +++ b/src/examples/segway/blocks.hpp @@ -102,7 +102,8 @@ protected: uORB::SubscriptionPollable _status; // publications - uORB::Publication _actuators; + uORB::PublicationData _actuators{ORB_ID(actuator_controls_0)}; + public: BlockUorbEnabledAutopilot(SuperBlock *parent, const char *name); virtual ~BlockUorbEnabledAutopilot() = default; diff --git a/src/lib/controllib/block/Block.cpp b/src/lib/controllib/block/Block.cpp index 17454c4e33..cfb10da0c8 100644 --- a/src/lib/controllib/block/Block.cpp +++ b/src/lib/controllib/block/Block.cpp @@ -43,7 +43,6 @@ #include #include -#include namespace control { @@ -118,24 +117,6 @@ void Block::updateSubscriptions() } } -void Block::updatePublications() -{ - uORB::PublicationNode *pub = getPublications().getHead(); - int count = 0; - - while (pub != nullptr) { - if (count++ > maxPublicationsPerBlock) { - char name[blockNameLengthMax]; - getName(name, blockNameLengthMax); - PX4_ERR("exceeded max publications for block: %s", name); - break; - } - - pub->update(); - pub = pub->getSibling(); - } -} - void SuperBlock::setDt(float dt) { Block::setDt(dt); @@ -191,26 +172,7 @@ void SuperBlock::updateChildSubscriptions() } } -void SuperBlock::updateChildPublications() -{ - Block *child = getChildren().getHead(); - int count = 0; - - while (child != nullptr) { - if (count++ > maxChildrenPerBlock) { - char name[blockNameLengthMax]; - getName(name, blockNameLengthMax); - PX4_ERR("exceeded max children for block: %s", name); - break; - } - - child->updatePublications(); - child = child->getSibling(); - } -} - } // namespace control 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 d39a24ed9c..2f00878d1d 100644 --- a/src/lib/controllib/block/Block.hpp +++ b/src/lib/controllib/block/Block.hpp @@ -40,7 +40,6 @@ #pragma once #include -#include #include #include @@ -50,7 +49,6 @@ namespace control static constexpr uint8_t maxChildrenPerBlock = 100; static constexpr uint8_t maxParamsPerBlock = 110; static constexpr uint8_t maxSubscriptionsPerBlock = 100; -static constexpr uint8_t maxPublicationsPerBlock = 100; static constexpr uint8_t blockNameLengthMax = 40; // forward declaration @@ -77,7 +75,6 @@ public: virtual void updateParams(); virtual void updateSubscriptions(); - virtual void updatePublications(); virtual void setDt(float dt) { _dt = dt; } float getDt() { return _dt; } @@ -88,7 +85,6 @@ protected: SuperBlock *getParent() { return _parent; } List &getSubscriptions() { return _subscriptions; } - List &getPublications() { return _publications; } List &getParams() { return _params; } const char *_name; @@ -96,7 +92,6 @@ protected: float _dt{0.0f}; List _subscriptions; - List _publications; List _params; }; @@ -130,18 +125,11 @@ public: if (getChildren().getHead() != nullptr) { updateChildSubscriptions(); } } - void updatePublications() override - { - Block::updatePublications(); - - if (getChildren().getHead() != nullptr) { updateChildPublications(); } - } protected: List &getChildren() { return _children; } void updateChildParams(); void updateChildSubscriptions(); - void updateChildPublications(); List _children; }; diff --git a/src/lib/drivers/accelerometer/PX4Accelerometer.hpp b/src/lib/drivers/accelerometer/PX4Accelerometer.hpp index 624fcfaba2..cfc872bfbb 100644 --- a/src/lib/drivers/accelerometer/PX4Accelerometer.hpp +++ b/src/lib/drivers/accelerometer/PX4Accelerometer.hpp @@ -68,7 +68,7 @@ private: void configure_filter(float cutoff_freq) { _filter.set_cutoff_frequency(_sample_rate, cutoff_freq); } - uORB::Publication _sensor_accel_pub; + uORB::PublicationData _sensor_accel_pub; math::LowPassFilter2pVector3f _filter{1000, 100}; Integrator _integrator{4000, false}; diff --git a/src/lib/drivers/barometer/PX4Barometer.hpp b/src/lib/drivers/barometer/PX4Barometer.hpp index beb52b2972..a72e4038d1 100644 --- a/src/lib/drivers/barometer/PX4Barometer.hpp +++ b/src/lib/drivers/barometer/PX4Barometer.hpp @@ -59,7 +59,7 @@ public: private: - uORB::Publication _sensor_baro_pub; + uORB::PublicationData _sensor_baro_pub; int _class_device_instance{-1}; diff --git a/src/lib/drivers/gyroscope/PX4Gyroscope.hpp b/src/lib/drivers/gyroscope/PX4Gyroscope.hpp index b731b1471d..274d92c8f8 100644 --- a/src/lib/drivers/gyroscope/PX4Gyroscope.hpp +++ b/src/lib/drivers/gyroscope/PX4Gyroscope.hpp @@ -68,7 +68,7 @@ private: void configure_filter(float cutoff_freq) { _filter.set_cutoff_frequency(_sample_rate, cutoff_freq); } - uORB::Publication _sensor_gyro_pub; + uORB::PublicationData _sensor_gyro_pub; math::LowPassFilter2pVector3f _filter{1000, 100}; Integrator _integrator{4000, true}; diff --git a/src/lib/drivers/magnetometer/PX4Magnetometer.hpp b/src/lib/drivers/magnetometer/PX4Magnetometer.hpp index f6f22d91d7..545a10ace4 100644 --- a/src/lib/drivers/magnetometer/PX4Magnetometer.hpp +++ b/src/lib/drivers/magnetometer/PX4Magnetometer.hpp @@ -63,7 +63,7 @@ public: private: - uORB::Publication _sensor_mag_pub; + uORB::PublicationData _sensor_mag_pub; const enum Rotation _rotation; diff --git a/src/modules/commander/Commander.hpp b/src/modules/commander/Commander.hpp index c2485d0db5..79879693f6 100644 --- a/src/modules/commander/Commander.hpp +++ b/src/modules/commander/Commander.hpp @@ -245,7 +245,7 @@ private: uORB::SubscriptionData _local_position_sub{ORB_ID(vehicle_local_position)}; // Publications - uORB::Publication _home_pub{ORB_ID(home_position)}; + uORB::PublicationData _home_pub{ORB_ID(home_position)}; orb_advert_t _status_pub{nullptr}; }; diff --git a/src/modules/ekf2/ekf2_main.cpp b/src/modules/ekf2/ekf2_main.cpp index 55e4712d31..b647ba84f2 100644 --- a/src/modules/ekf2/ekf2_main.cpp +++ b/src/modules/ekf2/ekf2_main.cpp @@ -275,18 +275,17 @@ private: uORB::Subscription _gps_subs[GPS_MAX_RECEIVERS] {{ORB_ID(vehicle_gps_position), 0}, {ORB_ID(vehicle_gps_position), 1}}; int _gps_orb_instance{ -1}; - orb_advert_t _att_pub{nullptr}; - orb_advert_t _wind_pub{nullptr}; - orb_advert_t _estimator_status_pub{nullptr}; - orb_advert_t _ekf_gps_drift_pub{nullptr}; - orb_advert_t _estimator_innovations_pub{nullptr}; - orb_advert_t _ekf2_timestamps_pub{nullptr}; - orb_advert_t _sensor_bias_pub{nullptr}; - orb_advert_t _blended_gps_pub{nullptr}; - - uORB::Publication _vehicle_local_position_pub; - uORB::Publication _vehicle_global_position_pub; - uORB::Publication _vehicle_odometry_pub; + uORB::Publication _estimator_innovations_pub{ORB_ID(ekf2_innovations)}; + uORB::Publication _ekf2_timestamps_pub{ORB_ID(ekf2_timestamps)}; + uORB::Publication _ekf_gps_drift_pub{ORB_ID(ekf_gps_drift)}; + uORB::Publication _blended_gps_pub{ORB_ID(ekf_gps_position)}; + uORB::Publication _estimator_status_pub{ORB_ID(estimator_status)}; + uORB::Publication _sensor_bias_pub{ORB_ID(sensor_bias)}; + uORB::Publication _att_pub{ORB_ID(vehicle_attitude)}; + uORB::Publication _vehicle_odometry_pub{ORB_ID(vehicle_odometry)}; + uORB::Publication _wind_pub{ORB_ID(wind_estimate)}; + uORB::PublicationData _vehicle_global_position_pub{ORB_ID(vehicle_global_position)}; + uORB::PublicationData _vehicle_local_position_pub{ORB_ID(vehicle_local_position)}; Ekf _ekf; @@ -535,9 +534,6 @@ Ekf2::Ekf2(): ModuleParams(nullptr), _perf_update_data(perf_alloc_once(PC_ELAPSED, "EKF2 data acquisition")), _perf_ekf_update(perf_alloc_once(PC_ELAPSED, "EKF2 update")), - _vehicle_local_position_pub(ORB_ID(vehicle_local_position)), - _vehicle_global_position_pub(ORB_ID(vehicle_global_position)), - _vehicle_odometry_pub(ORB_ID(vehicle_odometry)), _params(_ekf.getParamHandle()), _param_ekf2_min_obs_dt(_params->sensor_interval_min_ms), _param_ekf2_mag_delay(_params->mag_delay_ms), @@ -1066,7 +1062,7 @@ void Ekf2::run() gps.selected = _gps_select_index; // Publish to the EKF blended GPS topic - orb_publish_auto(ORB_ID(ekf_gps_position), &_blended_gps_pub, &gps, &_gps_orb_instance, ORB_PRIO_LOW); + _blended_gps_pub.publish(gps); // clear flag to avoid re-use of the same data _gps_new_output_data = false; @@ -1258,7 +1254,7 @@ void Ekf2::run() vehicle_local_position_s &lpos = _vehicle_local_position_pub.get(); // generate vehicle odometry data - vehicle_odometry_s &odom = _vehicle_odometry_pub.get(); + vehicle_odometry_s odom{}; lpos.timestamp = now; odom.timestamp = lpos.timestamp; @@ -1438,7 +1434,7 @@ void Ekf2::run() _vehicle_local_position_pub.update(); // publish vehicle odometry data - _vehicle_odometry_pub.update(); + _vehicle_odometry_pub.publish(odom); if (_ekf.global_position_is_valid() && !_preflt_fail) { // generate and publish global position data @@ -1509,12 +1505,7 @@ void Ekf2::run() bias.accel_y = sensors.accelerometer_m_s2[1] - accel_bias[1]; bias.accel_z = sensors.accelerometer_m_s2[2] - accel_bias[2]; - if (_sensor_bias_pub == nullptr) { - _sensor_bias_pub = orb_advertise(ORB_ID(sensor_bias), &bias); - - } else { - orb_publish(ORB_ID(sensor_bias), _sensor_bias_pub, &bias); - } + _sensor_bias_pub.publish(bias); } // publish estimator status @@ -1543,12 +1534,7 @@ void Ekf2::run() status.timeout_flags = 0.0f; // unused status.pre_flt_fail = _preflt_fail; - if (_estimator_status_pub == nullptr) { - _estimator_status_pub = orb_advertise(ORB_ID(estimator_status), &status); - - } else { - orb_publish(ORB_ID(estimator_status), _estimator_status_pub, &status); - } + _estimator_status_pub.publish(status); // publish GPS drift data only when updated to minimise overhead float gps_drift[3]; @@ -1562,12 +1548,7 @@ void Ekf2::run() drift_data.hspd = gps_drift[2]; drift_data.blocked = blocked; - if (_ekf_gps_drift_pub == nullptr) { - _ekf_gps_drift_pub = orb_advertise(ORB_ID(ekf_gps_drift), &drift_data); - - } else { - orb_publish(ORB_ID(ekf_gps_drift), _ekf_gps_drift_pub, &drift_data); - } + _ekf_gps_drift_pub.publish(drift_data); } { @@ -1732,23 +1713,12 @@ void Ekf2::run() _preflt_fail = false; } - if (_estimator_innovations_pub == nullptr) { - _estimator_innovations_pub = orb_advertise(ORB_ID(ekf2_innovations), &innovations); - - } else { - orb_publish(ORB_ID(ekf2_innovations), _estimator_innovations_pub, &innovations); - } + _estimator_innovations_pub.publish(innovations); } - } // publish ekf2_timestamps - if (_ekf2_timestamps_pub == nullptr) { - _ekf2_timestamps_pub = orb_advertise(ORB_ID(ekf2_timestamps), &ekf2_timestamps); - - } else { - orb_publish(ORB_ID(ekf2_timestamps), _ekf2_timestamps_pub, &ekf2_timestamps); - } + _ekf2_timestamps_pub.publish(ekf2_timestamps); } } @@ -1788,18 +1758,15 @@ bool Ekf2::publish_attitude(const sensor_combined_s &sensors, const hrt_abstime att.pitchspeed = sensors.gyro_rad[1] - gyro_bias[1]; att.yawspeed = sensors.gyro_rad[2] - gyro_bias[2]; - int instance; - orb_publish_auto(ORB_ID(vehicle_attitude), &_att_pub, &att, &instance, ORB_PRIO_HIGH); + _att_pub.publish(att); return true; } else if (_replay_mode) { // in replay mode we have to tell the replay module not to wait for an update // we do this by publishing an attitude with zero timestamp - vehicle_attitude_s att = {}; - - int instance; - orb_publish_auto(ORB_ID(vehicle_attitude), &_att_pub, &att, &instance, ORB_PRIO_HIGH); + vehicle_attitude_s att{}; + _att_pub.publish(att); } return false; @@ -1822,8 +1789,7 @@ bool Ekf2::publish_wind_estimate(const hrt_abstime ×tamp) wind_estimate.variance_north = wind_var[0]; wind_estimate.variance_east = wind_var[1]; - int instance; - orb_publish_auto(ORB_ID(wind_estimate), &_wind_pub, &wind_estimate, &instance, ORB_PRIO_DEFAULT); + _wind_pub.publish(wind_estimate); return true; } diff --git a/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp b/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp index b59b4c766d..356dd120ca 100644 --- a/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp +++ b/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp @@ -47,14 +47,6 @@ BlockLocalPositionEstimator::BlockLocalPositionEstimator() : _sub_sonar(nullptr), _sub_landing_target_pose(ORB_ID(landing_target_pose), 1000 / 40, 0, &getSubscriptions()), _sub_airdata(ORB_ID(vehicle_air_data), 0, 0, &getSubscriptions()), - - // publications - _pub_lpos(ORB_ID(vehicle_local_position), -1, &getPublications()), - _pub_gpos(ORB_ID(vehicle_global_position), -1, &getPublications()), - _pub_odom(ORB_ID(vehicle_odometry), -1, &getPublications()), - _pub_est_status(ORB_ID(estimator_status), -1, &getPublications()), - _pub_innov(ORB_ID(ekf2_innovations), -1, &getPublications()), - // map projection _map_ref(), diff --git a/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp b/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp index 62e559e228..c38e073fa3 100644 --- a/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp +++ b/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp @@ -266,11 +266,11 @@ private: uORB::SubscriptionPollable _sub_airdata; // publications - uORB::Publication _pub_lpos; - uORB::Publication _pub_gpos; - uORB::Publication _pub_odom; - uORB::Publication _pub_est_status; - uORB::Publication _pub_innov; + uORB::PublicationData _pub_lpos{ORB_ID(vehicle_local_position)}; + uORB::PublicationData _pub_gpos{ORB_ID(vehicle_global_position)}; + uORB::PublicationData _pub_odom{ORB_ID(vehicle_odometry)}; + uORB::PublicationData _pub_est_status{ORB_ID(estimator_status)}; + uORB::PublicationData _pub_innov{ORB_ID(ekf2_innovations)}; // map projection struct map_projection_reference_s _map_ref; diff --git a/src/modules/mc_att_control/mc_att_control.hpp b/src/modules/mc_att_control/mc_att_control.hpp index 4bcdba156f..ec099b4a10 100644 --- a/src/modules/mc_att_control/mc_att_control.hpp +++ b/src/modules/mc_att_control/mc_att_control.hpp @@ -41,6 +41,7 @@ #include #include #include +#include #include #include #include @@ -161,11 +162,12 @@ private: unsigned _gyro_count{1}; int _selected_gyro{0}; - orb_advert_t _v_rates_sp_pub{nullptr}; /**< rate setpoint publication */ + uORB::Publication _controller_status_pub{ORB_ID(rate_ctrl_status), ORB_PRIO_DEFAULT}; /**< controller status publication */ + uORB::Publication _landing_gear_pub{ORB_ID(landing_gear)}; + uORB::Publication _v_rates_sp_pub{ORB_ID(vehicle_rates_setpoint)}; /**< rate setpoint publication */ + orb_advert_t _actuators_0_pub{nullptr}; /**< attitude actuator controls publication */ - orb_advert_t _controller_status_pub{nullptr}; /**< controller status publication */ orb_advert_t _vehicle_attitude_setpoint_pub{nullptr}; - orb_advert_t _landing_gear_pub{nullptr}; orb_id_t _actuators_id{nullptr}; /**< pointer to correct actuator controls0 uORB metadata structure */ orb_id_t _attitude_sp_id{nullptr}; /**< pointer to correct attitude setpoint uORB metadata structure */ diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp index 013877a6c3..9a36343d1b 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -345,7 +345,7 @@ MulticopterAttitudeControl::generate_attitude_setpoint(float dt, bool reset_yaw_ _landing_gear.landing_gear = get_landing_gear_state(); _landing_gear.timestamp = hrt_absolute_time(); - orb_publish_auto(ORB_ID(landing_gear), &_landing_gear_pub, &_landing_gear, nullptr, ORB_PRIO_DEFAULT); + _landing_gear_pub.publish(_landing_gear); } /** @@ -508,7 +508,8 @@ MulticopterAttitudeControl::publish_rates_setpoint() _v_rates_sp.thrust_body[1] = 0.0f; _v_rates_sp.thrust_body[2] = -_thrust_sp; _v_rates_sp.timestamp = hrt_absolute_time(); - orb_publish_auto(ORB_ID(vehicle_rates_setpoint), &_v_rates_sp_pub, &_v_rates_sp, nullptr, ORB_PRIO_DEFAULT); + + _v_rates_sp_pub.publish(_v_rates_sp); } void @@ -522,7 +523,8 @@ MulticopterAttitudeControl::publish_rate_controller_status() rate_ctrl_status.rollspeed_integ = _rates_int(0); rate_ctrl_status.pitchspeed_integ = _rates_int(1); rate_ctrl_status.yawspeed_integ = _rates_int(2); - orb_publish_auto(ORB_ID(rate_ctrl_status), &_controller_status_pub, &rate_ctrl_status, nullptr, ORB_PRIO_DEFAULT); + + _controller_status_pub.publish(rate_ctrl_status); } void diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index 443b7f82a1..2543eae032 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -46,6 +46,7 @@ #include #include +#include #include #include #include @@ -107,11 +108,13 @@ private: Takeoff _takeoff; /**< state machine and ramp to bring the vehicle off the ground without jumps */ orb_advert_t _att_sp_pub{nullptr}; /**< attitude setpoint publication */ - orb_advert_t _traj_sp_pub{nullptr}; /**< trajectory setpoints publication */ - orb_advert_t _local_pos_sp_pub{nullptr}; /**< vehicle local position setpoint publication */ orb_advert_t _pub_vehicle_command{nullptr}; /**< vehicle command publication */ + orb_id_t _attitude_setpoint_id{nullptr}; - orb_advert_t _landing_gear_pub{nullptr}; + + uORB::Publication _landing_gear_pub{ORB_ID(landing_gear)}; + uORB::Publication _local_pos_sp_pub{ORB_ID(vehicle_local_position_setpoint)}; /**< vehicle local position setpoint publication */ + uORB::Publication _traj_sp_pub{ORB_ID(trajectory_setpoint)}; /**< trajectory setpoints publication */ int _local_pos_sub{-1}; /**< vehicle local position */ @@ -218,18 +221,6 @@ private: */ void publish_attitude(); - /** - * Publish local position setpoint. - * This is only required for logging. - */ - void publish_local_pos_sp(const vehicle_local_position_setpoint_s &local_pos_sp); - - /** - * Publish local position setpoint. - * This is only required for logging. - */ - void publish_trajectory_sp(const vehicle_local_position_setpoint_s &traj); - /** * Adjust the setpoint during landing. * Thrust is adjusted to support the land-detector during detection. @@ -573,7 +564,8 @@ MulticopterPositionControl::run() } } - publish_trajectory_sp(setpoint); + // publish trajectory setpoint + _traj_sp_pub.publish(setpoint); vehicle_constraints_s constraints = _flight_tasks.getConstraints(); landing_gear_s gear = _flight_tasks.getGear(); @@ -647,7 +639,7 @@ MulticopterPositionControl::run() // Publish local position setpoint // This message will be used by other modules (such as Landdetector) to determine // vehicle intention. - publish_local_pos_sp(local_pos_sp); + _local_pos_sp_pub.publish(local_pos_sp); // Inform FlightTask about the input and output of the velocity controller // This is used to properly initialize the velocity setpoint when onpening the position loop (position unlock) @@ -680,12 +672,7 @@ MulticopterPositionControl::run() _landing_gear.landing_gear = gear.landing_gear; _landing_gear.timestamp = hrt_absolute_time(); - if (_landing_gear_pub != nullptr) { - orb_publish(ORB_ID(landing_gear), _landing_gear_pub, &_landing_gear); - - } else { - _landing_gear_pub = orb_advertise(ORB_ID(landing_gear), &_landing_gear); - } + _landing_gear_pub.publish(_landing_gear); } _old_landing_gear_position = gear.landing_gear; @@ -975,30 +962,6 @@ MulticopterPositionControl::publish_attitude() } } -void -MulticopterPositionControl::publish_trajectory_sp(const vehicle_local_position_setpoint_s &traj) -{ - // publish trajectory - if (_traj_sp_pub != nullptr) { - orb_publish(ORB_ID(trajectory_setpoint), _traj_sp_pub, &traj); - - } else { - _traj_sp_pub = orb_advertise(ORB_ID(trajectory_setpoint), &traj); - } -} - -void -MulticopterPositionControl::publish_local_pos_sp(const vehicle_local_position_setpoint_s &local_pos_sp) -{ - // publish local position setpoint - if (_local_pos_sp_pub != nullptr) { - orb_publish(ORB_ID(vehicle_local_position_setpoint), _local_pos_sp_pub, &local_pos_sp); - - } else { - _local_pos_sp_pub = orb_advertise(ORB_ID(vehicle_local_position_setpoint), &local_pos_sp); - } -} - void MulticopterPositionControl::check_failure(bool task_failure, uint8_t nav_state) { if (!task_failure) { diff --git a/src/modules/navigator/gpsfailure.cpp b/src/modules/navigator/gpsfailure.cpp index c659e3f4b4..ff0785bec8 100644 --- a/src/modules/navigator/gpsfailure.cpp +++ b/src/modules/navigator/gpsfailure.cpp @@ -93,14 +93,7 @@ GpsFailure::on_active() q.copyTo(att_sp.q_d); att_sp.q_d_valid = true; - if (_att_sp_pub != nullptr) { - /* publish att sp*/ - orb_publish(ORB_ID(vehicle_attitude_setpoint), _att_sp_pub, &att_sp); - - } else { - /* advertise and publish */ - _att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp); - } + _att_sp_pub.publish(att_sp); /* Measure time */ if ((_param_nav_gpsf_lt.get() > FLT_EPSILON) && diff --git a/src/modules/navigator/gpsfailure.h b/src/modules/navigator/gpsfailure.h index a683a7a3d3..d956dfba53 100644 --- a/src/modules/navigator/gpsfailure.h +++ b/src/modules/navigator/gpsfailure.h @@ -43,6 +43,9 @@ #include "mission_block.h" +#include +#include + class Navigator; class GpsFailure : public MissionBlock, public ModuleParams @@ -72,7 +75,7 @@ private: hrt_abstime _timestamp_activation{0}; //*< timestamp when this mode was activated */ - orb_advert_t _att_sp_pub{nullptr}; + uORB::Publication _att_sp_pub{ORB_ID(vehicle_attitude_setpoint)}; /** * Set the GPSF item diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index fa986af6d7..ab8f9a3f12 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -440,12 +440,7 @@ MissionBlock::issue_command(const mission_item_s &item) // params[1] new value for selected actuator in ms 900...2000 actuators.control[(int)item.params[0]] = 1.0f / 2000 * -item.params[1]; - if (_actuator_pub != nullptr) { - orb_publish(ORB_ID(actuator_controls_2), _actuator_pub, &actuators); - - } else { - _actuator_pub = orb_advertise(ORB_ID(actuator_controls_2), &actuators); - } + _actuator_pub.publish(actuators); } else { _action_start = hrt_absolute_time(); diff --git a/src/modules/navigator/mission_block.h b/src/modules/navigator/mission_block.h index 9561b85ffc..ac342b65e9 100644 --- a/src/modules/navigator/mission_block.h +++ b/src/modules/navigator/mission_block.h @@ -45,6 +45,8 @@ #include #include +#include +#include #include #include #include @@ -132,5 +134,5 @@ protected: hrt_abstime _action_start{0}; hrt_abstime _time_wp_reached{0}; - orb_advert_t _actuator_pub{nullptr}; + uORB::Publication _actuator_pub{ORB_ID(actuator_controls_2)}; }; diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index 4312454473..ad5d3e7aaf 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -117,11 +117,6 @@ public: */ void load_fence_from_file(const char *filename); - /** - * Publish the geofence result - */ - void publish_geofence_result(); - void publish_vehicle_cmd(vehicle_command_s *vcmd); /** @@ -329,13 +324,14 @@ private: uORB::SubscriptionData _position_controller_status_sub{ORB_ID(position_controller_status)}; - orb_advert_t _geofence_result_pub{nullptr}; + uORB::Publication _geofence_result_pub{ORB_ID(geofence_result)}; + uORB::Publication _mission_result_pub{ORB_ID(mission_result)}; + uORB::Publication _pos_sp_triplet_pub{ORB_ID(position_setpoint_triplet)}; + uORB::Publication _vehicle_roi_pub{ORB_ID(vehicle_roi)}; + orb_advert_t _mavlink_log_pub{nullptr}; /**< the uORB advert to send messages over mavlink */ - orb_advert_t _mission_result_pub{nullptr}; - orb_advert_t _pos_sp_triplet_pub{nullptr}; orb_advert_t _vehicle_cmd_ack_pub{nullptr}; orb_advert_t _vehicle_cmd_pub{nullptr}; - orb_advert_t _vehicle_roi_pub{nullptr}; // Subscriptions home_position_s _home_pos{}; /**< home position for RTL */ diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 65ae550c52..52a159c05b 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -427,12 +427,7 @@ Navigator::run() _vroi.timestamp = hrt_absolute_time(); - if (_vehicle_roi_pub != nullptr) { - orb_publish(ORB_ID(vehicle_roi), _vehicle_roi_pub, &_vroi); - - } else { - _vehicle_roi_pub = orb_advertise(ORB_ID(vehicle_roi), &_vroi); - } + _vehicle_roi_pub.publish(_vroi); publish_vehicle_command_ack(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED); } @@ -473,7 +468,7 @@ Navigator::run() _geofence_violation_warning_sent = false; } - publish_geofence_result(); + _geofence_result_pub.publish(_geofence_result); } /* Do stuff according to navigation state set by commander */ @@ -746,12 +741,7 @@ Navigator::publish_position_setpoint_triplet() _pos_sp_triplet.timestamp = hrt_absolute_time(); /* lazily publish the position setpoint triplet only once available */ - if (_pos_sp_triplet_pub != nullptr) { - orb_publish(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_pub, &_pos_sp_triplet); - - } else { - _pos_sp_triplet_pub = orb_advertise(ORB_ID(position_setpoint_triplet), &_pos_sp_triplet); - } + _pos_sp_triplet_pub.publish(_pos_sp_triplet); _pos_sp_triplet_updated = false; } @@ -1115,14 +1105,7 @@ Navigator::publish_mission_result() _mission_result.timestamp = hrt_absolute_time(); /* lazily publish the mission result only once available */ - if (_mission_result_pub != nullptr) { - /* publish mission result */ - orb_publish(ORB_ID(mission_result), _mission_result_pub, &_mission_result); - - } else { - /* advertise and publish */ - _mission_result_pub = orb_advertise(ORB_ID(mission_result), &_mission_result); - } + _mission_result_pub.publish(_mission_result); /* reset some of the flags */ _mission_result.item_do_jump_changed = false; @@ -1132,20 +1115,6 @@ Navigator::publish_mission_result() _mission_result_updated = false; } -void -Navigator::publish_geofence_result() -{ - /* lazily publish the geofence result only once available */ - if (_geofence_result_pub != nullptr) { - /* publish mission result */ - orb_publish(ORB_ID(geofence_result), _geofence_result_pub, &_geofence_result); - - } else { - /* advertise and publish */ - _geofence_result_pub = orb_advertise(ORB_ID(geofence_result), &_geofence_result); - } -} - void Navigator::set_mission_failure(const char *reason) { diff --git a/src/modules/uORB/CMakeLists.txt b/src/modules/uORB/CMakeLists.txt index 4b181265df..6b3f4f0f81 100644 --- a/src/modules/uORB/CMakeLists.txt +++ b/src/modules/uORB/CMakeLists.txt @@ -41,7 +41,6 @@ if(NOT "${PX4_BOARD}" MATCHES "px4_io") # TODO: fix this hack (move uORB to plat MAIN uorb STACK_MAIN 2100 SRCS - Publication.cpp Subscription.cpp SubscriptionPollable.cpp uORB.cpp diff --git a/src/modules/uORB/Publication.cpp b/src/modules/uORB/Publication.cpp deleted file mode 100644 index 9811e3036e..0000000000 --- a/src/modules/uORB/Publication.cpp +++ /dev/null @@ -1,99 +0,0 @@ -/**************************************************************************** - * - * 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 Publication.cpp - * - */ - -#include "Publication.hpp" -#include - -namespace uORB -{ - -PublicationBase::PublicationBase(const struct orb_metadata *meta, int priority) : - _meta(meta), - _priority(priority) -{ -} - -PublicationBase::~PublicationBase() -{ - orb_unadvertise(_handle); -} - -bool PublicationBase::update(void *data) -{ - bool updated = false; - - if (_handle != nullptr) { - if (orb_publish(_meta, _handle, data) != PX4_OK) { - PX4_ERR("%s publish fail", _meta->o_name); - - } else { - updated = true; - } - - } else { - orb_advert_t handle = nullptr; - - if (_priority > 0) { - int instance; - handle = orb_advertise_multi(_meta, data, &instance, _priority); - - } else { - handle = orb_advertise(_meta, data); - } - - if (handle != nullptr) { - _handle = handle; - updated = true; - - } else { - PX4_ERR("%s advert fail", _meta->o_name); - } - } - - return updated; -} - -PublicationNode::PublicationNode(const struct orb_metadata *meta, int priority, List *list) : - PublicationBase(meta, priority) -{ - if (list != nullptr) { - list->add(this); - } -} - -} // namespace uORB diff --git a/src/modules/uORB/Publication.hpp b/src/modules/uORB/Publication.hpp index c9547c7b03..c146c5e853 100644 --- a/src/modules/uORB/Publication.hpp +++ b/src/modules/uORB/Publication.hpp @@ -39,7 +39,6 @@ #pragma once #include -#include #include namespace uORB @@ -49,7 +48,8 @@ namespace uORB * Base publication wrapper class, used in list traversal * of various publications. */ -class __EXPORT PublicationBase +template +class Publication { public: @@ -61,39 +61,61 @@ public: * @param priority The priority for multi pub/sub, 0-based, -1 means * don't publish as multi */ - PublicationBase(const struct orb_metadata *meta, int priority = -1); + Publication(const orb_metadata *meta, int priority = -1) : _meta(meta), _priority(priority) {} - virtual ~PublicationBase(); - - // no copy, assignment, move, move assignment - PublicationBase(const PublicationBase &) = delete; - PublicationBase &operator=(const PublicationBase &) = delete; - PublicationBase(PublicationBase &&) = delete; - PublicationBase &operator=(PublicationBase &&) = delete; + ~Publication() { orb_unadvertise(_handle); } /** - * Update the struct + * Publish the struct * @param data The uORB message struct we are updating. */ - bool update(void *data); + bool publish(const T &data) + { + bool updated = false; + + if (_handle != nullptr) { + if (orb_publish(_meta, _handle, &data) != PX4_OK) { + PX4_ERR("%s publish fail", _meta->o_name); + + } else { + updated = true; + } + + } else { + orb_advert_t handle = nullptr; + + if (_priority > 0) { + int instance; + handle = orb_advertise_multi(_meta, &data, &instance, _priority); + + } else { + handle = orb_advertise(_meta, &data); + } + + if (handle != nullptr) { + _handle = handle; + updated = true; + + } else { + PX4_ERR("%s advert fail", _meta->o_name); + } + } + + return updated; + } protected: - const struct orb_metadata *_meta; + const orb_metadata *_meta; const int _priority; orb_advert_t _handle{nullptr}; }; -/** - * alias class name so it is clear that the base class - * can be used by itself if desired - */ -typedef PublicationBase PublicationTiny; - /** * The publication base class as a list node. */ -class __EXPORT PublicationNode : public PublicationBase, public ListNode +template +class PublicationData : public Publication { public: /** @@ -102,70 +124,23 @@ public: * @param meta The uORB metadata (usually from * the ORB_ID() macro) for the topic. * @param priority The priority for multi pub, 0-based. - * @param list A list interface for adding to - * list during construction */ - PublicationNode(const struct orb_metadata *meta, int priority = -1, List *list = nullptr); - virtual ~PublicationNode() override = default; + PublicationData(const orb_metadata *meta, int priority = -1) : Publication(meta, priority) {} + ~PublicationData() = default; - /** - * This function is the callback for list traversal - * updates, a child class must implement it. - */ - virtual bool update() = 0; -}; + T &get() { return _data; } + void set(const T &data) { _data = data; } -/** - * Publication wrapper class - */ -template -class __EXPORT Publication final : public PublicationNode -{ -public: - /** - * Constructor - * - * @param meta The uORB metadata (usually from - * the ORB_ID() macro) for the topic. - * @param priority The priority for multi pub, 0-based. - * @param list A list interface for adding to - * list during construction - */ - Publication(const struct orb_metadata *meta, int priority = -1, List *list = nullptr) : - PublicationNode(meta, priority, list), - _data() - { - } - - ~Publication() override = default; - - // no copy, assignment, move, move assignment - Publication(const Publication &) = delete; - Publication &operator=(const Publication &) = delete; - Publication(Publication &&) = delete; - Publication &operator=(Publication &&) = delete; - - /* - * This function gets the T struct - * */ - T &get() { return _data; } - - /** - * Create an update function that uses the embedded struct. - */ - bool update() override - { - return PublicationBase::update((void *)(&_data)); - } - - bool update(const T &data) + // Publishes the embedded struct. + bool update() { return Publication::publish(_data); } + bool update(const T &data) { _data = data; - return update(); + return Publication::publish(_data); } private: - T _data; + T _data{}; }; } // namespace uORB