mavlink: add new streams in main loop, minor cleanup and fixes

5 files changed, 99 insertions, 48 deletions

diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp
index c97786553..b996413a8 100644
--- a/src/modules/mavlink/mavlink_main.cpp
+++ b/src/modules/mavlink/mavlink_main.cpp
@@ -171,6 +171,9 @@ Mavlink::Mavlink() :
 	_subscriptions(nullptr),
 	_streams(nullptr),
 	mission_pub(-1),
+	mavlink_param_queue_index(0),
+	_subscribe_to_stream(nullptr),
+	_subscribe_to_stream_rate(0.0f),
 
 /* performance counters */
 	_loop_perf(perf_alloc(PC_ELAPSED, "mavlink"))
@@ -301,6 +304,7 @@ Mavlink::destroy_all_instances()
 
 		while (inst_to_del->thread_running) {
 			printf(".");
+			fflush(stdout);
 			usleep(10000);
 			iterations++;
 
@@ -1443,7 +1447,7 @@ Mavlink::mavlink_missionlib_send_gcs_string(const char *string)
 	}
 }
 
-MavlinkOrbSubscription *Mavlink::add_orb_subscription(const orb_id_t topic, const size_t size)
+MavlinkOrbSubscription *Mavlink::add_orb_subscription(const orb_id_t topic)
 {
 	/* check if already subscribed to this topic */
 	MavlinkOrbSubscription *sub;
@@ -1456,7 +1460,7 @@ MavlinkOrbSubscription *Mavlink::add_orb_subscription(const orb_id_t topic, cons
 	}
 
 	/* add new subscription */
-	MavlinkOrbSubscription *sub_new = new MavlinkOrbSubscription(topic, size);
+	MavlinkOrbSubscription *sub_new = new MavlinkOrbSubscription(topic);
 
 	LL_APPEND(_subscriptions, sub_new);
 
@@ -1509,6 +1513,34 @@ Mavlink::configure_stream(const char *stream_name, const float rate)
 	return ERROR;
 }
 
+void
+Mavlink::configure_stream_threadsafe(const char *stream_name, const float rate)
+{
+	/* orb subscription must be done from the main thread,
+	 * set _subscribe_to_stream and _subscribe_to_stream_rate fields
+	 * which polled in mavlink main loop */
+	if (!_task_should_exit) {
+		/* wait for previous subscription completion */
+		while (_subscribe_to_stream != nullptr) {
+			usleep(MAIN_LOOP_DELAY / 2);
+		}
+
+		/* copy stream name */
+		unsigned n = strlen(stream_name) + 1;
+		char *s = new char[n];
+		memcpy(s, stream_name, n);
+
+		/* set subscription task */
+		_subscribe_to_stream_rate = rate;
+		_subscribe_to_stream = s;
+
+		/* wait for subscription */
+		do {
+			usleep(MAIN_LOOP_DELAY / 2);
+		} while (_subscribe_to_stream != nullptr);
+	}
+}
+
 int
 Mavlink::task_main(int argc, char *argv[])
 {
@@ -1686,8 +1718,8 @@ Mavlink::task_main(int argc, char *argv[])
 
 	thread_running = true;
 
-	MavlinkOrbSubscription *param_sub = add_orb_subscription(ORB_ID(parameter_update), sizeof(parameter_update_s));
-	MavlinkOrbSubscription *status_sub = add_orb_subscription(ORB_ID(vehicle_status), sizeof(vehicle_status_s));
+	MavlinkOrbSubscription *param_sub = add_orb_subscription(ORB_ID(parameter_update));
+	MavlinkOrbSubscription *status_sub = add_orb_subscription(ORB_ID(vehicle_status));
 
 	struct vehicle_status_s *status = (struct vehicle_status_s *) status_sub->get_data();
 
@@ -1726,6 +1758,9 @@ Mavlink::task_main(int argc, char *argv[])
 		break;
 	}
 
+	/* don't send parameters on startup without request */
+	mavlink_param_queue_index = param_count();
+
 	MavlinkRateLimiter slow_rate_limiter(2000.0f / rate_mult);
 	MavlinkRateLimiter fast_rate_limiter(100.0f / rate_mult);
 
@@ -1752,6 +1787,24 @@ Mavlink::task_main(int argc, char *argv[])
 			}
 		}
 
+		/* check for requested subscriptions */
+		if (_subscribe_to_stream != nullptr) {
+			if (OK == configure_stream(_subscribe_to_stream, _subscribe_to_stream_rate)) {
+				if (_subscribe_to_stream_rate > 0.0f) {
+					warnx("stream %s on device %s enabled with rate %.1f Hz", _subscribe_to_stream, device_name, _subscribe_to_stream_rate);
+
+				} else {
+					warnx("stream %s on device %s disabled", _subscribe_to_stream, device_name);
+				}
+
+			} else {
+				warnx("stream %s not found", _subscribe_to_stream, device_name);
+			}
+			delete _subscribe_to_stream;
+			_subscribe_to_stream = nullptr;
+		}
+
+		/* update streams */
 		MavlinkStream *stream;
 		LL_FOREACH(_streams, stream) {
 			stream->update(t);
@@ -1794,6 +1847,9 @@ Mavlink::task_main(int argc, char *argv[])
 		perf_end(_loop_perf);
 	}
 
+	delete _subscribe_to_stream;
+	_subscribe_to_stream = nullptr;
+
 	/* wait for threads to complete */
 	pthread_join(receive_thread, NULL);
 
@@ -1911,17 +1967,7 @@ Mavlink::stream(int argc, char *argv[])
 		Mavlink *inst = get_instance_for_device(device_name);
 
 		if (inst != nullptr) {
-			if (OK == inst->configure_stream(stream_name, rate)) {
-				if (rate > 0.0f) {
-					warnx("stream %s on device %s enabled with rate %.1f Hz", stream_name, device_name, rate);
-
-				} else {
-					warnx("stream %s on device %s disabled", stream_name, device_name);
-				}
-
-			} else {
-				warnx("stream %s not found", stream_name, device_name);
-			}
+			inst->configure_stream_threadsafe(stream_name, rate);
 
 		} else {
 			errx(1, "mavlink for device %s is not running", device_name);
diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h
index 532c9bcee..ebea53d52 100644
--- a/src/modules/mavlink/mavlink_main.h
+++ b/src/modules/mavlink/mavlink_main.h
@@ -180,7 +180,7 @@ public:
 	 */
 	int		set_hil_enabled(bool hil_enabled);
 
-	MavlinkOrbSubscription *add_orb_subscription(const orb_id_t topic, size_t size);
+	MavlinkOrbSubscription *add_orb_subscription(const orb_id_t topic);
 
 	mavlink_channel_t get_channel();
 
@@ -235,6 +235,9 @@ private:
 
 	bool mavlink_link_termination_allowed;
 
+	char *	_subscribe_to_stream;
+	float	_subscribe_to_stream_rate;
+
 	/**
 	 * Send one parameter.
 	 *
@@ -296,6 +299,7 @@ private:
 	int mavlink_open_uart(int baudrate, const char *uart_name, struct termios *uart_config_original, bool *is_usb);
 
 	int configure_stream(const char *stream_name, const float rate);
+	void configure_stream_threadsafe(const char *stream_name, const float rate);
 
 	static int	mavlink_dev_ioctl(struct file *filep, int cmd, unsigned long arg);
 
diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp
index 820faae1c..8be7d76d7 100644
--- a/src/modules/mavlink/mavlink_messages.cpp
+++ b/src/modules/mavlink/mavlink_messages.cpp
@@ -207,10 +207,10 @@ private:
 protected:
 	void subscribe(Mavlink *mavlink)
 	{
-		status_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_status), sizeof(struct vehicle_status_s));
+		status_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_status));
 		status = (struct vehicle_status_s *)status_sub->get_data();
 
-		pos_sp_triplet_sub = mavlink->add_orb_subscription(ORB_ID(position_setpoint_triplet), sizeof(position_setpoint_triplet_s));
+		pos_sp_triplet_sub = mavlink->add_orb_subscription(ORB_ID(position_setpoint_triplet));
 		pos_sp_triplet = (struct position_setpoint_triplet_s *)pos_sp_triplet_sub->get_data();
 	}
 
@@ -255,7 +255,7 @@ private:
 protected:
 	void subscribe(Mavlink *mavlink)
 	{
-		status_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_status), sizeof(struct vehicle_status_s));
+		status_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_status));
 		status = (struct vehicle_status_s *)status_sub->get_data();
 	}
 
@@ -310,7 +310,7 @@ private:
 protected:
 	void subscribe(Mavlink *mavlink)
 	{
-		sensor_sub = mavlink->add_orb_subscription(ORB_ID(sensor_combined), sizeof(struct sensor_combined_s));
+		sensor_sub = mavlink->add_orb_subscription(ORB_ID(sensor_combined));
 		sensor = (struct sensor_combined_s *)sensor_sub->get_data();
 	}
 
@@ -376,7 +376,7 @@ private:
 protected:
 	void subscribe(Mavlink *mavlink)
 	{
-		att_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_attitude), sizeof(struct vehicle_attitude_s));
+		att_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_attitude));
 		att = (struct vehicle_attitude_s *)att_sub->get_data();
 	}
 
@@ -412,7 +412,7 @@ private:
 protected:
 	void subscribe(Mavlink *mavlink)
 	{
-		att_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_attitude), sizeof(struct vehicle_attitude_s));
+		att_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_attitude));
 		att = (struct vehicle_attitude_s *)att_sub->get_data();
 	}
 
@@ -465,19 +465,19 @@ private:
 protected:
 	void subscribe(Mavlink *mavlink)
 	{
-		att_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_attitude), sizeof(struct vehicle_attitude_s));
+		att_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_attitude));
 		att = (struct vehicle_attitude_s *)att_sub->get_data();
 
-		pos_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_global_position), sizeof(struct vehicle_global_position_s));
+		pos_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_global_position));
 		pos = (struct vehicle_global_position_s *)pos_sub->get_data();
 
-		armed_sub = mavlink->add_orb_subscription(ORB_ID(actuator_armed), sizeof(struct actuator_armed_s));
+		armed_sub = mavlink->add_orb_subscription(ORB_ID(actuator_armed));
 		armed = (struct actuator_armed_s *)armed_sub->get_data();
 
-		act_sub = mavlink->add_orb_subscription(ORB_ID(actuator_controls_0), sizeof(struct actuator_controls_s));
+		act_sub = mavlink->add_orb_subscription(ORB_ID(actuator_controls_0));
 		act = (struct actuator_controls_s *)act_sub->get_data();
 
-		airspeed_sub = mavlink->add_orb_subscription(ORB_ID(airspeed), sizeof(struct airspeed_s));
+		airspeed_sub = mavlink->add_orb_subscription(ORB_ID(airspeed));
 		airspeed = (struct airspeed_s *)airspeed_sub->get_data();
 	}
 
@@ -524,7 +524,7 @@ private:
 protected:
 	void subscribe(Mavlink *mavlink)
 	{
-		gps_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_gps_position), sizeof(struct vehicle_gps_position_s));
+		gps_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_gps_position));
 		gps = (struct vehicle_gps_position_s *)gps_sub->get_data();
 	}
 
@@ -570,10 +570,10 @@ private:
 protected:
 	void subscribe(Mavlink *mavlink)
 	{
-		pos_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_global_position), sizeof(struct vehicle_global_position_s));
+		pos_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_global_position));
 		pos = (struct vehicle_global_position_s *)pos_sub->get_data();
 
-		home_sub = mavlink->add_orb_subscription(ORB_ID(home_position), sizeof(struct home_position_s));
+		home_sub = mavlink->add_orb_subscription(ORB_ID(home_position));
 		home = (struct home_position_s *)home_sub->get_data();
 	}
 
@@ -616,7 +616,7 @@ private:
 protected:
 	void subscribe(Mavlink *mavlink)
 	{
-		pos_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_local_position), sizeof(struct vehicle_local_position_s));
+		pos_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_local_position));
 		pos = (struct vehicle_local_position_s *)pos_sub->get_data();
 	}
 
@@ -656,7 +656,7 @@ private:
 protected:
 	void subscribe(Mavlink *mavlink)
 	{
-		home_sub = mavlink->add_orb_subscription(ORB_ID(home_position), sizeof(struct home_position_s));
+		home_sub = mavlink->add_orb_subscription(ORB_ID(home_position));
 		home = (struct home_position_s *)home_sub->get_data();
 	}
 
@@ -707,7 +707,7 @@ protected:
 			ORB_ID(actuator_outputs_3)
 		};
 
-		act_sub = mavlink->add_orb_subscription(act_topics[_n], sizeof(struct actuator_outputs_s));
+		act_sub = mavlink->add_orb_subscription(act_topics[_n]);
 		act = (struct actuator_outputs_s *)act_sub->get_data();
 	}
 
@@ -756,13 +756,13 @@ private:
 protected:
 	void subscribe(Mavlink *mavlink)
 	{
-		status_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_status), sizeof(struct vehicle_status_s));
+		status_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_status));
 		status = (struct vehicle_status_s *)status_sub->get_data();
 
-		pos_sp_triplet_sub = mavlink->add_orb_subscription(ORB_ID(position_setpoint_triplet), sizeof(position_setpoint_triplet_s));
+		pos_sp_triplet_sub = mavlink->add_orb_subscription(ORB_ID(position_setpoint_triplet));
 		pos_sp_triplet = (struct position_setpoint_triplet_s *)pos_sp_triplet_sub->get_data();
 
-		act_sub = mavlink->add_orb_subscription(ORB_ID(actuator_outputs_0), sizeof(struct actuator_outputs_s));
+		act_sub = mavlink->add_orb_subscription(ORB_ID(actuator_outputs_0));
 		act = (struct actuator_outputs_s *)act_sub->get_data();
 	}
 
@@ -861,7 +861,7 @@ private:
 protected:
 	void subscribe(Mavlink *mavlink)
 	{
-		pos_sp_triplet_sub = mavlink->add_orb_subscription(ORB_ID(position_setpoint_triplet), sizeof(position_setpoint_triplet_s));
+		pos_sp_triplet_sub = mavlink->add_orb_subscription(ORB_ID(position_setpoint_triplet));
 		pos_sp_triplet = (struct position_setpoint_triplet_s *)pos_sp_triplet_sub->get_data();
 	}
 
@@ -899,7 +899,7 @@ private:
 protected:
 	void subscribe(Mavlink *mavlink)
 	{
-		pos_sp_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_local_position_setpoint), sizeof(vehicle_local_position_setpoint_s));
+		pos_sp_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_local_position_setpoint));
 		pos_sp = (struct vehicle_local_position_setpoint_s *)pos_sp_sub->get_data();
 	}
 
@@ -937,7 +937,7 @@ private:
 protected:
 	void subscribe(Mavlink *mavlink)
 	{
-		att_sp_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_attitude_setpoint), sizeof(vehicle_attitude_setpoint_s));
+		att_sp_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_attitude_setpoint));
 		att_sp = (struct vehicle_attitude_setpoint_s *)att_sp_sub->get_data();
 	}
 
@@ -975,7 +975,7 @@ private:
 protected:
 	void subscribe(Mavlink *mavlink)
 	{
-		att_rates_sp_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_rates_setpoint), sizeof(vehicle_rates_setpoint_s));
+		att_rates_sp_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_rates_setpoint));
 		att_rates_sp = (struct vehicle_rates_setpoint_s *)att_rates_sp_sub->get_data();
 	}
 
@@ -1013,7 +1013,7 @@ private:
 protected:
 	void subscribe(Mavlink *mavlink)
 	{
-		rc_sub = mavlink->add_orb_subscription(ORB_ID(input_rc), sizeof(struct rc_input_values));
+		rc_sub = mavlink->add_orb_subscription(ORB_ID(input_rc));
 		rc = (struct rc_input_values *)rc_sub->get_data();
 	}
 
@@ -1062,7 +1062,7 @@ private:
 protected:
 	void subscribe(Mavlink *mavlink)
 	{
-		manual_sub = mavlink->add_orb_subscription(ORB_ID(manual_control_setpoint), sizeof(struct manual_control_setpoint_s));
+		manual_sub = mavlink->add_orb_subscription(ORB_ID(manual_control_setpoint));
 		manual = (struct manual_control_setpoint_s *)manual_sub->get_data();
 	}
 
diff --git a/src/modules/mavlink/mavlink_orb_subscription.cpp b/src/modules/mavlink/mavlink_orb_subscription.cpp
index e1208bca9..6279e5366 100644
--- a/src/modules/mavlink/mavlink_orb_subscription.cpp
+++ b/src/modules/mavlink/mavlink_orb_subscription.cpp
@@ -42,13 +42,14 @@
 #include <stdlib.h>
 #include <string.h>
 #include <uORB/uORB.h>
+#include <stdio.h>
 
 #include "mavlink_orb_subscription.h"
 
-MavlinkOrbSubscription::MavlinkOrbSubscription(const orb_id_t topic, size_t size) : _topic(topic), _last_check(0), next(nullptr)
+MavlinkOrbSubscription::MavlinkOrbSubscription(const orb_id_t topic) : _topic(topic), _last_check(0), next(nullptr)
 {
-	_data = malloc(size);
-	memset(_data, 0, size);
+	_data = malloc(topic->o_size);
+	memset(_data, 0, topic->o_size);
 	_fd = orb_subscribe(_topic);
 }
 
@@ -58,7 +59,7 @@ MavlinkOrbSubscription::~MavlinkOrbSubscription()
 	free(_data);
 }
 
-const struct orb_metadata *
+const orb_id_t
 MavlinkOrbSubscription::get_topic()
 {
 	return _topic;
diff --git a/src/modules/mavlink/mavlink_orb_subscription.h b/src/modules/mavlink/mavlink_orb_subscription.h
index 3cf33ccef..eacc27034 100644
--- a/src/modules/mavlink/mavlink_orb_subscription.h
+++ b/src/modules/mavlink/mavlink_orb_subscription.h
@@ -50,12 +50,12 @@ class MavlinkOrbSubscription
 public:
 	MavlinkOrbSubscription *next;
 
-	MavlinkOrbSubscription(const orb_id_t topic, size_t size);
+	MavlinkOrbSubscription(const orb_id_t topic);
 	~MavlinkOrbSubscription();
 
 	bool update(const hrt_abstime t);
 	void *get_data();
-	const struct orb_metadata *get_topic();
+	const orb_id_t get_topic();
 
 private:
 	const orb_id_t _topic;