uorb: do not open a node exclusively for an advertiser

Exclusive open is not required, but we now need to ensure the queue size
is set atomically.

It avoids a race condition between 2 single-instance advertisers,
where one of them would fail to open the node with -EBUSY.

ROMFS/px4fmu_common/init.d-posix/rcS

@@ -162,15 +162,15 @@ fi
 
 # Adapt timeout parameters if simulation runs faster or slower than realtime.
 if [ ! -z $PX4_SIM_SPEED_FACTOR ]; then
-	COM_DL_LOSS_T_LONGER=$((PX4_SIM_SPEED_FACTOR * 10))
+	COM_DL_LOSS_T_LONGER=$(echo "$PX4_SIM_SPEED_FACTOR * 10" | bc)
 	echo "COM_DL_LOSS_T set to $COM_DL_LOSS_T_LONGER"
 	param set COM_DL_LOSS_T $COM_DL_LOSS_T_LONGER
 
-	COM_RC_LOSS_T_LONGER=$((PX4_SIM_SPEED_FACTOR * 1))
+	COM_RC_LOSS_T_LONGER=$(echo "$PX4_SIM_SPEED_FACTOR * 1" | bc)
 	echo "COM_RC_LOSS_T set to $COM_RC_LOSS_T_LONGER"
 	param set COM_RC_LOSS_T $COM_RC_LOSS_T_LONGER
 
-	COM_OF_LOSS_T_LONGER=$((PX4_SIM_SPEED_FACTOR * 10))
+	COM_OF_LOSS_T_LONGER=$(echo "$PX4_SIM_SPEED_FACTOR * 10" | bc)
 	echo "COM_OF_LOSS_T set to $COM_OF_LOSS_T_LONGER"
 	param set COM_OF_LOSS_T $COM_OF_LOSS_T_LONGER
 fi

boards/px4/fmu-v5/init/rc.board_sensors

@@ -20,6 +20,7 @@ ist8310 -C -b 1 start
 ist8310 -C -b 2 start
 hmc5883 -C -T -X start
 qmc5883 -X start
+lis3mdl -X start
 
 # Possible internal compass
 ist8310 -C -b 5 start

src/drivers/drv_orb_dev.h

@@ -73,7 +73,7 @@
 /** Get the minimum interval at which the topic can be seen to be updated for this subscription */
 #define ORBIOCGETINTERVAL	_ORBIOC(16)
 
-/** Check whether the topic is published, sets *(unsigned long *)arg to 1 if published, 0 otherwise */
-#define ORBIOCISPUBLISHED	_ORBIOC(17)
+/** Check whether the topic is advertised, sets *(unsigned long *)arg to 1 if advertised, 0 otherwise */
+#define ORBIOCISADVERTISED	_ORBIOC(17)
 
 #endif /* _DRV_UORB_H */

src/lib/FlightTasks/tasks/AutoMapper/FlightTaskAutoMapper.cpp

@@ -122,7 +122,7 @@ void FlightTaskAutoMapper::_generateLandSetpoints()
 	_position_setpoint = Vector3f(_target(0), _target(1), NAN);
 	_velocity_setpoint = Vector3f(Vector3f(NAN, NAN, _param_mpc_land_speed.get()));
 	// set constraints
-	_constraints.tilt = _param_mpc_tiltmax_lnd.get();
+	_constraints.tilt = math::radians(_param_mpc_tiltmax_lnd.get());
 	_constraints.speed_down = _param_mpc_land_speed.get();
 	_gear.landing_gear = landing_gear_s::GEAR_DOWN;
 }

src/lib/FlightTasks/tasks/AutoMapper2/FlightTaskAutoMapper2.cpp

@@ -132,7 +132,7 @@ void FlightTaskAutoMapper2::_prepareLandSetpoints()
 	_velocity_setpoint = Vector3f(Vector3f(NAN, NAN, speed_lnd));
 
 	// set constraints
-	_constraints.tilt = _param_mpc_tiltmax_lnd.get();
+	_constraints.tilt = math::radians(_param_mpc_tiltmax_lnd.get());
 	_gear.landing_gear = landing_gear_s::GEAR_DOWN;
 }
 

src/modules/ekf2/ekf2_main.cpp

@@ -966,8 +966,8 @@ void Ekf2::run()
 					const float y_v2 = fminf(vel_body_wind(1) * vel_body_wind(1), max_airspeed_sq);
 					const float z_v2 = fminf(vel_body_wind(2) * vel_body_wind(2), max_airspeed_sq);
 
-					const float pstatic_err = 0.5f * airdata.rho *
-								  (K_pstatic_coef_x * x_v2) + (K_pstatic_coef_y * y_v2) + (_param_ekf2_pcoef_z.get() * z_v2);
+					const float pstatic_err = 0.5f * airdata.rho * (
+									  K_pstatic_coef_x * x_v2 + K_pstatic_coef_y * y_v2 + _param_ekf2_pcoef_z.get() * z_v2);
 
 					// correct baro measurement using pressure error estimate and assuming sea level gravity
 					balt_data_avg += pstatic_err / (airdata.rho * CONSTANTS_ONE_G);

src/modules/logger/logger.cpp

@@ -1798,7 +1798,8 @@ void Logger::write_format(LogType type, const orb_metadata &meta, WrittenFormats
 				strcmp(type_name, "uint64_t") != 0 &&
 				strcmp(type_name, "float") != 0 &&
 				strcmp(type_name, "double") != 0 &&
-				strcmp(type_name, "bool") != 0) {
+				strcmp(type_name, "bool") != 0 &&
+				strcmp(type_name, "char") != 0) {
 
 			// find orb meta for type
 			const orb_metadata *const*topics = orb_get_topics();

src/modules/mavlink/mavlink_main.cpp

@@ -1704,7 +1704,6 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream)
 		configure_stream_local("POSITION_TARGET_GLOBAL_INT", 10.0f);
 		configure_stream_local("POSITION_TARGET_LOCAL_NED", 10.0f);
 		configure_stream_local("RC_CHANNELS", 20.0f);
-		configure_stream_local("SCALED_IMU", 50.0f);
 		configure_stream_local("SERVO_OUTPUT_RAW_0", 10.0f);
 		configure_stream_local("SYS_STATUS", 5.0f);
 		configure_stream_local("SYSTEM_TIME", 1.0f);
@@ -1792,6 +1791,9 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream)
 		configure_stream_local("PING", 1.0f);
 		configure_stream_local("POSITION_TARGET_GLOBAL_INT", 10.0f);
 		configure_stream_local("RC_CHANNELS", 10.0f);
+		configure_stream_local("SCALED_IMU", 25.0f);
+		configure_stream_local("SCALED_IMU2", 25.0f);
+		configure_stream_local("SCALED_IMU3", 25.0f);
 		configure_stream_local("SERVO_OUTPUT_RAW_0", 20.0f);
 		configure_stream_local("SERVO_OUTPUT_RAW_1", 20.0f);
 		configure_stream_local("SYS_STATUS", 1.0f);

src/modules/mc_pos_control/PositionControl.cpp

@@ -331,9 +331,11 @@ void PositionControl::updateConstraints(const vehicle_constraints_s &constraints
 	// For safety check if adjustable constraints are below global constraints. If they are not stricter than global
 	// constraints, then just use global constraints for the limits.
 
+	const float tilt_max_radians = math::radians(math::max(_param_mpc_tiltmax_air.get(), _param_mpc_man_tilt_max.get()));
+
 	if (!PX4_ISFINITE(constraints.tilt)
-	    || !(constraints.tilt < math::max(_param_mpc_tiltmax_air.get(), _param_mpc_man_tilt_max.get()))) {
-		_constraints.tilt = math::max(_param_mpc_tiltmax_air.get(), _param_mpc_man_tilt_max.get());
+	    || !(constraints.tilt < tilt_max_radians)) {
+		_constraints.tilt = tilt_max_radians;
 	}
 
 	if (!PX4_ISFINITE(constraints.speed_up) || !(constraints.speed_up < _param_mpc_z_vel_max_up.get())) {
@@ -352,8 +354,4 @@ void PositionControl::updateConstraints(const vehicle_constraints_s &constraints
 void PositionControl::updateParams()
 {
 	ModuleParams::updateParams();
-
-	// Tilt needs to be in radians
-	_param_mpc_tiltmax_air.set(math::radians(_param_mpc_tiltmax_air.get()));
-	_param_mpc_man_tilt_max.set(math::radians(_param_mpc_man_tilt_max.get()));
 }

src/modules/mc_pos_control/PositionControl.hpp

@@ -232,9 +232,9 @@ private:
 		(ParamFloat<px4::params::MPC_Z_VEL_MAX_DN>) _param_mpc_z_vel_max_dn,
 		(ParamFloat<px4::params::MPC_Z_VEL_MAX_UP>) _param_mpc_z_vel_max_up,
 		(ParamFloat<px4::params::MPC_TILTMAX_AIR>)
-		_param_mpc_tiltmax_air, // maximum tilt for any position controlled mode in radians
+		_param_mpc_tiltmax_air, // maximum tilt for any position controlled mode in degrees
 		(ParamFloat<px4::params::MPC_MAN_TILT_MAX>)
-		_param_mpc_man_tilt_max, // maximum til for stabilized/altitude mode in radians
+		_param_mpc_man_tilt_max, // maximum til for stabilized/altitude mode in degrees
 		(ParamFloat<px4::params::MPC_Z_P>) _param_mpc_z_p,
 		(ParamFloat<px4::params::MPC_Z_VEL_P>) _param_mpc_z_vel_p,
 		(ParamFloat<px4::params::MPC_Z_VEL_I>) _param_mpc_z_vel_i,

src/modules/uORB/uORBDeviceMaster.cpp

@@ -55,7 +55,7 @@ uORB::DeviceMaster::~DeviceMaster()
 }
 
 int
-uORB::DeviceMaster::advertise(const struct orb_metadata *meta, int *instance, int priority)
+uORB::DeviceMaster::advertise(const struct orb_metadata *meta, bool is_advertiser, int *instance, int priority)
 {
 	int ret = PX4_ERROR;
 
@@ -119,17 +119,36 @@ uORB::DeviceMaster::advertise(const struct orb_metadata *meta, int *instance, in
 			delete node;
 
 			if (ret == -EEXIST) {
-				/* if the node exists already, get the existing one and check if
-				 * something has been published yet. */
+				/* if the node exists already, get the existing one and check if it's advertised. */
 				uORB::DeviceNode *existing_node = getDeviceNodeLocked(meta, group_tries);
 
-				if ((existing_node != nullptr) && !(existing_node->is_published())) {
-					/* nothing has been published yet, lets claim it */
-					existing_node->set_priority(priority);
+				/*
+				 * We can claim an existing node in these cases:
+				 * - The node is not advertised (yet). It means there is already one or more subscribers or it was
+				 *   unadvertised.
+				 * - We are a single-instance advertiser requesting the first instance.
+				 *   (Usually we don't end up here, but we might in case of a race condition between 2
+				 *   advertisers).
+				 * - We are a subscriber requesting a certain instance.
+				 *   (Also we usually don't end up in that case, but we might in case of a race condtion
+				 *   between an advertiser and subscriber).
+				 */
+				bool is_single_instance_advertiser = is_advertiser && !instance;
+
+				if (existing_node != nullptr &&
+				    (!existing_node->is_advertised() || is_single_instance_advertiser || !is_advertiser)) {
+					if (is_advertiser) {
+						existing_node->set_priority(priority);
+						/* Set as advertised to avoid race conditions (otherwise 2 multi-instance advertisers
+						 * could get the same instance).
+						 */
+						existing_node->mark_as_advertised();
+					}
+
 					ret = PX4_OK;
 
 				} else {
-					/* otherwise: data has already been published, keep looking */
+					/* otherwise: already advertised, keep looking */
 				}
 			}
 
@@ -137,7 +156,11 @@ uORB::DeviceMaster::advertise(const struct orb_metadata *meta, int *instance, in
 			free((void *)devpath);
 
 		} else {
-			// add to the node map;.
+			if (is_advertiser) {
+				node->mark_as_advertised();
+			}
+
+			// add to the node map.
 			_node_list.add(node);
 		}
 

src/modules/uORB/uORBDeviceMaster.hpp

@@ -60,7 +60,7 @@ class uORB::DeviceMaster
 {
 public:
 
-	int advertise(const struct orb_metadata *meta, int *instance, int priority);
+	int advertise(const struct orb_metadata *meta, bool is_advertiser, int *instance, int priority);
 
 	/**
 	 * Public interface for getDeviceNodeLocked(). Takes care of synchronization.

src/modules/uORB/uORBDeviceNode.cpp

@@ -73,35 +73,15 @@ uORB::DeviceNode::~DeviceNode()
 int
 uORB::DeviceNode::open(cdev::file_t *filp)
 {
-	int ret;
-
 	/* is this a publisher? */
 	if (filp->f_oflags == PX4_F_WRONLY) {
 
-		/* become the publisher if we can */
 		lock();
-
-		if (_publisher == 0) {
-			_publisher = px4_getpid();
-			ret = PX4_OK;
-
-		} else {
-			ret = -EBUSY;
-		}
-
+		mark_as_advertised();
 		unlock();
 
 		/* now complete the open */
-		if (ret == PX4_OK) {
-			ret = CDev::open(filp);
-
-			/* open failed - not the publisher anymore */
-			if (ret != PX4_OK) {
-				_publisher = 0;
-			}
-		}
-
-		return ret;
+		return CDev::open(filp);
 	}
 
 	/* is this a new subscriber? */
@@ -119,7 +99,7 @@ uORB::DeviceNode::open(cdev::file_t *filp)
 
 		filp->f_priv = (void *)sd;
 
-		ret = CDev::open(filp);
+		int ret = CDev::open(filp);
 
 		add_internal_subscriber();
 
@@ -142,11 +122,7 @@ uORB::DeviceNode::open(cdev::file_t *filp)
 int
 uORB::DeviceNode::close(cdev::file_t *filp)
 {
-	/* is this the publisher closing? */
-	if (px4_getpid() == _publisher) {
-		_publisher = 0;
-
-	} else {
+	if (filp->f_oflags == PX4_F_RDONLY) { /* subscriber */
 		SubscriberData *sd = filp_to_sd(filp);
 
 		if (sd != nullptr) {
@@ -275,7 +251,6 @@ uORB::DeviceNode::write(cdev::file_t *filp, const char *buffer, size_t buflen)
 	/* wrap-around happens after ~49 days, assuming a publisher rate of 1 kHz */
 	_generation++;
 
-	_published = true;
 
 	ATOMIC_LEAVE;
 
@@ -353,10 +328,12 @@ uORB::DeviceNode::ioctl(cdev::file_t *filp, int cmd, unsigned long arg)
 		*(int *)arg = get_priority();
 		return PX4_OK;
 
-	case ORBIOCSETQUEUESIZE:
-		//no need for locking here, since this is used only during the advertisement call,
-		//and only one advertiser is allowed to open the DeviceNode at the same time.
-		return update_queue_size(arg);
+	case ORBIOCSETQUEUESIZE: {
+			lock();
+			int ret = update_queue_size(arg);
+			unlock();
+			return ret;
+		}
 
 	case ORBIOCGETINTERVAL:
 		if (sd->update_interval) {
@@ -368,8 +345,8 @@ uORB::DeviceNode::ioctl(cdev::file_t *filp, int cmd, unsigned long arg)
 
 		return OK;
 
-	case ORBIOCISPUBLISHED:
-		*(unsigned long *)arg = _published;
+	case ORBIOCISADVERTISED:
+		*(unsigned long *)arg = _advertised;
 
 		return OK;
 
@@ -447,7 +424,7 @@ int uORB::DeviceNode::unadvertise(orb_advert_t handle)
 	 * of subscribers and publishers. But we also do not have a leak since future
 	 * publishers reuse the same DeviceNode object.
 	 */
-	devnode->_published = false;
+	devnode->_advertised = false;
 
 	return PX4_OK;
 }

src/modules/uORB/uORBDeviceNode.hpp

@@ -156,11 +156,13 @@ public:
 	void remove_internal_subscriber();
 
 	/**
-	 * Return true if this topic has been published.
+	 * Return true if this topic has been advertised.
 	 *
 	 * This is used in the case of multi_pub/sub to check if it's valid to advertise
 	 * and publish to this node or if another node should be tried. */
-	bool is_published() const { return _published; }
+	bool is_advertised() const { return _advertised; }
+
+	void mark_as_advertised() { _advertised = true; }
 
 	/**
 	 * Try to change the size of the queue. This can only be done as long as nobody published yet.
@@ -228,13 +230,10 @@ private:
 	hrt_abstime   _last_update{0}; /**< time the object was last updated */
 	volatile unsigned   _generation{0};  /**< object generation count */
 	uint8_t   _priority;  /**< priority of the topic */
-	bool _published{false};  /**< has ever data been published */
+	bool _advertised{false};  /**< has ever been advertised (not necessarily published data yet) */
 	uint8_t _queue_size; /**< maximum number of elements in the queue */
 	int8_t _subscriber_count{0};
 
-	px4_task_t _publisher{0}; /**< if nonzero, current publisher. Only used inside the advertise call.
-						We allow one publisher to have an open file descriptor at the same time. */
-
 	// statistics
 	uint32_t _lost_messages = 0; /**< nr of lost messages for all subscribers. If two subscribers lose the same
 					message, it is counted as two. */

src/modules/uORB/uORBManager.cpp

@@ -105,7 +105,7 @@ int uORB::Manager::orb_exists(const struct orb_metadata *meta, int instance)
 		uORB::DeviceNode *node = _device_master->getDeviceNode(meta, instance);
 
 		if (node != nullptr) {
-			if (node->is_published()) {
+			if (node->is_advertised()) {
 				return PX4_OK;
 			}
 		}
@@ -139,10 +139,10 @@ int uORB::Manager::orb_exists(const struct orb_metadata *meta, int instance)
 		int fd = px4_open(path, 0);
 
 		if (fd >= 0) {
-			unsigned long is_published;
+			unsigned long is_advertised;
 
-			if (px4_ioctl(fd, ORBIOCISPUBLISHED, (unsigned long)&is_published) == 0) {
-				if (!is_published) {
+			if (px4_ioctl(fd, ORBIOCISADVERTISED, (unsigned long)&is_advertised) == 0) {
+				if (!is_advertised) {
 					ret = PX4_ERROR;
 				}
 			}
@@ -316,14 +316,12 @@ int uORB::Manager::orb_get_interval(int handle, unsigned *interval)
 	return ret;
 }
 
-int uORB::Manager::node_advertise(const struct orb_metadata *meta, int *instance, int priority)
+int uORB::Manager::node_advertise(const struct orb_metadata *meta, bool is_advertiser, int *instance, int priority)
 {
 	int ret = PX4_ERROR;
 
-	/* fill advertiser data */
-
 	if (get_device_master()) {
-		ret = _device_master->advertise(meta, instance, priority);
+		ret = _device_master->advertise(meta, is_advertiser, instance, priority);
 	}
 
 	/* it's PX4_OK if it already exists */
@@ -373,7 +371,7 @@ int uORB::Manager::node_open(const struct orb_metadata *meta, bool advertiser, i
 	if (fd < 0) {
 
 		/* try to create the node */
-		ret = node_advertise(meta, instance, priority);
+		ret = node_advertise(meta, advertiser, instance, priority);
 
 		if (ret == PX4_OK) {
 			/* update the path, as it might have been updated during the node_advertise call */
@@ -385,7 +383,7 @@ int uORB::Manager::node_open(const struct orb_metadata *meta, bool advertiser, i
 			}
 		}
 
-		/* on success, try the open again */
+		/* on success, try to open again */
 		if (ret == PX4_OK) {
 			fd = px4_open(path, (advertiser) ? PX4_F_WRONLY : PX4_F_RDONLY);
 		}

src/modules/uORB/uORBManager.hpp

@@ -386,11 +386,9 @@ private: // class methods
 	/**
 	 * Advertise a node; don't consider it an error if the node has
 	 * already been advertised.
-	 *
-	 * @todo verify that the existing node is the same as the one
-	 *       we tried to advertise.
 	 */
-	int node_advertise(const struct orb_metadata *meta, int *instance = nullptr, int priority = ORB_PRIO_DEFAULT);
+	int node_advertise(const struct orb_metadata *meta, bool is_advertiser, int *instance = nullptr,
+			   int priority = ORB_PRIO_DEFAULT);
 
 	/**
 	 * Common implementation for orb_advertise and orb_subscribe.