create uORB::PublicationMulti for multi publications

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);
 	}
 

src/modules/mavlink/mavlink_receiver.h

@@ -50,6 +50,7 @@
 #include <px4_module_params.h>
 #include <uORB/Publication.hpp>
 #include <uORB/PublicationQueued.hpp>
+#include <uORB/PublicationMulti.hpp>
 #include <uORB/topics/actuator_armed.h>
 #include <uORB/topics/actuator_controls.h>
 #include <uORB/topics/actuator_outputs.h>
@@ -237,16 +238,16 @@ private:
 	uORB::Publication<vehicle_trajectory_waypoint_s>	_trajectory_waypoint_pub{ORB_ID(vehicle_trajectory_waypoint)};
 
 	// ORB publications (multi)
-	uORB::Publication<distance_sensor_s>		_distance_sensor_pub{ORB_ID(distance_sensor), ORB_PRIO_LOW};
-	uORB::Publication<distance_sensor_s>		_flow_distance_sensor_pub{ORB_ID(distance_sensor), ORB_PRIO_LOW};
-	uORB::Publication<input_rc_s>			_rc_pub{ORB_ID(input_rc), ORB_PRIO_LOW};
-	uORB::Publication<manual_control_setpoint_s>	_manual_pub{ORB_ID(manual_control_setpoint), ORB_PRIO_LOW};
-	uORB::Publication<ping_s>			_ping_pub{ORB_ID(ping), ORB_PRIO_LOW};
-	uORB::Publication<radio_status_s>		_radio_status_pub{ORB_ID(radio_status), ORB_PRIO_LOW};
-	uORB::Publication<sensor_accel_s>		_accel_pub{ORB_ID(sensor_accel), ORB_PRIO_LOW};
-	uORB::Publication<sensor_baro_s>		_baro_pub{ORB_ID(sensor_baro), ORB_PRIO_LOW};
-	uORB::Publication<sensor_gyro_s>		_gyro_pub{ORB_ID(sensor_gyro), ORB_PRIO_LOW};
-	uORB::Publication<sensor_mag_s>			_mag_pub{ORB_ID(sensor_mag), ORB_PRIO_LOW};
+	uORB::PublicationMulti<distance_sensor_s>		_distance_sensor_pub{ORB_ID(distance_sensor), ORB_PRIO_LOW};
+	uORB::PublicationMulti<distance_sensor_s>		_flow_distance_sensor_pub{ORB_ID(distance_sensor), ORB_PRIO_LOW};
+	uORB::PublicationMulti<input_rc_s>			_rc_pub{ORB_ID(input_rc), ORB_PRIO_LOW};
+	uORB::PublicationMulti<manual_control_setpoint_s>	_manual_pub{ORB_ID(manual_control_setpoint), ORB_PRIO_LOW};
+	uORB::PublicationMulti<ping_s>				_ping_pub{ORB_ID(ping), ORB_PRIO_LOW};
+	uORB::PublicationMulti<radio_status_s>			_radio_status_pub{ORB_ID(radio_status), ORB_PRIO_LOW};
+	uORB::PublicationMulti<sensor_accel_s>			_accel_pub{ORB_ID(sensor_accel), ORB_PRIO_LOW};
+	uORB::PublicationMulti<sensor_baro_s>			_baro_pub{ORB_ID(sensor_baro), ORB_PRIO_LOW};
+	uORB::PublicationMulti<sensor_gyro_s>			_gyro_pub{ORB_ID(sensor_gyro), ORB_PRIO_LOW};
+	uORB::PublicationMulti<sensor_mag_s>			_mag_pub{ORB_ID(sensor_mag), ORB_PRIO_LOW};
 
 	// ORB publications (queue length > 1)
 	uORB::PublicationQueued<gps_inject_data_s>	_gps_inject_data_pub{ORB_ID(gps_inject_data)};

src/modules/mc_att_control/mc_att_control.hpp

@@ -42,6 +42,7 @@
 #include <px4_posix.h>
 #include <px4_work_queue/WorkItem.hpp>
 #include <uORB/Publication.hpp>
+#include <uORB/PublicationMulti.hpp>
 #include <uORB/Subscription.hpp>
 #include <uORB/SubscriptionCallback.hpp>
 #include <uORB/topics/actuator_controls.h>
@@ -155,7 +156,7 @@ private:
 
 	uORB::SubscriptionCallbackWorkItem _vehicle_angular_velocity_sub{this, ORB_ID(vehicle_angular_velocity)};
 
-	uORB::Publication<rate_ctrl_status_s>		_controller_status_pub{ORB_ID(rate_ctrl_status), ORB_PRIO_DEFAULT};	/**< controller status publication */
+	uORB::PublicationMulti<rate_ctrl_status_s>	_controller_status_pub{ORB_ID(rate_ctrl_status), ORB_PRIO_DEFAULT};	/**< controller status publication */
 	uORB::Publication<landing_gear_s>		_landing_gear_pub{ORB_ID(landing_gear)};
 	uORB::Publication<vehicle_rates_setpoint_s>	_v_rates_sp_pub{ORB_ID(vehicle_rates_setpoint)};			/**< rate setpoint publication */
 

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

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 <uORB/uORB.h>
+#include <px4_defines.h>
 #include <systemlib/err.h>
+#include <uORB/uORB.h>
 
 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<typename T>
 class PublicationData : public Publication<T>
@@ -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<T>(meta, priority) {}
+	PublicationData(const orb_metadata *meta) : Publication<T>(meta) {}
 	~PublicationData() = default;
 
 	T	&get() { return _data; }

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 <px4_defines.h>
+#include <systemlib/err.h>
+#include <uORB/uORB.h>
+
+namespace uORB
+{
+
+/**
+ * Base publication multi wrapper class
+ */
+template<typename T>
+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<typename T>
+class PublicationMultiData : public PublicationMulti<T>
+{
+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<T>(meta, priority)
+	{}
+
+	~PublicationMultiData() = default;
+
+	T	&get() { return _data; }
+	void	set(const T &data) { _data = data; }
+
+	// Publishes the embedded struct.
+	bool	update() { return PublicationMulti<T>::publish(_data); }
+	bool	update(const T &data)
+	{
+		_data = data;
+		return PublicationMulti<T>::publish(_data);
+	}
+
+private:
+	T _data{};
+};
+
+} // namespace uORB

src/modules/uORB/PublicationQueued.hpp

@@ -38,8 +38,9 @@
 
 #pragma once
 
-#include <uORB/uORB.h>
 #include <px4_defines.h>
+#include <systemlib/err.h>
+#include <uORB/uORB.h>
 
 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: