Merge remote-tracking branch 'upstream/master' into launchdetectorreset

src/modules/mavlink/mavlink_main.cpp

@@ -1555,32 +1555,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 (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;
-			}
-		}
-
-	} else {
-		/* stream not found, nothing to disable */
+	if (interval == 0) {
+		/* stream was not active and is requested to be disabled, do nothing */
 		return OK;
 	}
 
+	/* search for stream with specified name in supported streams list */
+	for (unsigned int i = 0; streams_list[i] != nullptr; i++) {
+
+		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;
 }
 

src/modules/mavlink/mavlink_messages.cpp

@@ -1275,6 +1275,7 @@ protected:
 			// New message
 			mavlink_msg_rc_channels_send(_channel,
 					rc.timestamp_publication / 1000,
+					rc.channel_count,
 					((rc.channel_count > 0) ? rc.values[0] : UINT16_MAX),
 					((rc.channel_count > 1) ? rc.values[1] : UINT16_MAX),
 					((rc.channel_count > 2) ? rc.values[2] : UINT16_MAX),
@@ -1293,7 +1294,6 @@ protected:
 					((rc.channel_count > 15) ? rc.values[15] : UINT16_MAX),
 					((rc.channel_count > 16) ? rc.values[16] : UINT16_MAX),
 					((rc.channel_count > 17) ? rc.values[17] : UINT16_MAX),
-					rc.channel_count,
 					rc.rssi);
 		}
 	}
@@ -1625,7 +1625,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),

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;
 	}
 }
 

src/modules/systemlib/perf_counter.c

@@ -301,7 +301,7 @@ perf_print_counter_fd(int fd, perf_counter_t handle)
 	case PC_ELAPSED: {
 		struct perf_ctr_elapsed *pce = (struct perf_ctr_elapsed *)handle;
 
-		dprintf(fd, "%s: %llu events, %lluus elapsed, %llu avg, min %lluus max %lluus\n",
+		dprintf(fd, "%s: %llu events, %lluus elapsed, %lluus avg, min %lluus max %lluus\n",
 		       handle->name,
 		       pce->event_count,
 		       pce->time_total,
@@ -314,7 +314,7 @@ perf_print_counter_fd(int fd, perf_counter_t handle)
 	case PC_INTERVAL: {
 		struct perf_ctr_interval *pci = (struct perf_ctr_interval *)handle;
 
-		dprintf(fd, "%s: %llu events, %llu avg, min %lluus max %lluus\n",
+		dprintf(fd, "%s: %llu events, %lluus avg, min %lluus max %lluus\n",
 		       handle->name,
 		       pci->event_count,
 		       (pci->time_last - pci->time_first) / pci->event_count,