Merge remote-tracking branch 'origin/master' into offboard2_externalsetpointmessages

Conflicts:
	src/modules/mavlink/mavlink_receiver.cpp

4 files changed, 90 insertions, 21 deletions

diff --git a/src/modules/mavlink/mavlink_ftp.cpp b/src/modules/mavlink/mavlink_ftp.cpp
index 17d1babff..00c8df18c 100644
--- a/src/modules/mavlink/mavlink_ftp.cpp
+++ b/src/modules/mavlink/mavlink_ftp.cpp
@@ -39,6 +39,9 @@
 
 #include "mavlink_ftp.h"
 
+// Uncomment the line below to get better debug output. Never commit with this left on.
+//#define MAVLINK_FTP_DEBUG
+
 MavlinkFTP *MavlinkFTP::_server;
 
 MavlinkFTP *
@@ -125,7 +128,9 @@ MavlinkFTP::_worker(Request *req)
 		warnx("ftp: bad crc");
 	}
 
-	//printf("ftp: channel %u opc %u size %u offset %u\n", req->channel(), hdr->opcode, hdr->size, hdr->offset);
+#ifdef MAVLINK_FTP_DEBUG
+	printf("ftp: channel %u opc %u size %u offset %u\n", req->channel(), hdr->opcode, hdr->size, hdr->offset);
+#endif
 
 	switch (hdr->opcode) {
 	case kCmdNone:
@@ -172,7 +177,9 @@ out:
 	// handle success vs. error
 	if (errorCode == kErrNone) {
 		hdr->opcode = kRspAck;
-		//warnx("FTP: ack\n");
+#ifdef MAVLINK_FTP_DEBUG
+		warnx("FTP: ack\n");
+#endif
 	} else {
 		warnx("FTP: nak %u", errorCode);
 		hdr->opcode = kRspNak;
@@ -217,7 +224,9 @@ MavlinkFTP::_workList(Request *req)
 		return kErrNotDir;
 	}
     
-	//warnx("FTP: list %s offset %d", dirPath, hdr->offset);
+#ifdef MAVLINK_FTP_DEBUG
+	warnx("FTP: list %s offset %d", dirPath, hdr->offset);
+#endif
 
 	ErrorCode errorCode = kErrNone;
 	struct dirent entry, *result = nullptr;
@@ -247,11 +256,13 @@ MavlinkFTP::_workList(Request *req)
 
 		uint32_t fileSize = 0;
 		char buf[256];
+		char direntType;
 
-		// store the type marker
+		// Determine the directory entry type
 		switch (entry.d_type) {
 		case DTYPE_FILE:
-			hdr->data[offset++] = kDirentFile;
+			// For files we get the file size as well
+			direntType = kDirentFile;
 			snprintf(buf, sizeof(buf), "%s/%s", dirPath, entry.d_name);
 			struct stat st;
 			if (stat(buf, &st) == 0) {
@@ -259,29 +270,34 @@ MavlinkFTP::_workList(Request *req)
 			}
 			break;
 		case DTYPE_DIRECTORY:
-			hdr->data[offset++] = kDirentDir;
+			direntType = kDirentDir;
 			break;
 		default:
-			hdr->data[offset++] = kDirentUnknown;
+			direntType = kDirentUnknown;
 			break;
 		}
 		
 		if (entry.d_type == DTYPE_FILE) {
+			// Files send filename and file length
 			snprintf(buf, sizeof(buf), "%s\t%d", entry.d_name, fileSize);
 		} else {
+			// Everything else just sends name
 			strncpy(buf, entry.d_name, sizeof(buf));
 			buf[sizeof(buf)-1] = 0;
 		}
 		size_t nameLen = strlen(buf);
 
-		// name too big to fit?
-		if ((nameLen + offset + 2) > kMaxDataLength) {
+		// Do we have room for the name, the one char directory identifier and the null terminator?
+		if ((offset + nameLen + 2) > kMaxDataLength) {
 			break;
 		}
 		
-		// copy the name, which we know will fit
+		// Move the data into the buffer
+		hdr->data[offset++] = direntType;
 		strcpy((char *)&hdr->data[offset], buf);
-		//printf("FTP: list %s %s\n", dirPath, (char *)&hdr->data[offset-1]);
+#ifdef MAVLINK_FTP_DEBUG
+		printf("FTP: list %s %s\n", dirPath, (char *)&hdr->data[offset-1]);
+#endif
 		offset += nameLen + 1;
 	}
 
@@ -342,7 +358,9 @@ MavlinkFTP::_workRead(Request *req)
 	}
 
 	// Seek to the specified position
-	//warnx("seek %d", hdr->offset);
+#ifdef MAVLINK_FTP_DEBUG
+	warnx("seek %d", hdr->offset);
+#endif
 	if (lseek(_session_fds[session_index], hdr->offset, SEEK_SET) < 0) {
 		// Unable to see to the specified location
 		warnx("seek fail");
diff --git a/src/modules/mavlink/mavlink_ftp.h b/src/modules/mavlink/mavlink_ftp.h
index 800c98b69..43de89de9 100644
--- a/src/modules/mavlink/mavlink_ftp.h
+++ b/src/modules/mavlink/mavlink_ftp.h
@@ -154,15 +154,17 @@ private:
 
 			if (!success) {
 				warnx("FTP TX ERR");
-			} 
-			// else {
-				// warnx("wrote: sys: %d, comp: %d, chan: %d, len: %d, checksum: %d",
-				// 	_mavlink->get_system_id(),
-				// 	_mavlink->get_component_id(),
-				// 	_mavlink->get_channel(),
-				// 	len,
-				// 	msg.checksum);
-			// }
+			}
+#ifdef MAVLINK_FTP_DEBUG
+			else {
+				warnx("wrote: sys: %d, comp: %d, chan: %d, len: %d, checksum: %d",
+				      _mavlink->get_system_id(),
+				      _mavlink->get_component_id(),
+				      _mavlink->get_channel(),
+				      len,
+				      msg.checksum);
+			}
+#endif
 		}
 
 		uint8_t		*rawData() { return &_message.data[0]; }
diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index da651e4e7..7dd043bbe 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -110,6 +110,7 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) :
 	_force_sp_pub(-1),
 	_pos_sp_triplet_pub(-1),
 	_vicon_position_pub(-1),
+	_vision_position_pub(-1),
 	_telemetry_status_pub(-1),
 	_rc_pub(-1),
 	_manual_pub(-1),
@@ -157,6 +158,11 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
 	case MAVLINK_MSG_ID_SET_ATTITUDE_TARGET:
 		handle_message_set_attitude_target(msg);
 		break;
+
+	case MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE:
+		handle_message_vision_position_estimate(msg);
+		break;
+
 	case MAVLINK_MSG_ID_RADIO_STATUS:
 		handle_message_radio_status(msg);
 		break;
@@ -524,6 +530,45 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t
 }
 
 void
+MavlinkReceiver::handle_message_vision_position_estimate(mavlink_message_t *msg)
+{
+	mavlink_vision_position_estimate_t pos;
+	mavlink_msg_vision_position_estimate_decode(msg, &pos);
+
+	struct vision_position_estimate vision_position;
+	memset(&vision_position, 0, sizeof(vision_position));
+
+	// Use the component ID to identify the vision sensor
+	vision_position.id = msg->compid;
+
+	vision_position.timestamp_boot = hrt_absolute_time();
+	vision_position.timestamp_computer = pos.usec;
+	vision_position.x = pos.x;
+	vision_position.y = pos.y;
+	vision_position.z = pos.z;
+
+	// XXX fix this
+	vision_position.vx = 0.0f;
+	vision_position.vy = 0.0f;
+	vision_position.vz = 0.0f;
+
+	math::Quaternion q;
+	q.from_euler(pos.roll, pos.pitch, pos.yaw);
+
+	vision_position.q[0] = q(0);
+	vision_position.q[1] = q(1);
+	vision_position.q[2] = q(2);
+	vision_position.q[3] = q(3);
+
+	if (_vision_position_pub < 0) {
+		_vision_position_pub = orb_advertise(ORB_ID(vision_position_estimate), &vision_position);
+
+	} else {
+		orb_publish(ORB_ID(vision_position_estimate), _vision_position_pub, &vision_position);
+	}
+}
+
+void
 MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg)
 {
 	mavlink_set_attitude_target_t set_attitude_target;
@@ -610,6 +655,7 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg)
 		}
 	}
 }
+
 void
 MavlinkReceiver::handle_message_radio_status(mavlink_message_t *msg)
 {
diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h
index e3b635dd2..ed47a3a63 100644
--- a/src/modules/mavlink/mavlink_receiver.h
+++ b/src/modules/mavlink/mavlink_receiver.h
@@ -58,6 +58,7 @@
 #include <uORB/topics/vehicle_global_velocity_setpoint.h>
 #include <uORB/topics/position_setpoint_triplet.h>
 #include <uORB/topics/vehicle_vicon_position.h>
+#include <uORB/topics/vision_position_estimate.h>
 #include <uORB/topics/vehicle_attitude_setpoint.h>
 #include <uORB/topics/vehicle_rates_setpoint.h>
 #include <uORB/topics/optical_flow.h>
@@ -113,6 +114,7 @@ private:
 	void handle_message_optical_flow(mavlink_message_t *msg);
 	void handle_message_set_mode(mavlink_message_t *msg);
 	void handle_message_vicon_position_estimate(mavlink_message_t *msg);
+	void handle_message_vision_position_estimate(mavlink_message_t *msg);
 	void handle_message_quad_swarm_roll_pitch_yaw_thrust(mavlink_message_t *msg);
 	void handle_message_set_position_target_local_ned(mavlink_message_t *msg);
 	void handle_message_set_attitude_target(mavlink_message_t *msg);
@@ -149,6 +151,7 @@ private:
 	orb_advert_t _force_sp_pub;
 	orb_advert_t _pos_sp_triplet_pub;
 	orb_advert_t _vicon_position_pub;
+	orb_advert_t _vision_position_pub;
 	orb_advert_t _telemetry_status_pub;
 	orb_advert_t _rc_pub;
 	orb_advert_t _manual_pub;