Merge branch 'master' into navigator_rewrite_estimator

3 files changed, 36 insertions, 28 deletions

diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp
index a182d940d..ee5e7aa3a 100644
--- a/src/modules/mavlink/mavlink_main.cpp
+++ b/src/modules/mavlink/mavlink_main.cpp
@@ -1576,32 +1576,36 @@ Mavlink::configure_stream(const char *stream_name, const float rate)
 				/* delete stream */
 				LL_DELETE(_streams, stream);
 				delete stream;
+				warnx("deleted stream %s", stream->get_name());
 			}
 
 			return OK;
 		}
 	}
 
-	if (interval > 0) {
-		/* search for stream with specified name in supported streams list */
-		for (unsigned int i = 0; streams_list[i] != nullptr; i++) {
+	if (interval == 0) {
+		/* stream was not active and is requested to be disabled, do nothing */
+		return OK;
+	}
 
-			if (strcmp(stream_name, streams_list[i]->get_name()) == 0) {
-				/* create new instance */
-				stream = streams_list[i]->new_instance();
-				stream->set_channel(get_channel());
-				stream->set_interval(interval);
-				stream->subscribe(this);
-				LL_APPEND(_streams, stream);
-				return OK;
-			}
-		}
+	/* search for stream with specified name in supported streams list */
+	for (unsigned int i = 0; streams_list[i] != nullptr; i++) {
 
-	} else {
-		/* stream not found, nothing to disable */
-		return OK;
+		if (strcmp(stream_name, streams_list[i]->get_name()) == 0) {
+			/* create new instance */
+			stream = streams_list[i]->new_instance();
+			stream->set_channel(get_channel());
+			stream->set_interval(interval);
+			stream->subscribe(this);
+			LL_APPEND(_streams, stream);
+
+			return OK;
+		}
 	}
 
+	/* if we reach here, the stream list does not contain the stream */
+	warnx("stream %s not found", stream_name);
+
 	return ERROR;
 }
 
diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp
index 0017b4332..b443b8af8 100644
--- a/src/modules/mavlink/mavlink_messages.cpp
+++ b/src/modules/mavlink/mavlink_messages.cpp
@@ -1652,7 +1652,7 @@ StreamListItem *streams_list[] = {
 	new StreamListItem(&MavlinkStreamLocalPositionSetpoint::new_instance, &MavlinkStreamLocalPositionSetpoint::get_name_static),
 	new StreamListItem(&MavlinkStreamRollPitchYawThrustSetpoint::new_instance, &MavlinkStreamRollPitchYawThrustSetpoint::get_name_static),
 	new StreamListItem(&MavlinkStreamRollPitchYawRatesThrustSetpoint::new_instance, &MavlinkStreamRollPitchYawRatesThrustSetpoint::get_name_static),
-	new StreamListItem(&MavlinkStreamRCChannelsRaw::new_instance, &MavlinkStreamGlobalPositionInt::get_name_static),
+	new StreamListItem(&MavlinkStreamRCChannelsRaw::new_instance, &MavlinkStreamRCChannelsRaw::get_name_static),
 	new StreamListItem(&MavlinkStreamManualControl::new_instance, &MavlinkStreamManualControl::get_name_static),
 	new StreamListItem(&MavlinkStreamOpticalFlow::new_instance, &MavlinkStreamOpticalFlow::get_name_static),
 	new StreamListItem(&MavlinkStreamAttitudeControls::new_instance, &MavlinkStreamAttitudeControls::get_name_static),
diff --git a/src/modules/mavlink/mavlink_orb_subscription.cpp b/src/modules/mavlink/mavlink_orb_subscription.cpp
index 901fa8f05..3807323c2 100644
--- a/src/modules/mavlink/mavlink_orb_subscription.cpp
+++ b/src/modules/mavlink/mavlink_orb_subscription.cpp
@@ -44,6 +44,8 @@
 #include <uORB/uORB.h>
 #include <stdio.h>
 
+#include <systemlib/err.h>
+
 #include "mavlink_orb_subscription.h"
 
 MavlinkOrbSubscription::MavlinkOrbSubscription(const orb_id_t topic) :
@@ -77,21 +79,23 @@ MavlinkOrbSubscription::update(uint64_t *time, void* data)
 		time_topic = 0;
 	}
 
-	if (orb_copy(_topic, _fd, data)) {
-		/* error copying topic data */
-		memset(data, 0, _topic->o_size);
-		return false;
+	if (time_topic != *time) {
 
-	} else {
-		/* data copied successfully */
-		_published = true;
-		if (time_topic != *time) {
-			*time = time_topic;
-			return true;
+		if (orb_copy(_topic, _fd, data)) {
+			/* error copying topic data */
+			memset(data, 0, _topic->o_size);
+			//warnx("err copy, fd: %d, obj: %s, size: %d", _fd, _topic->o_name, _topic->o_size);
+			return false;
 
 		} else {
-			return false;
+			/* data copied successfully */
+			_published = true;
+			*time = time_topic;
+			return true;
 		}
+
+	} else {
+		return false;
 	}
 }