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

mavlink/include/mavlink/v1.0

@@ -1 +1 @@
-Subproject commit 04b1ad5b284d5e916858ca9f928e93d97bbf6ad9
+Subproject commit 4cd384001b5373e1ecaa6c4cd66994855cec4742

src/modules/mavlink/mavlink_ftp.cpp

@@ -35,6 +35,7 @@
 #include <unistd.h>
 #include <stdio.h>
 #include <fcntl.h>
+#include <sys/stat.h>
 
 #include "mavlink_ftp.h"
 
@@ -190,6 +191,8 @@ void
 MavlinkFTP::_reply(Request *req)
 {
 	auto hdr = req->header();
+	
+	hdr->magic = kProtocolMagic;
 
 	// message is assumed to be already constructed in the request buffer, so generate the CRC
 	hdr->crc32 = 0;
@@ -242,15 +245,18 @@ MavlinkFTP::_workList(Request *req)
 			break;
 		}
 
-		// name too big to fit?
+		uint32_t fileSize = 0;
-		if ((strlen(entry.d_name) + offset + 2) > kMaxDataLength) {
+		char buf[256];
-			break;
-		}
 
 		// store the type marker
 		switch (entry.d_type) {
 		case DTYPE_FILE:
 			hdr->data[offset++] = kDirentFile;
+			snprintf(buf, sizeof(buf), "%s/%s", dirPath, entry.d_name);
+			struct stat st;
+			if (stat(buf, &st) == 0) {
+				fileSize = st.st_size;
+			}
 			break;
 		case DTYPE_DIRECTORY:
 			hdr->data[offset++] = kDirentDir;
@@ -259,11 +265,24 @@ MavlinkFTP::_workList(Request *req)
 			hdr->data[offset++] = kDirentUnknown;
 			break;
 		}
+		
+		if (entry.d_type == DTYPE_FILE) {
+			snprintf(buf, sizeof(buf), "%s\t%d", entry.d_name, fileSize);
+		} else {
+			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) {
+			break;
+		}
+		
 		// copy the name, which we know will fit
-		strcpy((char *)&hdr->data[offset], entry.d_name);
+		strcpy((char *)&hdr->data[offset], buf);
 		//printf("FTP: list %s %s\n", dirPath, (char *)&hdr->data[offset-1]);
-		offset += strlen(entry.d_name) + 1;
+		offset += nameLen + 1;
 	}
 
 	closedir(dp);
@@ -282,6 +301,16 @@ MavlinkFTP::_workOpen(Request *req, bool create)
 		return kErrNoSession;
 	}
 
+	
+	uint32_t fileSize = 0;
+	if (!create) {
+		struct stat st;
+		if (stat(req->dataAsCString(), &st) != 0) {
+			return kErrNotFile;
+		}
+		fileSize = st.st_size;
+	}
+
 	int oflag = create ? (O_CREAT | O_EXCL | O_APPEND) : O_RDONLY;
     
 	int fd = ::open(req->dataAsCString(), oflag);
@@ -291,7 +320,12 @@ MavlinkFTP::_workOpen(Request *req, bool create)
 	_session_fds[session_index] = fd;
 
 	hdr->session = session_index;
-	hdr->size = 0;
+	if (create) {
+		hdr->size = 0;
+	} else {
+		hdr->size = sizeof(uint32_t);
+		*((uint32_t*)hdr->data) = fileSize;
+	}
 
 	return kErrNone;
 }

src/modules/mavlink/mavlink_ftp.h

@@ -146,7 +146,7 @@ private:
 			mavlink_message_t msg;
 			msg.checksum = 0;
 			unsigned len = mavlink_msg_encapsulated_data_pack_chan(_mavlink->get_system_id(), _mavlink->get_component_id(),
-				_mavlink->get_channel(), &msg, sequence(), rawData());
+				_mavlink->get_channel(), &msg, sequence()+1, rawData());
 
 			_mavlink->lockMessageBufferMutex();
 			bool success = _mavlink->message_buffer_write(&msg, len);

src/modules/mavlink/mavlink_messages.cpp

@@ -1385,43 +1385,43 @@ protected:
 };
 
 
-class MavlinkStreamGlobalPositionSetpointInt : public MavlinkStream
+class MavlinkStreamPositionTargetGlobalInt : public MavlinkStream
 {
 public:
 	const char *get_name() const
 	{
-		return MavlinkStreamGlobalPositionSetpointInt::get_name_static();
+		return MavlinkStreamPositionTargetGlobalInt::get_name_static();
 	}
 
 	static const char *get_name_static()
 	{
-		return "GLOBAL_POSITION_SETPOINT_INT";
+		return "POSITION_TARGET_GLOBAL_INT";
 	}
 
 	uint8_t get_id()
 	{
-		return MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_INT;
+		return MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT;
 	}
 
 	static MavlinkStream *new_instance(Mavlink *mavlink)
 	{
-		return new MavlinkStreamGlobalPositionSetpointInt(mavlink);
+		return new MavlinkStreamPositionTargetGlobalInt(mavlink);
 	}
 
 	unsigned get_size()
 	{
-		return MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_INT_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
+		return MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
 	}
 
 private:
 	MavlinkOrbSubscription *_pos_sp_triplet_sub;
 
 	/* do not allow top copying this class */
-	MavlinkStreamGlobalPositionSetpointInt(MavlinkStreamGlobalPositionSetpointInt &);
+	MavlinkStreamPositionTargetGlobalInt(MavlinkStreamPositionTargetGlobalInt &);
-	MavlinkStreamGlobalPositionSetpointInt& operator = (const MavlinkStreamGlobalPositionSetpointInt &);
+	MavlinkStreamPositionTargetGlobalInt& operator = (const MavlinkStreamPositionTargetGlobalInt &);
 
 protected:
-	explicit MavlinkStreamGlobalPositionSetpointInt(Mavlink *mavlink) : MavlinkStream(mavlink),
+	explicit MavlinkStreamPositionTargetGlobalInt(Mavlink *mavlink) : MavlinkStream(mavlink),
 		_pos_sp_triplet_sub(_mavlink->add_orb_subscription(ORB_ID(position_setpoint_triplet)))
 	{}
 
@@ -1430,15 +1430,14 @@ protected:
 		struct position_setpoint_triplet_s pos_sp_triplet;
 
 		if (_pos_sp_triplet_sub->update(&pos_sp_triplet)) {
-			mavlink_global_position_setpoint_int_t msg;
+			mavlink_position_target_global_int_t msg{};
 
 			msg.coordinate_frame = MAV_FRAME_GLOBAL;
-			msg.latitude = pos_sp_triplet.current.lat * 1e7;
+			msg.lat_int = pos_sp_triplet.current.lat * 1e7;
-			msg.longitude = pos_sp_triplet.current.lon * 1e7;
+			msg.lon_int = pos_sp_triplet.current.lon * 1e7;
-			msg.altitude = pos_sp_triplet.current.alt * 1000;
+			msg.alt = pos_sp_triplet.current.alt;
-			msg.yaw = pos_sp_triplet.current.yaw * M_RAD_TO_DEG_F * 100.0f;
 
-			_mavlink->send_message(MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_INT, &msg);
+			_mavlink->send_message(MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT, &msg);
 		}
 	}
 };
@@ -1454,12 +1453,12 @@ public:
 
 	static const char *get_name_static()
 	{
-		return "LOCAL_POSITION_SETPOINT";
+		return "POSITION_TARGET_LOCAL_NED";
 	}
 
 	uint8_t get_id()
 	{
-		return MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT;
+		return MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED;
 	}
 
 	static MavlinkStream *new_instance(Mavlink *mavlink)
@@ -1469,7 +1468,7 @@ public:
 
 	unsigned get_size()
 	{
-		return MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
+		return MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
 	}
 
 private:
@@ -1491,137 +1490,87 @@ protected:
 		struct vehicle_local_position_setpoint_s pos_sp;
 
 		if (_pos_sp_sub->update(&_pos_sp_time, &pos_sp)) {
-			mavlink_local_position_setpoint_t msg;
+			mavlink_position_target_local_ned_t msg{};
 
+			msg.time_boot_ms = pos_sp.timestamp / 1000;
 			msg.coordinate_frame = MAV_FRAME_LOCAL_NED;
 			msg.x = pos_sp.x;
 			msg.y = pos_sp.y;
 			msg.z = pos_sp.z;
-			msg.yaw = pos_sp.yaw;
 
-			_mavlink->send_message(MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT, &msg);
+			_mavlink->send_message(MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED, &msg);
 		}
 	}
 };
 
 
-class MavlinkStreamRollPitchYawThrustSetpoint : public MavlinkStream
+class MavlinkStreamAttitudeTarget : public MavlinkStream
 {
 public:
 	const char *get_name() const
 	{
-		return MavlinkStreamRollPitchYawThrustSetpoint::get_name_static();
+		return MavlinkStreamAttitudeTarget::get_name_static();
 	}
 
 	static const char *get_name_static()
 	{
-		return "ROLL_PITCH_YAW_THRUST_SETPOINT";
+		return "ATTITUDE_TARGET";
 	}
 
 	uint8_t get_id()
 	{
-		return MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT;
+		return MAVLINK_MSG_ID_ATTITUDE_TARGET;
 	}
 
 	static MavlinkStream *new_instance(Mavlink *mavlink)
 	{
-		return new MavlinkStreamRollPitchYawThrustSetpoint(mavlink);
+		return new MavlinkStreamAttitudeTarget(mavlink);
 	}
 
 	unsigned get_size()
 	{
-		return MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
+		return MAVLINK_MSG_ID_ATTITUDE_TARGET_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
 	}
 
 private:
-	MavlinkOrbSubscription *att_sp_sub;
+	MavlinkOrbSubscription *_att_sp_sub;
-	uint64_t att_sp_time;
+	MavlinkOrbSubscription *_att_rates_sp_sub;
+	uint64_t _att_sp_time;
+	uint64_t _att_rates_sp_time;
 
 	/* do not allow top copying this class */
-	MavlinkStreamRollPitchYawThrustSetpoint(MavlinkStreamRollPitchYawThrustSetpoint &);
+	MavlinkStreamAttitudeTarget(MavlinkStreamAttitudeTarget &);
-	MavlinkStreamRollPitchYawThrustSetpoint& operator = (const MavlinkStreamRollPitchYawThrustSetpoint &);
+	MavlinkStreamAttitudeTarget& operator = (const MavlinkStreamAttitudeTarget &);
 
 protected:
-	explicit MavlinkStreamRollPitchYawThrustSetpoint(Mavlink *mavlink) : MavlinkStream(mavlink),
+	explicit MavlinkStreamAttitudeTarget(Mavlink *mavlink) : MavlinkStream(mavlink),
-		att_sp_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_attitude_setpoint))),
+		_att_sp_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_attitude_setpoint))),
-		att_sp_time(0)
+		_att_rates_sp_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_rates_setpoint))),
+		_att_sp_time(0),
+		_att_rates_sp_time(0)
 	{}
 
 	void send(const hrt_abstime t)
 	{
 		struct vehicle_attitude_setpoint_s att_sp;
 
-		if (att_sp_sub->update(&att_sp_time, &att_sp)) {
+		if (_att_sp_sub->update(&_att_sp_time, &att_sp)) {
-			mavlink_roll_pitch_yaw_thrust_setpoint_t msg;
-
-			msg.time_boot_ms = att_sp.timestamp / 1000;
-			msg.roll = att_sp.roll_body;
-			msg.pitch = att_sp.pitch_body;
-			msg.yaw = att_sp.yaw_body;
-			msg.thrust = att_sp.thrust;
-
-			_mavlink->send_message(MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT, &msg);
-		}
-	}
-};
-
 
-class MavlinkStreamRollPitchYawRatesThrustSetpoint : public MavlinkStream
+			struct vehicle_rates_setpoint_s att_rates_sp;
-{
+			(void)_att_rates_sp_sub->update(&_att_rates_sp_time, &att_rates_sp);
-public:
-	const char *get_name() const
-	{
-		return MavlinkStreamRollPitchYawRatesThrustSetpoint::get_name_static();
-	}
-
-	static const char *get_name_static()
-	{
-		return "ROLL_PITCH_YAW_RATES_THRUST_SETPOINT";
-	}
 
-	uint8_t get_id()
+			mavlink_attitude_target_t msg{};
-	{
-		return MAVLINK_MSG_ID_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT;
-	}
 
-	static MavlinkStream *new_instance(Mavlink *mavlink)
+			msg.time_boot_ms = att_sp.timestamp / 1000;
-	{
+			mavlink_euler_to_quaternion(att_sp.roll_body, att_sp.pitch_body, att_sp.yaw_body, msg.q);
-		return new MavlinkStreamRollPitchYawRatesThrustSetpoint(mavlink);
-	}
-
-	unsigned get_size()
-	{
-		return MAVLINK_MSG_ID_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
-	}
-
-private:
-	MavlinkOrbSubscription *_att_rates_sp_sub;
-	uint64_t _att_rates_sp_time;
-
-	/* do not allow top copying this class */
-	MavlinkStreamRollPitchYawRatesThrustSetpoint(MavlinkStreamRollPitchYawRatesThrustSetpoint &);
-	MavlinkStreamRollPitchYawRatesThrustSetpoint& operator = (const MavlinkStreamRollPitchYawRatesThrustSetpoint &);
-
-protected:
-	explicit MavlinkStreamRollPitchYawRatesThrustSetpoint(Mavlink *mavlink) : MavlinkStream(mavlink),
-		_att_rates_sp_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_rates_setpoint))),
-		_att_rates_sp_time(0)
-	{}
-
-	void send(const hrt_abstime t)
-	{
-		struct vehicle_rates_setpoint_s att_rates_sp;
 
-		if (_att_rates_sp_sub->update(&_att_rates_sp_time, &att_rates_sp)) {
+			msg.body_roll_rate = att_rates_sp.roll;
-			mavlink_roll_pitch_yaw_rates_thrust_setpoint_t msg;
+			msg.body_pitch_rate = att_rates_sp.pitch;
+			msg.body_yaw_rate = att_rates_sp.yaw;
 
-			msg.time_boot_ms = att_rates_sp.timestamp / 1000;
+			msg.thrust = att_sp.thrust;
-			msg.roll_rate = att_rates_sp.roll;
-			msg.pitch_rate = att_rates_sp.pitch;
-			msg.yaw_rate = att_rates_sp.yaw;
-			msg.thrust = att_rates_sp.thrust;
 
-			_mavlink->send_message(MAVLINK_MSG_ID_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT, &msg);
+			_mavlink->send_message(MAVLINK_MSG_ID_ATTITUDE_TARGET, &msg);
 		}
 	}
 };
@@ -2149,10 +2098,9 @@ StreamListItem *streams_list[] = {
 	new StreamListItem(&MavlinkStreamServoOutputRaw<2>::new_instance, &MavlinkStreamServoOutputRaw<2>::get_name_static),
 	new StreamListItem(&MavlinkStreamServoOutputRaw<3>::new_instance, &MavlinkStreamServoOutputRaw<3>::get_name_static),
 	new StreamListItem(&MavlinkStreamHILControls::new_instance, &MavlinkStreamHILControls::get_name_static),
-	new StreamListItem(&MavlinkStreamGlobalPositionSetpointInt::new_instance, &MavlinkStreamGlobalPositionSetpointInt::get_name_static),
+	new StreamListItem(&MavlinkStreamPositionTargetGlobalInt::new_instance, &MavlinkStreamPositionTargetGlobalInt::get_name_static),
 	new StreamListItem(&MavlinkStreamLocalPositionSetpoint::new_instance, &MavlinkStreamLocalPositionSetpoint::get_name_static),
-	new StreamListItem(&MavlinkStreamRollPitchYawThrustSetpoint::new_instance, &MavlinkStreamRollPitchYawThrustSetpoint::get_name_static),
+	new StreamListItem(&MavlinkStreamAttitudeTarget::new_instance, &MavlinkStreamAttitudeTarget::get_name_static),
-	new StreamListItem(&MavlinkStreamRollPitchYawRatesThrustSetpoint::new_instance, &MavlinkStreamRollPitchYawRatesThrustSetpoint::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),

src/modules/mavlink/mavlink_receiver.cpp

@@ -148,10 +148,6 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
 		handle_message_vicon_position_estimate(msg);
 		break;
 
-	case MAVLINK_MSG_ID_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST:
-		handle_message_quad_swarm_roll_pitch_yaw_thrust(msg);
-		break;
-
 	case MAVLINK_MSG_ID_RADIO_STATUS:
 		handle_message_radio_status(msg);
 		break;
@@ -362,36 +358,6 @@ MavlinkReceiver::handle_message_vicon_position_estimate(mavlink_message_t *msg)
 	}
 }
 
-void
-MavlinkReceiver::handle_message_quad_swarm_roll_pitch_yaw_thrust(mavlink_message_t *msg)
-{
-	mavlink_set_quad_swarm_roll_pitch_yaw_thrust_t swarm_offboard_control;
-	mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust_decode(msg, &swarm_offboard_control);
-
-	/* Only accept system IDs from 1 to 4 */
-	if (mavlink_system.sysid >= 1 && mavlink_system.sysid <= 4) {
-		struct offboard_control_setpoint_s offboard_control_sp;
-		memset(&offboard_control_sp, 0, sizeof(offboard_control_sp));
-
-		/* Convert values * 1000 back */
-		offboard_control_sp.p1 = (float)swarm_offboard_control.roll[mavlink_system.sysid - 1] / 1000.0f;
-		offboard_control_sp.p2 = (float)swarm_offboard_control.pitch[mavlink_system.sysid - 1] / 1000.0f;
-		offboard_control_sp.p3 = (float)swarm_offboard_control.yaw[mavlink_system.sysid - 1] / 1000.0f;
-		offboard_control_sp.p4 = (float)swarm_offboard_control.thrust[mavlink_system.sysid - 1] / 1000.0f;
-
-		offboard_control_sp.mode = (enum OFFBOARD_CONTROL_MODE)swarm_offboard_control.mode;
-
-		offboard_control_sp.timestamp = hrt_absolute_time();
-
-		if (_offboard_control_sp_pub < 0) {
-			_offboard_control_sp_pub = orb_advertise(ORB_ID(offboard_control_setpoint), &offboard_control_sp);
-
-		} else {
-			orb_publish(ORB_ID(offboard_control_setpoint), _offboard_control_sp_pub, &offboard_control_sp);
-		}
-	}
-}
-
 void
 MavlinkReceiver::handle_message_radio_status(mavlink_message_t *msg)
 {

src/modules/mc_pos_control/mc_pos_control_main.cpp

@@ -788,6 +788,7 @@ MulticopterPositionControl::task_main()
 			}
 
 			/* fill local position setpoint */
+			_local_pos_sp.timestamp = hrt_absolute_time();
 			_local_pos_sp.x = _pos_sp(0);
 			_local_pos_sp.y = _pos_sp(1);
 			_local_pos_sp.z = _pos_sp(2);

src/modules/uORB/topics/vehicle_local_position_setpoint.h

@@ -50,11 +50,12 @@
  */
 
 struct vehicle_local_position_setpoint_s {
+	uint64_t timestamp;	/**< timestamp of the setpoint */
 	float x;		/**< in meters NED			  		*/
 	float y;		/**< in meters NED			  		*/
 	float z;		/**< in meters NED			  		*/
 	float yaw;		/**< in radians NED -PI..+PI  		*/
-}; /**< Local position in NED frame to go to */
+}; /**< Local position in NED frame */
 
 /**
  * @}