Merge branch 'master' of github.com:PX4/Firmware into cmake-2

launch/ardrone.launch

@@ -1,8 +1,10 @@
 <launch>
 <arg name="ns"/>
+<arg name="mavlink_fcu_url" default="udp://localhost:14565@localhost:14560"/>
 
 <include file="$(find px4)/launch/multicopter_x.launch">
 	<arg name="ns" value="$(arg ns)"/>
+	<arg name="mavlink_fcu_url" value="$(arg mavlink_fcu_url)" />
 </include>
 
 <group ns="$(arg ns)">

launch/iris.launch

@@ -1,8 +1,10 @@
 <launch>
 <arg name="ns"/>
+<arg name="mavlink_fcu_url" default="udp://localhost:14565@localhost:14560"/>
 
 <include file="$(find px4)/launch/multicopter_w.launch">
 	<arg name="ns" value="$(arg ns)"/>
+	<arg name="mavlink_fcu_url" value="$(arg mavlink_fcu_url)" />
 </include>
 
 <group ns="$(arg ns)">

launch/multi_uav.launch

@@ -0,0 +1,55 @@
+<?xml version="1.0"?>
+<launch>
+  <arg name="headless" default="false"/>
+  <arg name="gui" default="true"/>
+  <arg name="enable_logging" default="false"/>
+  <arg name="enable_ground_truth" default="true"/>
+  <arg name="debug" default="false"/>
+  <arg name="ns" default="iris"/>
+  <arg name="log_file" default="$(arg ns)"/>
+
+  <include file="$(find gazebo_ros)/launch/empty_world.launch">
+    <arg name="world_name" value="$(find rotors_gazebo)/worlds/basic.world"/>
+    <arg name="debug" value="$(arg debug)" />
+    <arg name="headless" value="$(arg headless)"/>
+    <arg name="gui" value="$(arg gui)"/>
+  </include>
+  
+   <group ns="iris">
+      <include file="$(find rotors_gazebo)/launch/spawn_iris.launch">
+        <arg name="model" value="$(find rotors_description)/urdf/iris_gesture_sensors.gazebo" />
+        <arg name="enable_logging" value="$(arg enable_logging)" />
+        <arg name="enable_ground_truth" value="$(arg enable_ground_truth)" />
+        <arg name="log_file" value="$(arg log_file)"/>
+        <arg name="x" value="1"/>
+        <arg name="y" value="0"/>
+      </include>
+      <arg name="fcu_url" default="udp://localhost:14560@localhost:14565" />
+      <include file="$(find mavros)/launch/px4.launch">
+        <arg name="fcu_url" value="$(arg fcu_url)" />
+      </include>
+   </group>
+   <include file="$(find px4)/launch/iris.launch">
+     <arg name="ns" value="iris"/>
+   </include>   
+      
+   <group ns="ardrone">
+      <include file="$(find rotors_gazebo)/launch/spawn_ardrone.launch">
+        <arg name="model" value="$(find rotors_description)/urdf/ardrone_base.xacro" />
+        <arg name="enable_logging" value="$(arg enable_logging)" />
+        <arg name="enable_ground_truth" value="$(arg enable_ground_truth)" />
+        <arg name="log_file" value="$(arg log_file)"/>
+        <arg name="x" value="-1"/>
+        <arg name="y" value="0"/>
+      </include>
+      <arg name="fcu_url" default="udp://localhost:14570@localhost:14575" />
+      <include file="$(find mavros)/launch/px4.launch">
+        <arg name="fcu_url" value="$(arg fcu_url)" />
+      </include>
+   </group>
+   <include file="$(find px4)/launch/ardrone.launch">
+	<arg name="ns" value="ardrone"/>
+	<arg name="mavlink_fcu_url" value="udp://localhost:14575@localhost:14570"/>
+   </include>
+
+</launch>   

launch/multicopter.launch

@@ -1,5 +1,6 @@
 <launch>
 <arg name="ns"/>
+<arg name="mavlink_fcu_url"/>
 
 <group ns="$(arg ns)">
 	<node pkg="joy" name="joy_node" type="joy_node"/>
@@ -10,7 +11,9 @@
 	<node pkg="px4" name="position_estimator" type="position_estimator"/>
 	<node pkg="px4" name="mc_att_control" type="mc_att_control"/>
 	<node pkg="px4" name="mc_pos_control" type="mc_pos_control"/>
-	<node pkg="px4" name="mavlink" type="mavlink"/>
+	<node pkg="px4" name="mavlink" type="mavlink">  
+	  <param name="mavlink_fcu_url" value="$(arg mavlink_fcu_url)" type="str"/>
+	</node>
 	<!-- <node pkg="rosbag" type="record" name="record" output="screen" args="-a -O px4_multicopter"/> -->
 </group>
 

launch/multicopter_w.launch

@@ -1,8 +1,10 @@
 <launch>
 <arg name="ns"/>
+<arg name="mavlink_fcu_url"/>
 
 <include file="$(find px4)/launch/multicopter.launch">
 	<arg name="ns" value="$(arg ns)"/>
+	<arg name="mavlink_fcu_url" value="$(arg mavlink_fcu_url)" />
 </include>
 
 <group ns="$(arg ns)">

launch/multicopter_x.launch

@@ -1,8 +1,10 @@
 <launch>
 <arg name="ns"/>
+<arg name="mavlink_fcu_url"/>
 
 <include file="$(find px4)/launch/multicopter.launch">
 	<arg name="ns" value="$(arg ns)"/>
+	<arg name="mavlink_fcu_url" value="$(arg mavlink_fcu_url)" />
 </include>
 
 <group ns="$(arg ns)">

makefiles/posix/config_posix_sitl.mk

@@ -97,7 +97,7 @@ MODULES 	+= platforms/posix/drivers/gpssim
 # Unit tests
 #
 #MODULES	+= platforms/posix/tests/hello
-#MODULES	+= platforms/posix/tests/vcdev_test
+MODULES	+= platforms/posix/tests/vcdev_test
 #MODULES	+= platforms/posix/tests/hrt_test
 #MODULES	+= platforms/posix/tests/wqueue
 

src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp

@@ -1227,9 +1227,6 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 
 				_l1_control.navigate_heading(target_bearing, _att.yaw, ground_speed_2d);
 
-				/* limit roll motion to prevent wings from touching the ground first */
-				_att_sp.roll_body = math::constrain(_att_sp.roll_body, math::radians(-10.0f), math::radians(10.0f));
-
 				land_noreturn_horizontal = true;
 
 			} else {
@@ -1241,6 +1238,10 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 			_att_sp.roll_body = _l1_control.nav_roll();
 			_att_sp.yaw_body = _l1_control.nav_bearing();
 
+			if (land_noreturn_horizontal) {
+				/* limit roll motion to prevent wings from touching the ground first */
+				_att_sp.roll_body = math::constrain(_att_sp.roll_body, math::radians(-10.0f), math::radians(10.0f));
+			}
 
 			/* Vertical landing control */
 			//xxx: using the tecs altitude controller for slope control for now

src/modules/mavlink/mavlink_mission.h

@@ -133,7 +133,7 @@ private:
 
 	bool _verbose;
 
-	static constexpr int	FILESYSTEM_ERRCOUNT_NOTIFY_LIMIT = 2;	///< Error count limit before stopping to report FS errors
+	static constexpr unsigned int	FILESYSTEM_ERRCOUNT_NOTIFY_LIMIT = 2;	///< Error count limit before stopping to report FS errors
 
 	/* do not allow top copying this class */
 	MavlinkMissionManager(MavlinkMissionManager &);

src/platforms/posix/tests/vcdev_test/vcdevtest_example.cpp

@@ -51,54 +51,128 @@ px4::AppState VCDevExample::appState;
 
 using namespace device;
 
-#define TESTDEV "/dev/vdevex"
+#define TESTDEV "/dev/vdevtest"
+
+static bool g_exit = false;
 
 static int writer_main(int argc, char *argv[])
 {
-	char buf[1] = { '1' };
+	char buf[1];
 
-	int fd = px4_open(TESTDEV, PX4_F_RDONLY);
+	int fd = px4_open(TESTDEV, PX4_F_WRONLY);
 	if (fd < 0) {
-		PX4_INFO("--- Open failed %d %d", fd, px4_errno);
+		PX4_INFO("Writer: Open failed %d %d", fd, px4_errno);
 		return -px4_errno;
 	}
 
 	int ret;
 	int i=0;
-	while (i<3) {
-		// Wait for 3 seconds
-		PX4_INFO("--- Sleeping for 4 sec\n");
-		ret = sleep(4);
+	while (!g_exit) {
+		// Wait for 2 seconds
+		PX4_INFO("Writer: Sleeping for 2 sec");
+		ret = sleep(2);
 		if (ret < 0) {
-			PX4_INFO("--- sleep failed %d %d\n", ret, errno);
+			PX4_INFO("Writer: sleep failed %d %d", ret, errno);
 			return ret;
 		}
 
-		PX4_INFO("--- writing to fd\n");
+		buf[0] = 'A'+(char)(i % 26);
+		PX4_INFO("Writer: writing char '%c'", buf[0]);
 		ret = px4_write(fd, buf, 1);
 	++i;
 	}
 	px4_close(fd);
+	PX4_INFO("Writer: stopped");
 	return ret;
 }
 
+class PrivData {
+public:
+	PrivData() : _read_offset(0) {}
+	~PrivData() {}
+	
+	size_t _read_offset;
+};
+	
 class VCDevNode : public VDev {
 public:
-	VCDevNode() : VDev("vcdevtest", TESTDEV) {};
+	VCDevNode() : 
+		VDev("vcdevtest", TESTDEV), 
+		_is_open_for_write(false),
+		_write_offset(0) {};
 
 	~VCDevNode() {}
 
+	virtual int open(device::file_t *handlep);
+	virtual int close(device::file_t *handlep);
 	virtual ssize_t write(device::file_t *handlep, const char *buffer, size_t buflen);
+	virtual ssize_t read(device::file_t *handlep, char *buffer, size_t buflen);
+private:
+	bool _is_open_for_write;
+	size_t _write_offset;
+	char     _buf[1000];
 };
 
+int VCDevNode::open(device::file_t *handlep)
+{
+	// Only allow one writer
+	if (_is_open_for_write && (handlep->flags & PX4_F_WRONLY)) {
+		errno = EBUSY;
+		return -1;
+	}
+	int ret = VDev::open(handlep);
+	if (ret != 0) {
+		return ret;
+	}
+	handlep->priv = new PrivData;
+
+	if (_is_open_for_write && (handlep->flags & PX4_F_WRONLY)) {
+		_is_open_for_write = true;
+	}
+	
+	return 0;
+}
+
+int VCDevNode::close(device::file_t *handlep)
+{
+	delete (PrivData *)handlep->priv;
+	handlep->priv = nullptr;
+	VDev::close(handlep);
+
+	// Enable a new writer of the device is re-opened for write
+	if ((handlep->flags & PX4_F_WRONLY) && _is_open_for_write) {
+		_is_open_for_write = false;
+	}
+	return 0;
+}
+
 ssize_t VCDevNode::write(device::file_t *handlep, const char *buffer, size_t buflen)
 {
+	for (size_t i=0; i<buflen && _write_offset<1000; i++) {
+		_buf[_write_offset] = buffer[i];
+		_write_offset++;
+	}
+
 	// ignore what was written, but let pollers know something was written
 	poll_notify(POLLIN);
 
 	return buflen;
 }
 
+ssize_t VCDevNode::read(device::file_t *handlep, char *buffer, size_t buflen)
+{
+	PrivData *p = (PrivData *)handlep->priv;
+	ssize_t chars_read = 0;
+	PX4_INFO("read %zu write %zu", p->_read_offset, _write_offset);
+	for (size_t i=0; i<buflen && (p->_read_offset < _write_offset); i++) {
+		buffer[i] = _buf[p->_read_offset];
+		p->_read_offset++;
+		chars_read++;
+	}
+	
+	return chars_read;
+}
+
 VCDevExample::~VCDevExample() {
 	if (_node) {
 		delete _node;
@@ -119,11 +193,57 @@ static int test_pub_block(int fd, unsigned long blocked)
 		PX4_INFO("ioctl PX4_DEVIOCGPUBBLOCK failed %d %d", ret, px4_errno);
 		return -px4_errno;
 	}
-	PX4_INFO("pub_blocked = %d %s\n", ret, (unsigned long)ret == blocked ? "PASS" : "FAIL");
+	PX4_INFO("pub_blocked = %d %s", ret, (unsigned long)ret == blocked ? "PASS" : "FAIL");
 
 	return 0;
 }
 
+int VCDevExample::do_poll(int fd, int timeout, int iterations, int delayms_after_poll)
+{
+	int pollret, readret;
+	int loop_count = 0;
+	char readbuf[10];
+	px4_pollfd_struct_t fds[1];
+
+	fds[0].fd = fd;
+	fds[0].events = POLLIN;
+	fds[0].revents = 0;
+
+	bool mustblock = (timeout < 0);
+
+	// Test indefinte blocking poll
+	while ((!appState.exitRequested()) && (loop_count < iterations)) {
+		pollret = px4_poll(fds, 1, timeout);
+		if (pollret < 0) {
+			PX4_ERR("Reader: px4_poll failed %d %d FAIL", pollret, px4_errno);
+			goto fail;
+		} 
+		PX4_INFO("Reader: px4_poll returned %d", pollret);
+		if (pollret) {
+			readret = px4_read(fd, readbuf, 10);
+			if (readret != 1) {
+				if (mustblock) {
+					PX4_ERR("Reader:     read failed %d FAIL", readret);
+					goto fail;
+				}
+				else {
+					PX4_INFO("Reader:     read failed %d FAIL", readret);
+				}
+			}
+			else {
+				readbuf[readret] = '\0';
+				PX4_INFO("Reader: px4_poll     returned %d, read '%s' PASS", pollret, readbuf);
+			}
+		}
+		if (delayms_after_poll) {
+			usleep(delayms_after_poll*1000);
+		}
+		loop_count++;
+	}
+	return 0;
+fail:
+	return 1;
+}
 int VCDevExample::main()
 {
 	appState.setRunning(true);
@@ -131,12 +251,12 @@ int VCDevExample::main()
 	_node = new VCDevNode();
 
 	if (_node == 0) {
-		PX4_INFO("Failed to allocate VCDevNode\n");
+		PX4_INFO("Failed to allocate VCDevNode");
 		return -ENOMEM;
 	}
 
 	if (_node->init() != PX4_OK) {
-		PX4_INFO("Failed to init VCDevNode\n");
+		PX4_INFO("Failed to init VCDevNode");
 		return 1;
 	}
 
@@ -153,7 +273,7 @@ int VCDevExample::main()
 		PX4_INFO("ioctl DIOC_GETPRIV failed %d %d", ret, px4_errno);
 		return -px4_errno;
 	}
-	PX4_INFO("priv data = %p %s\n", p, p == (void *)_node ? "PASS" : "FAIL");
+	PX4_INFO("priv data = %p %s", p, p == (void *)_node ? "PASS" : "FAIL");
 
 	ret = test_pub_block(fd, 1);
 	if (ret < 0)
@@ -162,10 +282,8 @@ int VCDevExample::main()
 	if (ret < 0)
 		return ret;
 
-	int i=0;
-	px4_pollfd_struct_t fds[1];
 
-	// Start a task that will write something in 3 seconds
+	// Start a task that will write something in 4 seconds
 	(void)px4_task_spawn_cmd("writer", 
 				       SCHED_DEFAULT,
 				       SCHED_PRIORITY_MAX - 6,
@@ -173,42 +291,28 @@ int VCDevExample::main()
 				       writer_main,
 				       (char* const*)NULL);
 
-	while (!appState.exitRequested() && i<13) {
-		PX4_INFO("=====================\n");
-		PX4_INFO("====  sleeping 2 sec ====\n");
-		sleep(2);
+	ret = 0;
 
-		fds[0].fd = fd;
-		fds[0].events = POLLIN;
-		fds[0].revents = 0;
-
-		PX4_INFO("==== Calling Poll\n");
-		ret = px4_poll(fds, 1, 1000);
-		PX4_INFO("==== Done poll\n");
-		if (ret < 0) {
-			PX4_INFO("==== poll failed %d %d\n", ret, px4_errno);
-			px4_close(fd);
-		} 
-		else if (i > 0) {
-			if (ret == 0) {
-				PX4_INFO("==== Nothing to read - PASS\n");
-			}
-			else {
-				PX4_INFO("==== poll returned %d\n", ret);
-			}
-		}
-		else if (i == 0) {
-			if (ret == 1) {
-				PX4_INFO("==== %d to read - %s\n", ret, fds[0].revents & POLLIN ? "PASS" : "FAIL");
-			}
-			else {
-				PX4_INFO("==== %d to read - FAIL\n", ret);
-			}
-		
-		}
-		++i;
+	PX4_INFO("TEST: BLOCKING POLL ---------------");
+	if (do_poll(fd, -1, 3, 0)) {
+		ret = 1;
+		goto fail2;
+	}
+	PX4_INFO("TEST: ZERO TIMEOUT POLL -----------");
+	if(do_poll(fd, 0, 3, 0)) {
+		ret = 1;
+		goto fail2;
+	}
+	PX4_INFO("TEST: 1 SEC TIMOUT POLL ------------");
+	if(do_poll(fd, 1000, 3, 0)) {
+		ret = 1;
+		goto fail2;
 	}
-	px4_close(fd);
 
-	return 0;
+fail2:
+	g_exit = true;
+	px4_close(fd);
+	PX4_INFO("TEST: waiting for writer to stop");
+	sleep(3);
+	return ret;
 }

src/platforms/posix/tests/vcdev_test/vcdevtest_example.h

@@ -54,5 +54,7 @@ public:
 	static px4::AppState appState; /* track requests to terminate app */
 
 private:
+	int do_poll(int fd, int timeout, int iterations, int delayms_after_poll);
+
 	VCDevNode *_node;
 };

src/platforms/ros/nodes/mavlink/mavlink.cpp

@@ -46,7 +46,7 @@
 
 using namespace px4;
 
-Mavlink::Mavlink() :
+Mavlink::Mavlink(std::string mavlink_fcu_url) :
 	_n(),
 	_v_att_sub(_n.subscribe("vehicle_attitude", 1, &Mavlink::VehicleAttitudeCallback, this)),
 	_v_local_pos_sub(_n.subscribe("vehicle_local_position", 1, &Mavlink::VehicleLocalPositionCallback, this)),
@@ -55,7 +55,7 @@ Mavlink::Mavlink() :
 	_offboard_control_mode_pub(_n.advertise<offboard_control_mode>("offboard_control_mode", 1)),
 	_force_sp_pub(_n.advertise<vehicle_force_setpoint>("vehicle_force_setpoint", 1))
 {
-	_link = mavconn::MAVConnInterface::open_url("udp://localhost:14565@localhost:14560");
+	_link = mavconn::MAVConnInterface::open_url(mavlink_fcu_url);
 	_link->message_received.connect(boost::bind(&Mavlink::handle_msg, this, _1, _2, _3));
 	_att_sp = {};
 	_offboard_control_mode = {};
@@ -64,7 +64,10 @@ Mavlink::Mavlink() :
 int main(int argc, char **argv)
 {
 	ros::init(argc, argv, "mavlink");
-	Mavlink m;
+	ros::NodeHandle privateNh("~");
+	std::string mavlink_fcu_url;
+	privateNh.param<std::string>("mavlink_fcu_url",      mavlink_fcu_url,   std::string("udp://localhost:14565@localhost:14560"));
+	Mavlink m(mavlink_fcu_url);
 	ros::spin();
 	return 0;
 }

src/platforms/ros/nodes/mavlink/mavlink.h

@@ -55,7 +55,7 @@ namespace px4
 class Mavlink
 {
 public:
-	Mavlink();
+	Mavlink(std::string mavlink_fcu_url);
 
 	~Mavlink() {}