Fix bugs.

MavLinkCom/MavLinkCom.vcxproj.filters

@@ -135,9 +135,6 @@
     <ClInclude Include="include\MavLinkFtpClient.hpp">
       <Filter>include</Filter>
     </ClInclude>
-    <ClInclude Include="src\impl\MavLinkFtpClientImpl.hpp">
-      <Filter>include</Filter>
-    </ClInclude>
     <ClInclude Include="include\MavLinkLog.hpp">
       <Filter>include</Filter>
     </ClInclude>
@@ -213,6 +210,9 @@
     <ClInclude Include="common_utils\json.hpp">
       <Filter>common_utils</Filter>
     </ClInclude>
+    <ClInclude Include="src\impl\MavLinkFtpClientImpl.hpp">
+      <Filter>src\impl</Filter>
+    </ClInclude>
   </ItemGroup>
   <ItemGroup>
     <Filter Include="Mavlink">

MavLinkCom/MavLinkTest/Commands.cpp

@@ -441,21 +441,21 @@ void PlayLogCommand::Execute(std::shared_ptr<MavLinkVehicle> com)
             Utils::copy(quaternion_, mocap.q);
             com->writeMessage(mocap);
             break;
-        }
-        case MavLinkHeartbeat::kMessageId: {
-            MavLinkHeartbeat heartbeat_msg;
-            heartbeat_msg.decode(msg);
-
-            if (heartbeat_msg.base_mode != last_basemode || heartbeat_msg.custom_mode != last_custommode) {
-                last_basemode = heartbeat_msg.base_mode;
-                last_custommode = heartbeat_msg.custom_mode;
-                //TODO: avoid passing hadcoded HIL flag
-                //com->setMode(last_basemode | static_cast<int>(MAV_MODE_FLAG::MAV_MODE_FLAG_HIL_ENABLED), last_custommode);
-            }
-
-            break;
-        }
-        default:
+        }
+        case MavLinkHeartbeat::kMessageId: {
+            MavLinkHeartbeat heartbeat_msg;
+            heartbeat_msg.decode(msg);
+
+            if (heartbeat_msg.base_mode != last_basemode || heartbeat_msg.custom_mode != last_custommode) {
+                last_basemode = heartbeat_msg.base_mode;
+                last_custommode = heartbeat_msg.custom_mode;
+                //TODO: avoid passing hadcoded HIL flag
+                //com->setMode(last_basemode | static_cast<int>(MAV_MODE_FLAG::MAV_MODE_FLAG_HIL_ENABLED), last_custommode);
+            }
+
+            break;
+        }
+        default:
             break;
         }
     }
@@ -2205,9 +2205,6 @@ void FtpCommand::doGet() {
 
 	std::string fsTarget = normalize(target);
 	std::string leaf = FileSystem::getFileName(fsTarget);
-	if (leaf.size() > 0 && leaf[0] == '/' || leaf[0] == '\\'){
-		leaf = leaf.substr(1);
-	}
 	bool wildcards;
 	if (!parse(leaf, wildcards)) {
 		printf("wildcard pattern '%s' is too complex\n", leaf.c_str());

MavLinkCom/common_utils/FileSystem.hpp

@@ -134,6 +134,7 @@ class FileSystem
 		size_t size = fullPath.size();
 		size_t pos = fullPath.find_last_of(kPathSeparator);
 		if (pos != std::string::npos) {
+			pos++;
 			return fullPath.substr(pos, size - pos);
 		}
 		return fullPath;

MavLinkCom/src/impl/MavLinkConnectionImpl.cpp

@@ -271,9 +271,8 @@ void MavLinkConnectionImpl::readPackets()
 
 		int count = safePort->read(buffer, MAXBUFFER);
 		if (count <= 0) {
-			// error, so just try again...
-			std::lock_guard<std::mutex> guard(telemetry_mutex_);
-			telemetry_.crcErrors++;
+			// error? well let's try again, but we should be careful not to spin too fast and kill the CPU
+			std::this_thread::sleep_for(std::chrono::milliseconds(1));
 			continue;
 		}
 		for (int i = 0; i < count; i++)

MavLinkCom/src/impl/MavLinkFtpClientImpl.cpp

@@ -83,7 +83,6 @@ static const char	kDirentFile = 'F';	///< Identifies File returned from List com
 static const char	kDirentDir = 'D';	///< Identifies Directory returned from List command
 static const char	kDirentSkip = 'S';	///< Identifies Skipped entry from List command
 
-
 MavLinkFtpClientImpl::MavLinkFtpClientImpl(int system_id, int component_id)
 	: MavLinkNodeImpl(system_id, component_id)
 {
@@ -318,7 +317,7 @@ bool MavLinkFtpClientImpl::createLocalFile()
 		{
 			// user was lazy, only told us where to put the file, so we borrow the name of the file
 			// from the source.
-			auto remote = FileSystem::getFileName(remote_file_);
+			auto remote = FileSystem::getFileName(normalize(remote_file_));
 			local_file_ = FileSystem::combine(path, remote);
 		}
 		else
@@ -650,8 +649,16 @@ void MavLinkFtpClientImpl::handleResponse(const MavLinkMessage& msg)
 			{
 				success_ = false;
 				if (progress_ != nullptr) {
-					progress_->error = error;
-					progress_->message = Utils::stringf("ftp error %d", error);
+					if (error = kErrFailErrno) {
+						const uint8_t* data = &(payload->data);
+						error = static_cast<int>(data[1]);
+						progress_->error = error;
+						progress_->message = Utils::stringf("ftp kErrFailErrno %d", error);
+					}
+					else {
+						progress_->error = error;
+						progress_->message = Utils::stringf("ftp error %d", error);
+					}
 				}
 			}
 			errorCode_ = error;
@@ -728,3 +735,27 @@ void MavLinkFtpClientImpl::recordMessageReceived()
 		progress_->longest_delay = static_cast<double>(duration.count());
 	}
 }
+
+
+std::string MavLinkFtpClientImpl::replaceAll(std::string s, char toFind, char toReplace) {
+	size_t pos = s.find_first_of(toFind, 0);
+	while (pos != std::string::npos) {
+		s.replace(pos, 1, 1, toReplace);
+		pos = s.find_first_of(toFind, 0);
+	}
+	return s;
+}
+
+std::string MavLinkFtpClientImpl::normalize(std::string arg) {
+	if (FileSystem::kPathSeparator == '\\') {
+		return replaceAll(arg, '/', '\\'); // make sure user input matches what FileSystem will do when resolving paths.
+	}
+	return arg;
+}
+
+std::string MavLinkFtpClientImpl::toPX4Path(std::string arg) {
+	if (FileSystem::kPathSeparator == '\\') {
+		return replaceAll(arg, '\\', '/'); // PX4 uses '/'
+	}
+	return arg;
+}

MavLinkCom/src/impl/MavLinkFtpClientImpl.hpp

@@ -49,6 +49,9 @@ namespace mavlinkcom_impl {
 		void recordMessageReceived();
 		void runStateMachine();
 		void retry();
+		std::string replaceAll(std::string s, char toFind, char toReplace);
+		std::string normalize(std::string arg);
+		std::string toPX4Path(std::string arg);
 
 		// the following state is needed to implement a restartable state machine for each command
 		// with a watchdog

MavLinkCom/src/serial_com/SerialPort.cpp

@@ -291,7 +291,7 @@ class SerialPort::serialport_impl
 		int readTimeout = -1;
 		int writeTimeout = -1;
 
-		fd = open(portName, O_RDWR | O_NONBLOCK | O_NOCTTY);
+		fd = open(portName, O_RDWR | O_NOCTTY);
 		if (fd == -1)
 		{
 			return -1;