Merge branch 'master' of github.com:PX4/Firmware into asfilter

src/drivers/px4io/px4io_serial.cpp

@@ -157,6 +157,10 @@ private:
 	perf_counter_t		_pc_idle;
 	perf_counter_t		_pc_badidle;
 
+	/* do not allow top copying this class */
+	PX4IO_serial(PX4IO_serial &);
+	PX4IO_serial& operator = (const PX4IO_serial &);
+
 };
 
 IOPacket PX4IO_serial::_dma_buffer;
@@ -173,7 +177,9 @@ PX4IO_serial::PX4IO_serial() :
 	_tx_dma(nullptr),
 	_rx_dma(nullptr),
 	_rx_dma_status(_dma_status_inactive),
-	_pc_txns(perf_alloc(PC_ELAPSED,		"io_txns     ")),
+	_bus_semaphore(SEM_INITIALIZER(0)),
+	_completion_semaphore(SEM_INITIALIZER(0)),
+	_pc_txns(perf_alloc(PC_ELAPSED, "io_txns     ")),
 	_pc_dmasetup(perf_alloc(PC_ELAPSED,	"io_dmasetup ")),
 	_pc_retries(perf_alloc(PC_COUNT,	"io_retries  ")),
 	_pc_timeouts(perf_alloc(PC_COUNT,	"io_timeouts ")),

src/drivers/px4io/px4io_uploader.cpp

@@ -65,7 +65,8 @@
 
 PX4IO_Uploader::PX4IO_Uploader() :
 	_io_fd(-1),
-	_fw_fd(-1)
+	_fw_fd(-1),
+	bl_rev(0)
 {
 }
 
@@ -245,7 +246,7 @@ PX4IO_Uploader::upload(const char *filenames[])
 }
 
 int
-PX4IO_Uploader::recv(uint8_t &c, unsigned timeout)
+PX4IO_Uploader::recv_byte_with_timeout(uint8_t *c, unsigned timeout)
 {
 	struct pollfd fds[1];
 
@@ -262,19 +263,19 @@ PX4IO_Uploader::recv(uint8_t &c, unsigned timeout)
 		return -ETIMEDOUT;
 	}
 
-	read(_io_fd, &c, 1);
+	read(_io_fd, c, 1);
 #ifdef UDEBUG
-	log("recv 0x%02x", c);
+	log("recv_bytes 0x%02x", c);
 #endif
 	return OK;
 }
 
 int
-PX4IO_Uploader::recv(uint8_t *p, unsigned count)
+PX4IO_Uploader::recv_bytes(uint8_t *p, unsigned count)
 {
-	int ret;
+	int ret = OK;
 	while (count--) {
-		ret = recv(*p++, 5000);
+		ret = recv_byte_with_timeout(p++, 5000);
 
 		if (ret != OK)
 			break;
@@ -289,10 +290,10 @@ PX4IO_Uploader::drain()
 	int ret;
 
 	do {
-		// the small recv timeout here is to allow for fast
+		// the small recv_bytes timeout here is to allow for fast
 		// drain when rebooting the io board for a forced
 		// update of the fw without using the safety switch
-		ret = recv(c, 40);
+		ret = recv_byte_with_timeout(&c, 40);
 
 #ifdef UDEBUG
 		if (ret == OK) {
@@ -331,12 +332,12 @@ PX4IO_Uploader::get_sync(unsigned timeout)
 	uint8_t c[2];
 	int ret;
 
-	ret = recv(c[0], timeout);
+	ret = recv_byte_with_timeout(c, timeout);
 
 	if (ret != OK)
 		return ret;
 
-	ret = recv(c[1], timeout);
+	ret = recv_byte_with_timeout(c + 1, timeout);
 
 	if (ret != OK)
 		return ret;
@@ -372,7 +373,7 @@ PX4IO_Uploader::get_info(int param, uint32_t &val)
 	send(param);
 	send(PROTO_EOC);
 
-	ret = recv((uint8_t *)&val, sizeof(val));
+	ret = recv_bytes((uint8_t *)&val, sizeof(val));
 
 	if (ret != OK)
 		return ret;
@@ -513,7 +514,7 @@ PX4IO_Uploader::verify_rev2(size_t fw_size)
 		for (ssize_t i = 0; i < count; i++) {
 			uint8_t c;
 
-			ret = recv(c, 5000);
+			ret = recv_byte_with_timeout(&c, 5000);
 
 			if (ret != OK) {
 				log("%d: got %d waiting for bytes", sent + i, ret);
@@ -600,7 +601,7 @@ PX4IO_Uploader::verify_rev3(size_t fw_size_local)
 	send(PROTO_GET_CRC);
 	send(PROTO_EOC);
 
-	ret = recv((uint8_t*)(&crc), sizeof(crc));
+	ret = recv_bytes((uint8_t*)(&crc), sizeof(crc));
 
 	if (ret != OK) {
 		log("did not receive CRC checksum");

src/drivers/px4io/uploader.h

@@ -74,19 +74,19 @@ private:
 		INFO_BOARD_REV		= 3,		/**< board revision */
 		INFO_FLASH_SIZE		= 4,		/**< max firmware size in bytes */
 
-		PROG_MULTI_MAX		= 60,		/**< protocol max is 255, must be multiple of 4 */
+		PROG_MULTI_MAX		= 248,		/**< protocol max is 255, must be multiple of 4 */
 
 	};
 
 	int			_io_fd;
 	int			_fw_fd;
 
-	uint32_t	bl_rev; /**< bootloader revision */
+	uint32_t		bl_rev; /**< bootloader revision */
 
 	void			log(const char *fmt, ...);
 
-	int			recv(uint8_t &c, unsigned timeout);
+	int			recv_byte_with_timeout(uint8_t *c, unsigned timeout);
-	int			recv(uint8_t *p, unsigned count);
+	int			recv_bytes(uint8_t *p, unsigned count);
 	void			drain();
 	int			send(uint8_t c);
 	int			send(uint8_t *p, unsigned count);

src/modules/mavlink/mavlink_main.cpp

@@ -483,7 +483,12 @@ Mavlink::forward_message(const mavlink_message_t *msg, Mavlink *self)
 	Mavlink *inst;
 	LL_FOREACH(_mavlink_instances, inst) {
 		if (inst != self) {
-			inst->pass_message(msg);
+
+			/* if not in normal mode, we are an onboard link
+			 * onboard links should only pass on messages from the same system ID */
+			if(!(self->_mode != MAVLINK_MODE_NORMAL && msg->sysid != mavlink_system.sysid)) {
+				inst->pass_message(msg);
+			}
 		}
 	}
 }

src/modules/mavlink/mavlink_receiver.cpp

@@ -241,7 +241,8 @@ MavlinkReceiver::handle_message_command_long(mavlink_message_t *msg)
 		} else {
 
 			if (msg->sysid == mavlink_system.sysid && msg->compid == mavlink_system.compid) {
-				warnx("ignoring CMD spoofed with same SYS/COMP ID");
+				warnx("ignoring CMD spoofed with same SYS/COMP (%d/%d) ID",
+				      mavlink_system.sysid, mavlink_system.compid);
 				return;
 			}
 

src/modules/navigator/mission.cpp

@@ -314,30 +314,36 @@ Mission::set_mission_items()
 	/* set previous position setpoint to current */
 	set_previous_pos_setpoint();
 
+	/* get home distance state */
+	bool home_dist_ok = check_dist_1wp();
+	/* the home dist check provides user feedback, so we initialize it to this */
+	bool user_feedback_done = !home_dist_ok;
+
 	/* try setting onboard mission item */
 	if (_param_onboard_enabled.get() && read_mission_item(true, true, &_mission_item)) {
 		/* if mission type changed, notify */
 		if (_mission_type != MISSION_TYPE_ONBOARD) {
-			mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: onboard mission running");
+			mavlink_log_critical(_navigator->get_mavlink_fd(), "onboard mission now running");
 		}
 		_mission_type = MISSION_TYPE_ONBOARD;
 
 	/* try setting offboard mission item */
-	} else if (check_dist_1wp() && read_mission_item(false, true, &_mission_item)) {
+	} else if (home_dist_ok && read_mission_item(false, true, &_mission_item)) {
 		/* if mission type changed, notify */
 		if (_mission_type != MISSION_TYPE_OFFBOARD) {
-			mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: offboard mission running");
+			mavlink_log_critical(_navigator->get_mavlink_fd(), "offboard mission now running");
 		}
 		_mission_type = MISSION_TYPE_OFFBOARD;
 
 	} else {
-		/* no mission available, switch to loiter */
+		/* no mission available or mission finished, switch to loiter */
 		if (_mission_type != MISSION_TYPE_NONE) {
-			mavlink_log_info(_navigator->get_mavlink_fd(),
+			mavlink_log_critical(_navigator->get_mavlink_fd(), "mission finished, loitering");
-					"#audio: mission finished");
+		} else if (!user_feedback_done) {
-		} else {
+			/* only tell users that we got no mission if there has not been any
-			mavlink_log_info(_navigator->get_mavlink_fd(),
+			 * better, more specific feedback yet
-					"#audio: no mission available");
+			 */
+			mavlink_log_critical(_navigator->get_mavlink_fd(), "no valid mission available");
 		}
 		_mission_type = MISSION_TYPE_NONE;
 
@@ -397,7 +403,7 @@ Mission::set_mission_items()
 			takeoff_alt = fmaxf(takeoff_alt, _navigator->get_home_position()->alt + _param_takeoff_alt.get());
 		}
 
-		mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: takeoff to %.1fm above home", (double)(takeoff_alt - _navigator->get_home_position()->alt));
+		mavlink_log_critical(_navigator->get_mavlink_fd(), "takeoff to %.1f meters above home", (double)(takeoff_alt - _navigator->get_home_position()->alt));
 
 		_mission_item.lat = _navigator->get_global_position()->lat;
 		_mission_item.lon = _navigator->get_global_position()->lon;
@@ -483,7 +489,7 @@ Mission::read_mission_item(bool onboard, bool is_current, struct mission_item_s
 		if (dm_read(dm_item, *mission_index_ptr, &mission_item_tmp, len) != len) {
 			/* not supposed to happen unless the datamanager can't access the SD card, etc. */
 			mavlink_log_critical(_navigator->get_mavlink_fd(),
-			                     "#audio: ERROR waypoint could not be read");
+			                     "ERROR waypoint could not be read");
 			return false;
 		}
 
@@ -502,7 +508,7 @@ Mission::read_mission_item(bool onboard, bool is_current, struct mission_item_s
 						/* not supposed to happen unless the datamanager can't access the
 						 * dataman */
 						mavlink_log_critical(_navigator->get_mavlink_fd(),
-								"#audio: ERROR DO JUMP waypoint could not be written");
+								"ERROR DO JUMP waypoint could not be written");
 						return false;
 					}
 				}
@@ -511,8 +517,8 @@ Mission::read_mission_item(bool onboard, bool is_current, struct mission_item_s
 				*mission_index_ptr = mission_item_tmp.do_jump_mission_index;
 
 			} else {
-				mavlink_log_info(_navigator->get_mavlink_fd(),
+				mavlink_log_critical(_navigator->get_mavlink_fd(),
-						 "#audio: DO JUMP repetitions completed");
+						 "DO JUMP repetitions completed");
 				/* no more DO_JUMPS, therefore just try to continue with next mission item */
 				(*mission_index_ptr)++;
 			}
@@ -526,7 +532,7 @@ Mission::read_mission_item(bool onboard, bool is_current, struct mission_item_s
 
 	/* we have given up, we don't want to cycle forever */
 	mavlink_log_critical(_navigator->get_mavlink_fd(),
-			     "#audio: ERROR DO JUMP is cycling, giving up");
+			     "ERROR DO JUMP is cycling, giving up");
 	return false;
 }