plugin: ftp: Implemnet reset call.

Sometimes kCmdReset can restore normal operation, but it might be dangerous. Issue #128.
mavlink · Sep 2, 2014 · fcd26f9 · fcd26f9
1 parent 5d3b775
commit fcd26f9
Show file tree

Hide file tree

Showing 3 changed files with 27 additions and 3 deletions.
diff --git a/mavros/CMakeLists.txt b/mavros/CMakeLists.txt
@@ -13,6 +13,7 @@ find_package(catkin REQUIRED COMPONENTS
   roscpp
   sensor_msgs
   std_msgs
+  std_srvs
   tf
   angles
 )

diff --git a/mavros/package.xml b/mavros/package.xml
@@ -29,6 +29,7 @@
   <build_depend>geometry_msgs</build_depend>
   <build_depend>sensor_msgs</build_depend>
   <build_depend>std_msgs</build_depend>
+  <build_depend>std_srvs</build_depend>
   <build_depend>tf</build_depend>
   <build_depend>mavlink</build_depend>
   <build_depend>angles</build_depend>
@@ -42,6 +43,7 @@
   <run_depend>geometry_msgs</run_depend>
   <run_depend>sensor_msgs</run_depend>
   <run_depend>std_msgs</run_depend>
+  <run_depend>std_srvs</run_depend>
   <run_depend>tf</run_depend>
   <run_depend>mavlink</run_depend>
   <run_depend>angles</run_depend>

diff --git a/mavros/src/plugins/ftp.cpp b/mavros/src/plugins/ftp.cpp
@@ -29,6 +29,7 @@
 #include <mavros/mavros_plugin.h>
 #include <pluginlib/class_list_macros.h>
 
+#include <std_srvs/Empty.h>
 #include <mavros/FileEntry.h>
 #include <mavros/FileList.h>
 #include <mavros/FileOpen.h>
@@ -98,6 +99,11 @@ uint32_t crc32part(const uint8_t *src, size_t len, uint32_t crc32val)
 // XXX: end
 
 
+/**
+ * @brief FTP Request message abstraction class
+ *
+ * @note This class not portable, and works on little-endian machines only.
+ */
 class FTPRequest {
 public:
 	/// @brief This is the payload which is in mavlink_file_transfer_protocol_t.payload.
@@ -277,6 +283,7 @@ class FTPPlugin : public MavRosPlugin {
 		open_srv = ftp_nh.advertiseService("open", &FTPPlugin::open_cb, this);
 		close_srv = ftp_nh.advertiseService("close", &FTPPlugin::close_cb, this);
 		read_srv = ftp_nh.advertiseService("read", &FTPPlugin::read_cb, this);
+		reset_srv = ftp_nh.advertiseService("reset", &FTPPlugin::reset_cb, this);
 	}
 
 	std::string const get_name() const {
@@ -296,6 +303,7 @@ class FTPPlugin : public MavRosPlugin {
 	ros::ServiceServer open_srv;
 	ros::ServiceServer close_srv;
 	ros::ServiceServer read_srv;
+	ros::ServiceServer reset_srv;
 
 	enum OpState {
 		OP_IDLE,
@@ -339,6 +347,9 @@ class FTPPlugin : public MavRosPlugin {
 	//! Maximum difference between allocated space and used
 	static constexpr size_t MAX_RESERVE_DIFF = 0x10000;
 
+	//! @todo timeout timer
+	//! @todo write support
+
 	/* -*- message handler -*- */
 
 	void handle_file_transfer_protocol(const mavlink_message_t *msg, uint8_t sysid, uint8_t compid) {
@@ -472,6 +483,7 @@ class FTPPlugin : public MavRosPlugin {
 		}
 		else if (hdr->size == 0) {
 			// kCmdCreate ACK
+			open_size = 0;
 		}
 
 		ROS_DEBUG_NAMED("ftp", "FTP:Open %s: success, session %u, size %zu",
@@ -510,9 +522,8 @@ class FTPPlugin : public MavRosPlugin {
 			read_offset += bytes_to_copy;
 			send_read_command();
 		}
-		else {
+		else
 			read_file_end();
-		}
 	}
 
 	/* -*- send helpers -*- */
@@ -573,7 +584,7 @@ class FTPPlugin : public MavRosPlugin {
 		ROS_DEBUG_STREAM_NAMED("ftp", "FTP:m: kCmdRead: " << active_session << " off: " << read_offset);
 		FTPRequest req(FTPRequest::kCmdRead, active_session);
 		req.header()->offset = read_offset;
-		req.header()->size = FTPRequest::DATA_MAXSZ;
+		req.header()->size = 0 /* FTPRequest::DATA_MAXSZ */;
 		req.send(uas, last_send_seqnr);
 	}
 
@@ -771,6 +782,16 @@ class FTPPlugin : public MavRosPlugin {
 
 		return true;
 	}
+
+	/**
+	 * @brief Reset communication on both sides.
+	 * @note This call break other calls, so use carefully.
+	 */
+	bool reset_cb(std_srvs::Empty::Request &req,
+			std_srvs::Empty::Response &res) {
+		send_reset();
+		return true;
+	}
 };
 
 }; // namespace mavplugin