From e8a11086eb6b926d16e836157bd3ae8ac9b7d717 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Sun, 4 Aug 2019 14:19:11 -0400 Subject: [PATCH] create uORB::PublicationMulti for multi publications --- src/drivers/optical_flow/paw3902/PAW3902.hpp | 4 +- .../accelerometer/PX4Accelerometer.hpp | 4 +- src/lib/drivers/barometer/PX4Barometer.hpp | 4 +- src/lib/drivers/gyroscope/PX4Gyroscope.hpp | 4 +- .../drivers/magnetometer/PX4Magnetometer.hpp | 4 +- .../drivers/rangefinder/PX4Rangefinder.hpp | 4 +- src/modules/mavlink/mavlink_receiver.cpp | 7 +- src/modules/mavlink/mavlink_receiver.h | 21 +-- src/modules/mc_att_control/mc_att_control.hpp | 3 +- src/modules/uORB/CMakeLists.txt | 16 +++ src/modules/uORB/Publication.hpp | 40 ++---- src/modules/uORB/PublicationMulti.hpp | 133 ++++++++++++++++++ src/modules/uORB/PublicationQueued.hpp | 18 +-- 13 files changed, 193 insertions(+), 69 deletions(-) create mode 100644 src/modules/uORB/PublicationMulti.hpp diff --git a/src/drivers/optical_flow/paw3902/PAW3902.hpp b/src/drivers/optical_flow/paw3902/PAW3902.hpp index 483e5b8798..59d692d2a1 100644 --- a/src/drivers/optical_flow/paw3902/PAW3902.hpp +++ b/src/drivers/optical_flow/paw3902/PAW3902.hpp @@ -51,7 +51,7 @@ #include #include #include -#include +#include #include /* Configuration Constants */ @@ -117,7 +117,7 @@ private: bool changeMode(Mode newMode); - uORB::Publication _optical_flow_pub{ORB_ID(optical_flow)}; + uORB::PublicationMulti _optical_flow_pub{ORB_ID(optical_flow)}; perf_counter_t _sample_perf; perf_counter_t _interval_perf; diff --git a/src/lib/drivers/accelerometer/PX4Accelerometer.hpp b/src/lib/drivers/accelerometer/PX4Accelerometer.hpp index 604c8291e5..55af0320c6 100644 --- a/src/lib/drivers/accelerometer/PX4Accelerometer.hpp +++ b/src/lib/drivers/accelerometer/PX4Accelerometer.hpp @@ -41,7 +41,7 @@ #include #include #include -#include +#include #include class PX4Accelerometer : public cdev::CDev, public ModuleParams @@ -68,7 +68,7 @@ private: void configure_filter(float cutoff_freq) { _filter.set_cutoff_frequency(_sample_rate, cutoff_freq); } - uORB::PublicationData _sensor_accel_pub; + uORB::PublicationMultiData _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 a72e4038d1..ca48928119 100644 --- a/src/lib/drivers/barometer/PX4Barometer.hpp +++ b/src/lib/drivers/barometer/PX4Barometer.hpp @@ -38,7 +38,7 @@ #include #include #include -#include +#include #include class PX4Barometer : public cdev::CDev @@ -59,7 +59,7 @@ public: private: - uORB::PublicationData _sensor_baro_pub; + uORB::PublicationMultiData _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 6868038b46..50e99e4655 100644 --- a/src/lib/drivers/gyroscope/PX4Gyroscope.hpp +++ b/src/lib/drivers/gyroscope/PX4Gyroscope.hpp @@ -41,7 +41,7 @@ #include #include #include -#include +#include #include class PX4Gyroscope : public cdev::CDev, public ModuleParams @@ -68,7 +68,7 @@ private: void configure_filter(float cutoff_freq) { _filter.set_cutoff_frequency(_sample_rate, cutoff_freq); } - uORB::PublicationData _sensor_gyro_pub; + uORB::PublicationMultiData _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 545a10ace4..e909602dd8 100644 --- a/src/lib/drivers/magnetometer/PX4Magnetometer.hpp +++ b/src/lib/drivers/magnetometer/PX4Magnetometer.hpp @@ -38,7 +38,7 @@ #include #include #include -#include +#include #include class PX4Magnetometer : public cdev::CDev @@ -63,7 +63,7 @@ public: private: - uORB::PublicationData _sensor_mag_pub; + uORB::PublicationMultiData _sensor_mag_pub; const enum Rotation _rotation; diff --git a/src/lib/drivers/rangefinder/PX4Rangefinder.hpp b/src/lib/drivers/rangefinder/PX4Rangefinder.hpp index c1d484c921..c9cb7bb08e 100644 --- a/src/lib/drivers/rangefinder/PX4Rangefinder.hpp +++ b/src/lib/drivers/rangefinder/PX4Rangefinder.hpp @@ -38,7 +38,7 @@ #include #include #include -#include +#include #include class PX4Rangefinder : public cdev::CDev @@ -64,7 +64,7 @@ public: private: - uORB::PublicationData _distance_sensor_pub; + uORB::PublicationMultiData _distance_sensor_pub; int _class_device_instance{-1}; diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index fb7ac3e15c..b23945d8eb 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -518,7 +518,7 @@ MavlinkReceiver::handle_message_command_ack(mavlink_message_t *msg) mavlink_command_ack_t ack; mavlink_msg_command_ack_decode(msg, &ack); - MavlinkCommandSender::instance().handle_mavlink_command_ack(ack, msg->sysid, msg->compid); + MavlinkCommandSender::instance().handle_mavlink_command_ack(ack, msg->sysid, msg->compid, _mavlink->get_channel()); vehicle_command_ack_s command_ack{}; @@ -1608,6 +1608,7 @@ MavlinkReceiver::handle_message_trajectory_representation_waypoints(mavlink_mess trajectory_waypoint.waypoints[i].yaw = trajectory.pos_yaw[i]; trajectory_waypoint.waypoints[i].yaw_speed = trajectory.vel_yaw[i]; + trajectory_waypoint.waypoints[i].type = UINT8_MAX; } for (int i = 0; i < number_valid_points; ++i) { @@ -2216,10 +2217,6 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg) matrix::Quatf q(hil_state.attitude_quaternion); q.copyTo(hil_attitude.q); - hil_attitude.rollspeed = hil_state.rollspeed; - hil_attitude.pitchspeed = hil_state.pitchspeed; - hil_attitude.yawspeed = hil_state.yawspeed; - _attitude_pub.publish(hil_attitude); } diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index c058081ea7..21de7bb057 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -50,6 +50,7 @@ #include #include #include +#include #include #include #include @@ -237,16 +238,16 @@ private: uORB::Publication _trajectory_waypoint_pub{ORB_ID(vehicle_trajectory_waypoint)}; // ORB publications (multi) - uORB::Publication _distance_sensor_pub{ORB_ID(distance_sensor), ORB_PRIO_LOW}; - uORB::Publication _flow_distance_sensor_pub{ORB_ID(distance_sensor), ORB_PRIO_LOW}; - uORB::Publication _rc_pub{ORB_ID(input_rc), ORB_PRIO_LOW}; - uORB::Publication _manual_pub{ORB_ID(manual_control_setpoint), ORB_PRIO_LOW}; - uORB::Publication _ping_pub{ORB_ID(ping), ORB_PRIO_LOW}; - uORB::Publication _radio_status_pub{ORB_ID(radio_status), ORB_PRIO_LOW}; - uORB::Publication _accel_pub{ORB_ID(sensor_accel), ORB_PRIO_LOW}; - uORB::Publication _baro_pub{ORB_ID(sensor_baro), ORB_PRIO_LOW}; - uORB::Publication _gyro_pub{ORB_ID(sensor_gyro), ORB_PRIO_LOW}; - uORB::Publication _mag_pub{ORB_ID(sensor_mag), ORB_PRIO_LOW}; + uORB::PublicationMulti _distance_sensor_pub{ORB_ID(distance_sensor), ORB_PRIO_LOW}; + uORB::PublicationMulti _flow_distance_sensor_pub{ORB_ID(distance_sensor), ORB_PRIO_LOW}; + uORB::PublicationMulti _rc_pub{ORB_ID(input_rc), ORB_PRIO_LOW}; + uORB::PublicationMulti _manual_pub{ORB_ID(manual_control_setpoint), ORB_PRIO_LOW}; + uORB::PublicationMulti _ping_pub{ORB_ID(ping), ORB_PRIO_LOW}; + uORB::PublicationMulti _radio_status_pub{ORB_ID(radio_status), ORB_PRIO_LOW}; + uORB::PublicationMulti _accel_pub{ORB_ID(sensor_accel), ORB_PRIO_LOW}; + uORB::PublicationMulti _baro_pub{ORB_ID(sensor_baro), ORB_PRIO_LOW}; + uORB::PublicationMulti _gyro_pub{ORB_ID(sensor_gyro), ORB_PRIO_LOW}; + uORB::PublicationMulti _mag_pub{ORB_ID(sensor_mag), ORB_PRIO_LOW}; // ORB publications (queue length > 1) uORB::PublicationQueued _gps_inject_data_pub{ORB_ID(gps_inject_data)}; diff --git a/src/modules/mc_att_control/mc_att_control.hpp b/src/modules/mc_att_control/mc_att_control.hpp index 4965901da2..e871c16c15 100644 --- a/src/modules/mc_att_control/mc_att_control.hpp +++ b/src/modules/mc_att_control/mc_att_control.hpp @@ -42,6 +42,7 @@ #include #include #include +#include #include #include #include @@ -155,7 +156,7 @@ private: uORB::SubscriptionCallbackWorkItem _vehicle_angular_velocity_sub{this, ORB_ID(vehicle_angular_velocity)}; - uORB::Publication _controller_status_pub{ORB_ID(rate_ctrl_status), ORB_PRIO_DEFAULT}; /**< controller status publication */ + uORB::PublicationMulti _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 */ diff --git a/src/modules/uORB/CMakeLists.txt b/src/modules/uORB/CMakeLists.txt index 6b3f4f0f81..38ca2f9f5f 100644 --- a/src/modules/uORB/CMakeLists.txt +++ b/src/modules/uORB/CMakeLists.txt @@ -41,14 +41,30 @@ if(NOT "${PX4_BOARD}" MATCHES "px4_io") # TODO: fix this hack (move uORB to plat MAIN uorb STACK_MAIN 2100 SRCS + ORBSet.hpp + Publication.hpp + PublicationMulti.hpp + PublicationQueued.hpp Subscription.cpp + Subscription.hpp + SubscriptionCallback.hpp + SubscriptionInterval.hpp SubscriptionPollable.cpp + SubscriptionPollable.hpp uORB.cpp + uORB.h + uORBCommon.hpp + uORBCommunicator.hpp uORBDeviceMaster.cpp + uORBDeviceMaster.hpp uORBDeviceNode.cpp + uORBDeviceNode.hpp uORBMain.cpp uORBManager.cpp + uORBManager.hpp + uORBTopics.h uORBUtils.cpp + uORBUtils.hpp DEPENDS cdev uorb_msgs diff --git a/src/modules/uORB/Publication.hpp b/src/modules/uORB/Publication.hpp index f50f6cdaa3..eac2a8f52d 100644 --- a/src/modules/uORB/Publication.hpp +++ b/src/modules/uORB/Publication.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 @@ -38,8 +38,9 @@ #pragma once -#include +#include #include +#include namespace uORB { @@ -56,10 +57,8 @@ public: * Constructor * * @param meta The uORB metadata (usually from the ORB_ID() macro) for the topic. - * @param priority The priority for multi pub/sub, 0 means don't publish as multi */ - Publication(const orb_metadata *meta, uint8_t priority = 0) : _meta(meta), _priority(priority) {} - + Publication(const orb_metadata *meta) : _meta(meta) {} ~Publication() { orb_unadvertise(_handle); } /** @@ -68,44 +67,29 @@ public: */ bool publish(const T &data) { - bool updated = false; - if (_handle != nullptr) { - updated = (orb_publish(_meta, _handle, &data) == PX4_OK); + return (orb_publish(_meta, _handle, &data) == PX4_OK); } 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); - } + orb_advert_t handle = orb_advertise(_meta, &data); if (handle != nullptr) { _handle = handle; - updated = true; - - } else { - PX4_ERR("%s advert fail", _meta->o_name); + return true; } } - return updated; + return false; } protected: const orb_metadata *_meta; orb_advert_t _handle{nullptr}; - - const uint8_t _priority; }; /** - * The publication class with data. + * The publication class with data embedded. */ template class PublicationData : public Publication @@ -114,11 +98,9 @@ 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 meta The uORB metadata (usually from the ORB_ID() macro) for the topic. */ - PublicationData(const orb_metadata *meta, uint8_t priority = 0) : Publication(meta, priority) {} + PublicationData(const orb_metadata *meta) : Publication(meta) {} ~PublicationData() = default; T &get() { return _data; } diff --git a/src/modules/uORB/PublicationMulti.hpp b/src/modules/uORB/PublicationMulti.hpp new file mode 100644 index 0000000000..f00acfa753 --- /dev/null +++ b/src/modules/uORB/PublicationMulti.hpp @@ -0,0 +1,133 @@ +/**************************************************************************** + * + * Copyright (c) 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 Publication.hpp + * + */ + +#pragma once + +#include +#include +#include + +namespace uORB +{ + +/** + * Base publication multi wrapper class + */ +template +class PublicationMulti +{ +public: + + /** + * Constructor + * + * @param meta The uORB metadata (usually from the ORB_ID() macro) for the topic. + * @param priority The priority for multi pub/sub, 0 means don't publish as multi + */ + PublicationMulti(const orb_metadata *meta, uint8_t priority = ORB_PRIO_DEFAULT) : + _meta(meta), + _priority(priority) + {} + + ~PublicationMulti() { orb_unadvertise(_handle); } + + /** + * Publish the struct + * @param data The uORB message struct we are updating. + */ + bool publish(const T &data) + { + if (_handle != nullptr) { + return (orb_publish(_meta, _handle, &data) == PX4_OK); + + } else { + int instance = 0; + orb_advert_t handle = orb_advertise_multi(_meta, &data, &instance, _priority); + + if (handle != nullptr) { + _handle = handle; + return true; + } + } + + return false; + } + +protected: + const orb_metadata *_meta; + + orb_advert_t _handle{nullptr}; + + const uint8_t _priority; +}; + +/** + * The publication multi class with data embedded. + */ +template +class PublicationMultiData : public PublicationMulti +{ +public: + /** + * Constructor + * + * @param meta The uORB metadata (usually from the ORB_ID() macro) for the topic. + * @param priority The priority for multi pub + */ + PublicationMultiData(const orb_metadata *meta, uint8_t priority = ORB_PRIO_DEFAULT) : + PublicationMulti(meta, priority) + {} + + ~PublicationMultiData() = default; + + T &get() { return _data; } + void set(const T &data) { _data = data; } + + // Publishes the embedded struct. + bool update() { return PublicationMulti::publish(_data); } + bool update(const T &data) + { + _data = data; + return PublicationMulti::publish(_data); + } + +private: + T _data{}; +}; + +} // namespace uORB diff --git a/src/modules/uORB/PublicationQueued.hpp b/src/modules/uORB/PublicationQueued.hpp index d3db333106..c8080878df 100644 --- a/src/modules/uORB/PublicationQueued.hpp +++ b/src/modules/uORB/PublicationQueued.hpp @@ -38,8 +38,9 @@ #pragma once -#include #include +#include +#include namespace uORB { @@ -55,11 +56,9 @@ public: /** * Constructor * - * @param meta The uORB metadata (usually from - * the ORB_ID() macro) for the topic. + * @param meta The uORB metadata (usually from the ORB_ID() macro) for the topic. */ PublicationQueued(const orb_metadata *meta) : _meta(meta) {} - ~PublicationQueued() { orb_unadvertise(_handle); } /** @@ -68,24 +67,19 @@ public: */ bool publish(const T &data) { - bool updated = false; - if (_handle != nullptr) { - updated = (orb_publish(_meta, _handle, &data) == PX4_OK); + return (orb_publish(_meta, _handle, &data) == PX4_OK); } else { orb_advert_t handle = orb_advertise_queue(_meta, &data, T::ORB_QUEUE_LENGTH); if (handle != nullptr) { _handle = handle; - updated = true; - - } else { - PX4_ERR("%s advertise failed", _meta->o_name); + return true; } } - return updated; + return false; } protected: