mavlink: replace MavlinkOrbSubscription with uORB::Subscription

* uORB orb_stat() and update(uint64_t *time, void *dst) are now obsolete and have been deleted
 * mavlink messages add more advertised checks in streams get_size() check to improve data rate calculation across different scenarios

src/modules/mavlink/CMakeLists.txt

@@ -50,7 +50,6 @@ px4_add_module(
 		mavlink_main.cpp
 		mavlink_messages.cpp
 		mavlink_mission.cpp
-		mavlink_orb_subscription.cpp
 		mavlink_parameters.cpp
 		mavlink_rate_limiter.cpp
 		mavlink_receiver.cpp

src/modules/mavlink/mavlink_high_latency2.cpp

@@ -43,56 +43,11 @@
 #include <lib/ecl/geo/geo.h>
 #include <lib/mathlib/mathlib.h>
 #include <lib/matrix/matrix/math.hpp>
-#include <uORB/topics/actuator_controls.h>
-#include <uORB/topics/airspeed.h>
-#include <uORB/topics/battery_status.h>
-#include <uORB/topics/estimator_status.h>
-#include <uORB/topics/geofence_result.h>
-#include <uORB/topics/position_controller_status.h>
-#include <uORB/topics/tecs_status.h>
-#include <uORB/topics/vehicle_attitude_setpoint.h>
-#include <uORB/topics/vehicle_local_position.h>
-#include <uORB/topics/vehicle_global_position.h>
-#include <uORB/topics/vehicle_gps_position.h>
-#include <uORB/topics/vehicle_status.h>
-#include <uORB/topics/vehicle_status_flags.h>
-#include <uORB/topics/vtol_vehicle_status.h>
-#include <uORB/topics/wind_estimate.h>
-#include <uORB/uORB.h>
 
 using matrix::wrap_2pi;
 
-MavlinkStreamHighLatency2::MavlinkStreamHighLatency2(Mavlink *mavlink) : MavlinkStream(mavlink),
-	_actuator_sub_0(_mavlink->add_orb_subscription(ORB_ID(actuator_controls_0))),
-	_actuator_time_0(0),
-	_actuator_sub_1(_mavlink->add_orb_subscription(ORB_ID(actuator_controls_1))),
-	_actuator_time_1(0),
-	_airspeed_sub(_mavlink->add_orb_subscription(ORB_ID(airspeed))),
-	_airspeed_time(0),
-	_attitude_sp_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_attitude_setpoint))),
-	_attitude_sp_time(0),
-	_estimator_status_sub(_mavlink->add_orb_subscription(ORB_ID(estimator_status))),
-	_estimator_status_time(0),
-	_pos_ctrl_status_sub(_mavlink->add_orb_subscription(ORB_ID(position_controller_status))),
-	_pos_ctrl_status_time(0),
-	_geofence_sub(_mavlink->add_orb_subscription(ORB_ID(geofence_result))),
-	_geofence_time(0),
-	_local_pos_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_local_position))),
-	_local_pos_time(0),
-	_global_pos_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_global_position))),
-	_global_pos_time(0),
-	_gps_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_gps_position))),
-	_gps_time(0),
-	_mission_result_sub(_mavlink->add_orb_subscription(ORB_ID(mission_result))),
-	_mission_result_time(0),
-	_status_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_status))),
-	_status_time(0),
-	_status_flags_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_status_flags))),
-	_status_flags_time(0),
-	_tecs_status_sub(_mavlink->add_orb_subscription(ORB_ID(tecs_status))),
-	_tecs_time(0),
-	_wind_sub(_mavlink->add_orb_subscription(ORB_ID(wind_estimate))),
-	_wind_time(0),
+MavlinkStreamHighLatency2::MavlinkStreamHighLatency2(Mavlink *mavlink) :
+	MavlinkStream(mavlink),
 	_airspeed(SimpleAnalyzer::AVERAGE),
 	_airspeed_sp(SimpleAnalyzer::AVERAGE),
 	_climb_rate(SimpleAnalyzer::MAX),
@@ -103,11 +58,6 @@ MavlinkStreamHighLatency2::MavlinkStreamHighLatency2(Mavlink *mavlink) : Mavlink
 	_throttle(SimpleAnalyzer::AVERAGE),
 	_windspeed(SimpleAnalyzer::AVERAGE)
 {
-
-	for (int i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) {
-		_batteries[i].subscription = _mavlink->add_orb_subscription(ORB_ID(battery_status), i);
-	}
-
 	reset_last_sent();
 }
 
@@ -116,7 +66,7 @@ bool MavlinkStreamHighLatency2::send(const hrt_abstime t)
 	// only send the struct if transmitting is allowed
 	// this assures that the stream timer is only reset when actually a message is transmitted
 	if (_mavlink->should_transmit()) {
-		mavlink_high_latency2_t msg = {};
+		mavlink_high_latency2_t msg{};
 		set_default_values(msg);
 
 		bool updated = _airspeed.valid();
@@ -218,7 +168,7 @@ bool MavlinkStreamHighLatency2::send(const hrt_abstime t)
 
 	} else {
 		// reset the analysers every 60 seconds if nothing should be transmitted
-		if ((t - _last_reset_time) > 60000000) {
+		if ((t - _last_reset_time) > 60_s) {
 			reset_analysers(t);
 			PX4_DEBUG("Resetting HIGH_LATENCY2 analysers");
 		}
@@ -250,30 +200,29 @@ void MavlinkStreamHighLatency2::reset_analysers(const hrt_abstime t)
 
 bool MavlinkStreamHighLatency2::write_airspeed(mavlink_high_latency2_t *msg)
 {
-	struct airspeed_s airspeed;
+	airspeed_s airspeed;
 
-	const bool updated = _airspeed_sub->update(&_airspeed_time, &airspeed);
-
-	if (_airspeed_time > 0) {
+	if (_airspeed_sub.update(&airspeed)) {
 		if (airspeed.confidence < 0.95f) { // the same threshold as for the commander
 			msg->failure_flags |= HL_FAILURE_FLAG_DIFFERENTIAL_PRESSURE;
 		}
+
+		return true;
 	}
 
-	return updated;
+	return false;
 }
 
 bool MavlinkStreamHighLatency2::write_attitude_sp(mavlink_high_latency2_t *msg)
 {
-	struct vehicle_attitude_setpoint_s attitude_sp;
+	vehicle_attitude_setpoint_s attitude_sp;
 
-	const bool updated = _attitude_sp_sub->update(&_attitude_sp_time, &attitude_sp);
-
-	if (_attitude_sp_time > 0) {
+	if (_attitude_sp_sub.update(&attitude_sp)) {
 		msg->target_heading = static_cast<uint8_t>(math::degrees(wrap_2pi(attitude_sp.yaw_body)) * 0.5f);
+		return true;
 	}
 
-	return updated;
+	return false;
 }
 
 bool MavlinkStreamHighLatency2::write_battery_status(mavlink_high_latency2_t *msg)
@@ -282,7 +231,7 @@ bool MavlinkStreamHighLatency2::write_battery_status(mavlink_high_latency2_t *ms
 	bool updated = false;
 
 	for (int i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) {
-		if (_batteries[i].subscription->update(&_batteries[i].timestamp, &battery)) {
+		if (_batteries[i].subscription.update(&battery)) {
 			updated = true;
 			_batteries[i].connected = battery.connected;
 
@@ -297,58 +246,56 @@ bool MavlinkStreamHighLatency2::write_battery_status(mavlink_high_latency2_t *ms
 
 bool MavlinkStreamHighLatency2::write_estimator_status(mavlink_high_latency2_t *msg)
 {
-	struct estimator_status_s estimator_status;
+	estimator_status_s estimator_status;
 
-	const bool updated = _estimator_status_sub->update(&_estimator_status_time, &estimator_status);
-
-	if (_estimator_status_time > 0) {
+	if (_estimator_status_sub.update(&estimator_status)) {
 		if (estimator_status.gps_check_fail_flags > 0 ||
 		    estimator_status.filter_fault_flags > 0 ||
 		    estimator_status.innovation_check_flags > 0) {
+
 			msg->failure_flags |= HL_FAILURE_FLAG_ESTIMATOR;
 		}
+
+		return true;
 	}
 
-	return updated;
+	return false;
 }
 
 bool MavlinkStreamHighLatency2::write_fw_ctrl_status(mavlink_high_latency2_t *msg)
 {
-	position_controller_status_s pos_ctrl_status = {};
+	position_controller_status_s pos_ctrl_status;
 
-	const bool updated = _pos_ctrl_status_sub->update(&_pos_ctrl_status_time, &pos_ctrl_status);
-
-	if (_pos_ctrl_status_time > 0) {
+	if (_pos_ctrl_status_sub.update(&pos_ctrl_status)) {
 		uint16_t target_distance;
 		convert_limit_safe(pos_ctrl_status.wp_dist * 0.1f, target_distance);
 		msg->target_distance = target_distance;
+		return true;
 	}
 
-	return updated;
+	return false;
 }
 
 bool MavlinkStreamHighLatency2::write_geofence_result(mavlink_high_latency2_t *msg)
 {
-	struct geofence_result_s geofence;
+	geofence_result_s geofence;
 
-	const bool updated = _geofence_sub->update(&_geofence_time, &geofence);
-
-	if (_geofence_time > 0) {
+	if (_geofence_sub.update(&geofence)) {
 		if (geofence.geofence_violated) {
 			msg->failure_flags |= HL_FAILURE_FLAG_GEOFENCE;
 		}
+
+		return true;
 	}
 
-	return updated;
+	return false;
 }
 
 bool MavlinkStreamHighLatency2::write_global_position(mavlink_high_latency2_t *msg)
 {
-	struct vehicle_global_position_s global_pos;
+	vehicle_global_position_s global_pos;
 
-	const bool updated = _global_pos_sub->update(&_global_pos_time, &global_pos);
-
-	if (_global_pos_time > 0) {
+	if (_global_pos_sub.update(&global_pos)) {
 		msg->latitude = global_pos.lat * 1e7;
 		msg->longitude = global_pos.lon * 1e7;
 
@@ -364,46 +311,45 @@ bool MavlinkStreamHighLatency2::write_global_position(mavlink_high_latency2_t *m
 		msg->altitude = altitude;
 
 		msg->heading = static_cast<uint8_t>(math::degrees(wrap_2pi(global_pos.yaw)) * 0.5f);
+
+		return true;
 	}
 
-	return updated;
+	return false;
 }
 
 bool MavlinkStreamHighLatency2::write_mission_result(mavlink_high_latency2_t *msg)
 {
-	struct mission_result_s mission_result;
+	mission_result_s mission_result;
 
-	const bool updated = _mission_result_sub->update(&_mission_result_time, &mission_result);
-
-	if (_mission_result_time > 0) {
+	if (_mission_result_sub.update(&mission_result)) {
 		msg->wp_num = mission_result.seq_current;
+		return true;
 	}
 
-	return updated;
+	return false;
 }
 
 bool MavlinkStreamHighLatency2::write_tecs_status(mavlink_high_latency2_t *msg)
 {
-	struct tecs_status_s tecs_status;
+	tecs_status_s tecs_status;
 
-	const bool updated = _tecs_status_sub->update(&_tecs_time, &tecs_status);
-
-	if (_tecs_time > 0) {
+	if (_tecs_status_sub.update(&tecs_status)) {
 		int16_t target_altitude;
 		convert_limit_safe(tecs_status.altitude_sp, target_altitude);
 		msg->target_altitude = target_altitude;
+
+		return true;
 	}
 
-	return updated;
+	return false;
 }
 
 bool MavlinkStreamHighLatency2::write_vehicle_status(mavlink_high_latency2_t *msg)
 {
-	struct vehicle_status_s status;
+	vehicle_status_s status;
 
-	const bool updated = _status_sub->update(&_status_time, &status);
-
-	if (_status_time > 0) {
+	if (_status_sub.update(&status)) {
 		if ((status.onboard_control_sensors_enabled & MAV_SYS_STATUS_SENSOR_ABSOLUTE_PRESSURE)
 		    && !(status.onboard_control_sensors_health & MAV_SYS_STATUS_SENSOR_ABSOLUTE_PRESSURE)) {
 			msg->failure_flags |= HL_FAILURE_FLAG_ABSOLUTE_PRESSURE;
@@ -452,18 +398,18 @@ bool MavlinkStreamHighLatency2::write_vehicle_status(mavlink_high_latency2_t *ms
 		uint8_t mavlink_base_mode;
 		get_mavlink_navigation_mode(&status, &mavlink_base_mode, &custom_mode);
 		msg->custom_mode = custom_mode.custom_mode_hl;
+
+		return true;
 	}
 
-	return updated;
+	return false;
 }
 
 bool MavlinkStreamHighLatency2::write_vehicle_status_flags(mavlink_high_latency2_t *msg)
 {
-	struct vehicle_status_flags_s status_flags;
+	vehicle_status_flags_s status_flags;
 
-	const bool updated = _status_flags_sub->update(&_status_flags_time, &status_flags);
-
-	if (_status_flags_time > 0) {
+	if (_status_flags_sub.update(&status_flags)) {
 		if (!status_flags.condition_global_position_valid) { //TODO check if there is a better way to get only GPS failure
 			msg->failure_flags |= HL_FAILURE_FLAG_GPS;
 		}
@@ -471,23 +417,24 @@ bool MavlinkStreamHighLatency2::write_vehicle_status_flags(mavlink_high_latency2
 		if (status_flags.offboard_control_signal_lost && status_flags.offboard_control_signal_found_once) {
 			msg->failure_flags |= HL_FAILURE_FLAG_OFFBOARD_LINK;
 		}
+
+		return true;
 	}
 
-	return updated;
+	return false;
 }
 
 bool MavlinkStreamHighLatency2::write_wind_estimate(mavlink_high_latency2_t *msg)
 {
-	struct wind_estimate_s wind;
+	wind_estimate_s wind;
 
-	const bool updated = _wind_sub->update(&_wind_time, &wind);
-
-	if (_wind_time > 0) {
-		msg->wind_heading = static_cast<uint8_t>(
-					    math::degrees(wrap_2pi(atan2f(wind.windspeed_east, wind.windspeed_north))) * 0.5f);
+	if (_wind_sub.update(&wind)) {
+		msg->wind_heading = static_cast<uint8_t>(math::degrees(wrap_2pi(atan2f(wind.windspeed_east,
+				    wind.windspeed_north))) * 0.5f);
+		return true;
 	}
 
-	return updated;
+	return false;
 }
 
 void MavlinkStreamHighLatency2::update_data()
@@ -519,7 +466,7 @@ void MavlinkStreamHighLatency2::update_airspeed()
 {
 	airspeed_s airspeed;
 
-	if (_airspeed_sub->update(&airspeed)) {
+	if (_airspeed_sub.update(&airspeed)) {
 		_airspeed.add_value(airspeed.indicated_airspeed_m_s, _update_rate_filtered);
 		_temperature.add_value(airspeed.air_temperature_celsius, _update_rate_filtered);
 	}
@@ -529,7 +476,7 @@ void MavlinkStreamHighLatency2::update_tecs_status()
 {
 	tecs_status_s tecs_status;
 
-	if (_tecs_status_sub->update(&tecs_status)) {
+	if (_tecs_status_sub.update(&tecs_status)) {
 		_airspeed_sp.add_value(tecs_status.airspeed_sp, _update_rate_filtered);
 	}
 }
@@ -539,7 +486,7 @@ void MavlinkStreamHighLatency2::update_battery_status()
 	battery_status_s battery;
 
 	for (int i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) {
-		if (_batteries[i].subscription->update(&battery)) {
+		if (_batteries[i].subscription.update(&battery)) {
 			_batteries[i].connected = battery.connected;
 			_batteries[i].analyzer.add_value(battery.remaining, _update_rate_filtered);
 		}
@@ -550,7 +497,7 @@ void MavlinkStreamHighLatency2::update_local_position()
 {
 	vehicle_local_position_s local_pos;
 
-	if (_local_pos_sub->update(&local_pos)) {
+	if (_local_pos_sub.update(&local_pos)) {
 		_climb_rate.add_value(fabsf(local_pos.vz), _update_rate_filtered);
 		_groundspeed.add_value(sqrtf(local_pos.vx * local_pos.vx + local_pos.vy * local_pos.vy), _update_rate_filtered);
 	}
@@ -560,7 +507,7 @@ void MavlinkStreamHighLatency2::update_gps()
 {
 	vehicle_gps_position_s gps;
 
-	if (_gps_sub->update(&gps)) {
+	if (_gps_sub.update(&gps)) {
 		_eph.add_value(gps.eph, _update_rate_filtered);
 		_epv.add_value(gps.epv, _update_rate_filtered);
 	}
@@ -570,17 +517,17 @@ void MavlinkStreamHighLatency2::update_vehicle_status()
 {
 	vehicle_status_s status;
 
-	if (_status_sub->update(&status)) {
+	if (_status_sub.update(&status)) {
 		if (status.arming_state == vehicle_status_s::ARMING_STATE_ARMED) {
-			struct actuator_controls_s actuator = {};
+			actuator_controls_s actuator{};
 
 			if (status.is_vtol && status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) {
-				if (_actuator_sub_1->update(&actuator)) {
+				if (_actuator_1_sub.copy(&actuator)) {
 					_throttle.add_value(actuator.control[actuator_controls_s::INDEX_THROTTLE], _update_rate_filtered);
 				}
 
 			} else {
-				if (_actuator_sub_0->update(&actuator)) {
+				if (_actuator_0_sub.copy(&actuator)) {
 					_throttle.add_value(actuator.control[actuator_controls_s::INDEX_THROTTLE], _update_rate_filtered);
 				}
 			}
@@ -595,7 +542,7 @@ void MavlinkStreamHighLatency2::update_wind_estimate()
 {
 	wind_estimate_s wind;
 
-	if (_wind_sub->update(&wind)) {
+	if (_wind_sub.update(&wind)) {
 		_windspeed.add_value(sqrtf(wind.windspeed_north * wind.windspeed_north + wind.windspeed_east * wind.windspeed_east),
 				     _update_rate_filtered);
 	}

src/modules/mavlink/mavlink_high_latency2.h

@@ -44,6 +44,22 @@
 #include "mavlink_simple_analyzer.h"
 #include "mavlink_stream.h"
 
+#include <uORB/Subscription.hpp>
+#include <uORB/topics/actuator_controls.h>
+#include <uORB/topics/airspeed.h>
+#include <uORB/topics/battery_status.h>
+#include <uORB/topics/estimator_status.h>
+#include <uORB/topics/geofence_result.h>
+#include <uORB/topics/position_controller_status.h>
+#include <uORB/topics/tecs_status.h>
+#include <uORB/topics/wind_estimate.h>
+#include <uORB/topics/vehicle_attitude_setpoint.h>
+#include <uORB/topics/vehicle_global_position.h>
+#include <uORB/topics/vehicle_gps_position.h>
+#include <uORB/topics/vehicle_local_position.h>
+#include <uORB/topics/vehicle_status.h>
+#include <uORB/topics/vehicle_status_flags.h>
+
 class MavlinkStreamHighLatency2 : public MavlinkStream
 {
 public:
@@ -85,57 +101,28 @@ public:
 private:
 
 	struct PerBatteryData {
-		PerBatteryData() {}
-		MavlinkOrbSubscription *subscription{nullptr};
+		PerBatteryData(uint8_t instance) : subscription(ORB_ID(battery_status), instance) {}
+		uORB::Subscription subscription;
 		SimpleAnalyzer analyzer{SimpleAnalyzer::AVERAGE};
 		uint64_t timestamp{0};
 		bool connected{false};
 	};
 
-	MavlinkOrbSubscription *_actuator_sub_0;
-	uint64_t _actuator_time_0;
-
-	MavlinkOrbSubscription *_actuator_sub_1;
-	uint64_t _actuator_time_1;
-
-	MavlinkOrbSubscription *_airspeed_sub;
-	uint64_t _airspeed_time;
-
-	MavlinkOrbSubscription *_attitude_sp_sub;
-	uint64_t _attitude_sp_time;
-
-	MavlinkOrbSubscription *_estimator_status_sub;
-	uint64_t _estimator_status_time;
-
-	MavlinkOrbSubscription *_pos_ctrl_status_sub;
-	uint64_t _pos_ctrl_status_time;
-
-	MavlinkOrbSubscription *_geofence_sub;
-	uint64_t _geofence_time;
-
-	MavlinkOrbSubscription *_local_pos_sub;
-	uint64_t _local_pos_time;
-
-	MavlinkOrbSubscription *_global_pos_sub;
-	uint64_t _global_pos_time;
-
-	MavlinkOrbSubscription *_gps_sub;
-	uint64_t _gps_time;
-
-	MavlinkOrbSubscription *_mission_result_sub;
-	uint64_t _mission_result_time;
-
-	MavlinkOrbSubscription *_status_sub;
-	uint64_t _status_time;
-
-	MavlinkOrbSubscription *_status_flags_sub;
-	uint64_t _status_flags_time;
-
-	MavlinkOrbSubscription *_tecs_status_sub;
-	uint64_t _tecs_time;
-
-	MavlinkOrbSubscription *_wind_sub;
-	uint64_t _wind_time;
+	uORB::Subscription _actuator_0_sub{ORB_ID(actuator_controls_0)};
+	uORB::Subscription _actuator_1_sub{ORB_ID(actuator_controls_1)};
+	uORB::Subscription _airspeed_sub{ORB_ID(airspeed)};
+	uORB::Subscription _attitude_sp_sub{ORB_ID(vehicle_attitude_setpoint)};
+	uORB::Subscription _estimator_status_sub{ORB_ID(estimator_status)};
+	uORB::Subscription _pos_ctrl_status_sub{ORB_ID(position_controller_status)};
+	uORB::Subscription _geofence_sub{ORB_ID(geofence_result)};
+	uORB::Subscription _global_pos_sub{ORB_ID(vehicle_global_position)};
+	uORB::Subscription _local_pos_sub{ORB_ID(vehicle_local_position)};
+	uORB::Subscription _gps_sub{ORB_ID(vehicle_gps_position)};
+	uORB::Subscription _mission_result_sub{ORB_ID(mission_result)};
+	uORB::Subscription _status_sub{ORB_ID(vehicle_status)};
+	uORB::Subscription _status_flags_sub{ORB_ID(vehicle_status_flags)};
+	uORB::Subscription _tecs_status_sub{ORB_ID(tecs_status)};
+	uORB::Subscription _wind_sub{ORB_ID(wind_estimate)};
 
 	SimpleAnalyzer _airspeed;
 	SimpleAnalyzer _airspeed_sp;
@@ -151,7 +138,7 @@ private:
 	hrt_abstime _last_update_time = 0;
 	float _update_rate_filtered = 0.0f;
 
-	PerBatteryData _batteries[ORB_MULTI_MAX_INSTANCES];
+	PerBatteryData _batteries[ORB_MULTI_MAX_INSTANCES] {0, 1, 2, 3};
 
 	/* do not allow top copying this class */
 	MavlinkStreamHighLatency2(MavlinkStreamHighLatency2 &);

src/modules/mavlink/mavlink_main.cpp

@@ -1155,12 +1155,11 @@ Mavlink::send_statustext_emergency(const char *string)
 void
 Mavlink::send_autopilot_capabilites()
 {
-	struct vehicle_status_s status;
+	uORB::Subscription status_sub{ORB_ID(vehicle_status)};
+	vehicle_status_s status;
 
-	MavlinkOrbSubscription *status_sub = this->add_orb_subscription(ORB_ID(vehicle_status));
-
-	if (status_sub->update(&status)) {
-		mavlink_autopilot_version_t msg = {};
+	if (status_sub.copy(&status)) {
+		mavlink_autopilot_version_t msg{};
 
 		msg.capabilities = MAV_PROTOCOL_CAPABILITY_MISSION_FLOAT;
 		msg.capabilities |= MAV_PROTOCOL_CAPABILITY_MISSION_INT;
@@ -1242,27 +1241,6 @@ Mavlink::send_protocol_version()
 	set_proto_version(curr_proto_ver);
 }
 
-MavlinkOrbSubscription *
-Mavlink::add_orb_subscription(const orb_id_t topic, int instance, bool disable_sharing)
-{
-	if (!disable_sharing) {
-		/* check if already subscribed to this topic */
-		for (MavlinkOrbSubscription *sub : _subscriptions) {
-			if (sub->get_topic() == topic && sub->get_instance() == instance) {
-				/* already subscribed */
-				return sub;
-			}
-		}
-	}
-
-	/* add new subscription */
-	MavlinkOrbSubscription *sub_new = new MavlinkOrbSubscription(topic, instance);
-
-	_subscriptions.add(sub_new);
-
-	return sub_new;
-}
-
 int
 Mavlink::configure_stream(const char *stream_name, const float rate)
 {
@@ -2138,22 +2116,17 @@ Mavlink::task_main(int argc, char *argv[])
 
 	uORB::Subscription parameter_update_sub{ORB_ID(parameter_update)};
 
-	MavlinkOrbSubscription *cmd_sub = add_orb_subscription(ORB_ID(vehicle_command), 0, true);
-	MavlinkOrbSubscription *status_sub = add_orb_subscription(ORB_ID(vehicle_status));
-	uint64_t status_time = 0;
-	MavlinkOrbSubscription *ack_sub = add_orb_subscription(ORB_ID(vehicle_command_ack), 0, true);
-	/* We don't want to miss the first advertise of an ACK, so we subscribe from the
-	 * beginning and not just when the topic exists. */
-	ack_sub->subscribe_from_beginning(true);
-	cmd_sub->subscribe_from_beginning(true);
+	uORB::Subscription cmd_sub{ORB_ID(vehicle_command)};
+	uORB::Subscription status_sub{ORB_ID(vehicle_status)};
+	uORB::Subscription ack_sub{ORB_ID(vehicle_command_ack)};
 
 	/* command ack */
 	uORB::PublicationQueued<vehicle_command_ack_s> command_ack_pub{ORB_ID(vehicle_command_ack)};
 
-	MavlinkOrbSubscription *mavlink_log_sub = add_orb_subscription(ORB_ID(mavlink_log));
+	uORB::Subscription mavlink_log_sub{ORB_ID(mavlink_log)};
 
 	vehicle_status_s status{};
-	status_sub->update(&status_time, &status);
+	status_sub.copy(&status);
 
 	/* Activate sending the data by default (for the IRIDIUM mode it will be disabled after the first round of packages is sent)*/
 	_transmitting_enabled = true;
@@ -2268,7 +2241,7 @@ Mavlink::task_main(int argc, char *argv[])
 
 		configure_sik_radio();
 
-		if (status_sub->update(&status_time, &status)) {
+		if (status_sub.update(&status)) {
 			/* switch HIL mode if required */
 			set_hil_enabled(status.hil_state == vehicle_status_s::HIL_STATE_ON);
 
@@ -2291,9 +2264,9 @@ Mavlink::task_main(int argc, char *argv[])
 			}
 		}
 
-		vehicle_command_s vehicle_cmd{};
+		vehicle_command_s vehicle_cmd;
 
-		if (cmd_sub->update_if_changed(&vehicle_cmd)) {
+		if (cmd_sub.update(&vehicle_cmd)) {
 			if ((vehicle_cmd.command == vehicle_command_s::VEHICLE_CMD_CONTROL_HIGH_LATENCY) &&
 			    (_mode == MAVLINK_MODE_IRIDIUM)) {
 				if (vehicle_cmd.param1 > 0.5f) {
@@ -2330,9 +2303,9 @@ Mavlink::task_main(int argc, char *argv[])
 
 		/* send command ACK */
 		uint16_t current_command_ack = 0;
-		vehicle_command_ack_s command_ack{};
+		vehicle_command_ack_s command_ack;
 
-		if (ack_sub->update_if_changed(&command_ack)) {
+		if (ack_sub.update(&command_ack)) {
 			if (!command_ack.from_external) {
 				mavlink_command_ack_t msg;
 				msg.result = command_ack.result;
@@ -2351,9 +2324,9 @@ Mavlink::task_main(int argc, char *argv[])
 			}
 		}
 
-		mavlink_log_s mavlink_log{};
+		mavlink_log_s mavlink_log;
 
-		if (mavlink_log_sub->update_if_changed(&mavlink_log)) {
+		if (mavlink_log_sub.update(&mavlink_log)) {
 			_logbuffer.put(&mavlink_log);
 		}
 
@@ -2497,9 +2470,6 @@ Mavlink::task_main(int argc, char *argv[])
 	/* delete streams */
 	_streams.clear();
 
-	/* delete subscriptions */
-	_subscriptions.clear();
-
 	if (_uart_fd >= 0 && !_is_usb_uart) {
 		/* close UART */
 		::close(_uart_fd);

src/modules/mavlink/mavlink_main.h

@@ -79,7 +79,6 @@
 
 #include "mavlink_command_sender.h"
 #include "mavlink_messages.h"
-#include "mavlink_orb_subscription.h"
 #include "mavlink_shell.h"
 #include "mavlink_ulog.h"
 
@@ -342,15 +341,6 @@ public:
 
 	void			handle_message(const mavlink_message_t *msg);
 
-	/**
-	 * Add a mavlink orb topic subscription while ensuring that only a single object exists
-	 * for a given topic id and instance.
-	 * @param topic orb topic id
-	 * @param instance topic instance
-	 * @param disable_sharing if true, force creating a new instance
-	 */
-	MavlinkOrbSubscription *add_orb_subscription(const orb_id_t topic, int instance = 0, bool disable_sharing = false);
-
 	int			get_instance_id() const { return _instance_id; }
 
 	/**
@@ -565,7 +555,6 @@ private:
 
 	unsigned		_main_loop_delay{1000};	/**< mainloop delay, depends on data rate */
 
-	List<MavlinkOrbSubscription *>	_subscriptions;
 	List<MavlinkStream *>		_streams;
 
 	MavlinkShell		*_mavlink_shell{nullptr};

src/modules/mavlink/mavlink_orb_subscription.cpp

@@ -1,57 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2014-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 mavlink_orb_subscription.cpp
- * uORB subscription implementation.
- *
- */
-
-#include "mavlink_orb_subscription.h"
-
-bool
-MavlinkOrbSubscription::is_published()
-{
-	const bool published = _sub.advertised();
-
-	if (published) {
-		return true;
-
-	} else if (!published && _subscribe_from_beginning) {
-		// For some topics like vehicle_command_ack, we want to subscribe
-		// from the beginning in order not to miss or delay the first publish respective advertise.
-		return _sub.subscribe();
-	}
-
-	return false;
-}

src/modules/mavlink/mavlink_orb_subscription.h

@@ -1,103 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2014 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 mavlink_orb_subscription.h
- * uORB subscription definition.
- *
- * @author Anton Babushkin <anton.babushkin@me.com>
- */
-
-#ifndef MAVLINK_ORB_SUBSCRIPTION_H_
-#define MAVLINK_ORB_SUBSCRIPTION_H_
-
-#include <drivers/drv_hrt.h>
-#include <containers/List.hpp>
-#include <uORB/Subscription.hpp>
-
-class MavlinkOrbSubscription : public ListNode<MavlinkOrbSubscription *>
-{
-public:
-
-	MavlinkOrbSubscription(const orb_id_t topic, int instance) : _sub(topic, instance) {}
-	~MavlinkOrbSubscription() = default;
-
-	/**
-	 * Check if subscription updated based on timestamp.
-	 *
-	 * @return true only if topic was updated based on a timestamp and
-	 * copied to buffer successfully.
-	 * If topic was not updated since last check it will return false but
-	 * still copy the data.
-	 * If no data available data buffer will be filled with zeros.
-	 */
-	bool update(uint64_t *time, void *data) { return _sub.update(time, data); }
-
-	/**
-	 * Copy topic data to given buffer.
-	 *
-	 * @return true only if topic data copied successfully.
-	 */
-	bool update(void *data) { return _sub.copy(data); }
-
-	/**
-	 * Check if the subscription has been updated.
-	 *
-	 * @return true if there has been an update which has been
-	 * copied successfully.
-	 */
-	bool update_if_changed(void *data) { return _sub.update(data); }
-
-	/**
-	 * Check if the topic has been published.
-	 *
-	 * This call will return true if the topic was ever published.
-	 * @return true if the topic has been published at least once.
-	 * If no data is available the buffer will be filled with zeros.
-	 */
-	bool is_published();
-
-	void subscribe_from_beginning(bool from_beginning) { _subscribe_from_beginning = from_beginning; }
-
-	orb_id_t get_topic() const { return _sub.get_topic(); }
-	int get_instance() const { return _sub.get_instance(); }
-
-private:
-
-	uORB::Subscription	_sub;
-
-	bool _subscribe_from_beginning{false}; ///< we need to subscribe from the beginning, e.g. for vehicle_command_acks
-};
-
-
-#endif /* MAVLINK_ORB_SUBSCRIPTION_H_ */