create uORB::PublicationMulti for multi publications

src/drivers/optical_flow/paw3902/PAW3902.hpp

@@ -51,7 +51,7 @@
 #include <lib/parameters/param.h>
 #include <drivers/drv_hrt.h>
 #include <drivers/drv_range_finder.h>
-#include <uORB/Publication.hpp>
+#include <uORB/PublicationMulti.hpp>
 #include <uORB/topics/optical_flow.h>
 
 /* Configuration Constants */
@@ -117,7 +117,7 @@ class PAW3902 : public device::SPI, public px4::ScheduledWorkItem
 
 	bool changeMode(Mode newMode);
 
-	uORB::Publication<optical_flow_s> _optical_flow_pub{ORB_ID(optical_flow)};
+	uORB::PublicationMulti<optical_flow_s> _optical_flow_pub{ORB_ID(optical_flow)};
 
 	perf_counter_t	_sample_perf;
 	perf_counter_t	_interval_perf;

src/lib/drivers/accelerometer/PX4Accelerometer.hpp

@@ -41,7 +41,7 @@
 #include <mathlib/math/filter/LowPassFilter2pVector3f.hpp>
 #include <px4_module_params.h>
 #include <uORB/uORB.h>
-#include <uORB/Publication.hpp>
+#include <uORB/PublicationMulti.hpp>
 #include <uORB/topics/sensor_accel.h>
 
 class PX4Accelerometer : public cdev::CDev, public ModuleParams
@@ -68,7 +68,7 @@ class PX4Accelerometer : public cdev::CDev, public ModuleParams
 
 	void configure_filter(float cutoff_freq) { _filter.set_cutoff_frequency(_sample_rate, cutoff_freq); }
 
-	uORB::PublicationData<sensor_accel_s>	_sensor_accel_pub;
+	uORB::PublicationMultiData<sensor_accel_s>	_sensor_accel_pub;
 
 	math::LowPassFilter2pVector3f _filter{1000, 100};
 	Integrator _integrator{4000, false};

src/lib/drivers/barometer/PX4Barometer.hpp

@@ -38,7 +38,7 @@
 #include <lib/cdev/CDev.hpp>
 #include <lib/conversion/rotation.h>
 #include <uORB/uORB.h>
-#include <uORB/Publication.hpp>
+#include <uORB/PublicationMulti.hpp>
 #include <uORB/topics/sensor_baro.h>
 
 class PX4Barometer : public cdev::CDev
@@ -59,7 +59,7 @@ class PX4Barometer : public cdev::CDev
 
 private:
 
-	uORB::PublicationData<sensor_baro_s>	_sensor_baro_pub;
+	uORB::PublicationMultiData<sensor_baro_s>	_sensor_baro_pub;
 
 	int			_class_device_instance{-1};
 

src/lib/drivers/gyroscope/PX4Gyroscope.hpp

@@ -41,7 +41,7 @@
 #include <mathlib/math/filter/LowPassFilter2pVector3f.hpp>
 #include <px4_module_params.h>
 #include <uORB/uORB.h>
-#include <uORB/Publication.hpp>
+#include <uORB/PublicationMulti.hpp>
 #include <uORB/topics/sensor_gyro.h>
 
 class PX4Gyroscope : public cdev::CDev, public ModuleParams
@@ -68,7 +68,7 @@ class PX4Gyroscope : public cdev::CDev, public ModuleParams
 
 	void configure_filter(float cutoff_freq) { _filter.set_cutoff_frequency(_sample_rate, cutoff_freq); }
 
-	uORB::PublicationData<sensor_gyro_s>	_sensor_gyro_pub;
+	uORB::PublicationMultiData<sensor_gyro_s>	_sensor_gyro_pub;
 
 	math::LowPassFilter2pVector3f _filter{1000, 100};
 	Integrator _integrator{4000, true};

src/lib/drivers/magnetometer/PX4Magnetometer.hpp

@@ -38,7 +38,7 @@
 #include <lib/cdev/CDev.hpp>
 #include <lib/conversion/rotation.h>
 #include <uORB/uORB.h>
-#include <uORB/Publication.hpp>
+#include <uORB/PublicationMulti.hpp>
 #include <uORB/topics/sensor_mag.h>
 
 class PX4Magnetometer : public cdev::CDev
@@ -63,7 +63,7 @@ class PX4Magnetometer : public cdev::CDev
 
 private:
 
-	uORB::PublicationData<sensor_mag_s>	_sensor_mag_pub;
+	uORB::PublicationMultiData<sensor_mag_s>	_sensor_mag_pub;
 
 	const enum Rotation	_rotation;
 

src/lib/drivers/rangefinder/PX4Rangefinder.hpp

@@ -38,7 +38,7 @@
 #include <lib/cdev/CDev.hpp>
 #include <lib/conversion/rotation.h>
 #include <uORB/uORB.h>
-#include <uORB/Publication.hpp>
+#include <uORB/PublicationMulti.hpp>
 #include <uORB/topics/distance_sensor.h>
 
 class PX4Rangefinder : public cdev::CDev
@@ -64,7 +64,7 @@ class PX4Rangefinder : public cdev::CDev
 
 private:
 
-	uORB::PublicationData<distance_sensor_s>	_distance_sensor_pub;
+	uORB::PublicationMultiData<distance_sensor_s>	_distance_sensor_pub;
 
 	int			_class_device_instance{-1};
 

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 @@ class MavlinkReceiver : public ModuleParams
 	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 @@ class MulticopterAttitudeControl : public ModuleBase<MulticopterAttitudeControl>
 
 	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 @@ class Publication
 	 * 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 @@ class Publication
 	 */
 	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 @@ class PublicationData : public Publication<T>
 	/**
 	 * 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; }