diff --git a/MavLinkCom/MavLinkComGenerator/MavLinkGenerator.cs b/MavLinkCom/MavLinkComGenerator/MavLinkGenerator.cs deleted file mode 100644 index 6e3f6e19d3..0000000000 --- a/MavLinkCom/MavLinkComGenerator/MavLinkGenerator.cs +++ /dev/null @@ -1,630 +0,0 @@ -// Copyright (c) Microsoft Corporation. All rights reserved. -// Licensed under the MIT License. - -using System; -using System.Collections.Generic; -using System.IO; -using System.Linq; -using System.Security.Cryptography; -using System.Text; -using System.Threading.Tasks; - -namespace MavLinkComGenerator -{ - class MavLinkGenerator - { - StreamWriter header; - StreamWriter impl; - MavLink definitions; - - public void GenerateMessages(MavLink definitions, string outputDirectory) - { - this.definitions = definitions; - - using (header = new StreamWriter(Path.Combine(outputDirectory, "MavLinkMessages.hpp"))) - { - using (impl = new StreamWriter(Path.Combine(outputDirectory, "MavLinkMessages.cpp"))) - { - header.WriteLine("// Copyright (c) Microsoft Corporation. All rights reserved."); - header.WriteLine("// Licensed under the MIT License."); - header.WriteLine("#ifndef MavLinkCom_MavLinkMessages_hpp"); - header.WriteLine("#define MavLinkCom_MavLinkMessages_hpp"); - header.WriteLine(""); - header.WriteLine("#include "); - header.WriteLine("#include "); - header.WriteLine("#include \"MavLinkMessageBase.hpp\""); - header.WriteLine(""); - header.WriteLine("namespace mavlinkcom"); - header.WriteLine("{"); - header.WriteLine(""); - - impl.WriteLine("// Copyright (c) Microsoft Corporation. All rights reserved."); - impl.WriteLine("// Licensed under the MIT License."); - impl.WriteLine("#include \"MavLinkMessages.hpp\""); ; - impl.WriteLine("#include "); - impl.WriteLine("using namespace mavlinkcom;"); - impl.WriteLine(""); - - GenerateEnums(); - GenerateMessages(); - GenerateCommands(); - GenerateDecodeMethod(); - - header.WriteLine("}"); - header.WriteLine(""); - header.WriteLine("#endif"); - } - } - } - - - private void GenerateCommands() - { - var cmds = (from e in definitions.enums where e.name == "MAV_CMD" select e).FirstOrDefault(); - if (cmds == null) - { - return; - } - foreach (var cmd in cmds.entries) - { - Console.WriteLine("generating command {0}", cmd.name); - - if (!string.IsNullOrWhiteSpace(cmd.description)) - { - WriteComment("", cmd.description); - } - string name = CamelCase(cmd.name); - header.WriteLine("class {0} : public MavLinkCommand {{", name); - header.WriteLine("public:"); - header.WriteLine(" const static uint16_t kCommandId = {0};", cmd.value); - header.WriteLine(" {0}() {{ command = kCommandId; }}", name); - UniqueList unique = new UniqueList(); - foreach (var p in cmd.parameters) - { - string fieldName = p.label; - if (!p.reserved) - { - if (string.IsNullOrWhiteSpace(fieldName) && !string.IsNullOrWhiteSpace(p.description)) - { - fieldName = NameFromDescription(p.description); - } - else - { - fieldName = LegalizeIdentifier(fieldName); - } - - if (fieldName != "Empty" && fieldName != "Reserved") - { - if (!string.IsNullOrWhiteSpace(p.description)) - { - WriteComment(" ", p.description); - } - header.WriteLine(" float {0} = 0;", unique.Add(fieldName)); - } - } - } - - unique = new UniqueList(); - header.WriteLine("protected:"); - header.WriteLine(" virtual void pack();"); - - impl.WriteLine("void {0}::pack() {{", name); - int i = 0; - foreach (var p in cmd.parameters) - { - i++; - if (!p.reserved) - { - string fieldName = p.label; - if (string.IsNullOrWhiteSpace(fieldName) && !string.IsNullOrWhiteSpace(p.description)) - { - fieldName = NameFromDescription(p.description); - } - else - { - fieldName = LegalizeIdentifier(fieldName); - } - if (fieldName != "Empty" && fieldName != "Reserved") - { - impl.WriteLine(" param{0} = {1};", i, unique.Add(fieldName)); - } - } - } - impl.WriteLine("}"); - - header.WriteLine(" virtual void unpack();"); - impl.WriteLine("void {0}::unpack() {{", name); - - unique = new UniqueList(); - i = 0; - foreach (var p in cmd.parameters) - { - i++; - if (!p.reserved) - { - string fieldName = p.label; - if (string.IsNullOrWhiteSpace(fieldName) && !string.IsNullOrWhiteSpace(p.description)) - { - fieldName = NameFromDescription(p.description); - } - else - { - fieldName = LegalizeIdentifier(fieldName); - } - if (fieldName != "Empty" && fieldName != "Reserved") - { - impl.WriteLine(" {1} = param{0};", i, unique.Add(fieldName)); - } - } - } - impl.WriteLine("}"); - header.WriteLine("};"); - } - - - } - - char[] nameSeparators = new char[] { '_' }; - - private string CamelCase(string name) - { - StringBuilder sb = new StringBuilder(); - foreach (string word in name.Split(nameSeparators, StringSplitOptions.RemoveEmptyEntries)) - { - string lower = word.ToLowerInvariant(); - string camel = Char.ToUpperInvariant(lower[0]).ToString(); - if (lower.Length > 1) - { - camel += lower.Substring(1); - } - sb.Append(camel); - } - return sb.ToString(); - } - - char[] terminators = new char[] { '.', '-', ':', ',', '(', '[' }; - char[] whitespace = new char[] { ' ', '\t', '\n', '\r' }; - - private string NameFromDescription(string description) - { - int i = description.IndexOf("this sets"); - if (i > 0) - { - description = description.Substring(i + 9); - } - i = description.IndexOfAny(terminators); - if (i > 0) - { - description = description.Substring(0, i); - } - return LegalizeIdentifier(description); - } - - private string LegalizeIdentifier(string identifier) - { - StringBuilder sb = new StringBuilder(); - - string[] words = identifier.Split(whitespace, StringSplitOptions.RemoveEmptyEntries); - int count = 0; - for (int j = 0; j < words.Length; j++) - { - string w = words[j]; - w = RemoveIllegalNameChars(w); - w = CamelCase(w); - if (j > 0 && (w == "In" || w == "From" || w == "And" || w == "As" || w == "The" || w == "And" || w == "It" || w == "On")) - { - continue; // skip filler words - } - sb.Append(w); - count++; - if (count == 3) - { - break; - } - } - if (sb.Length == 0) - { - sb.Append("p"); - } - return sb.ToString(); - } - - private string RemoveIllegalNameChars(string w) - { - StringBuilder sb = new StringBuilder(); - for (int i = 0, n = w.Length; i < n; i++) - { - char ch = w[i]; - if (!char.IsLetter(ch) && ch != '_') - { - sb.Append("p"); - } - if (char.IsLetterOrDigit(ch) || ch == '_') - { - sb.Append(ch); - } - } - if (sb.Length == 0) - { - sb.Append("p"); - } - return sb.ToString(); - } - - - private void GenerateMessages() - { - foreach (var m in definitions.messages) - { - int id = int.Parse(m.id); - if (id > 255) - { - // these require mavlink 2... - continue; - } - - string name = CamelCase(m.name); - Console.WriteLine("generating message {0}", name); - if (!string.IsNullOrWhiteSpace(m.description)) - { - WriteComment("", m.description); - } - header.WriteLine("class MavLink{0} : public MavLinkMessageBase {{", name); - header.WriteLine("public:"); - header.WriteLine(" const static uint8_t kMessageId = {0};", m.id); - header.WriteLine(" MavLink{0}() {{ msgid = kMessageId; }}", name); - - // find array types and split the type string "int[16]" into element type "int" and isArray=true and array_length=16. - foreach (var field in m.fields) - { - string type = field.type; - if (type.Contains('[')) - { - var tuple = ParseArrayType(type); - field.type = tuple.Item1; - field.isArray = true; - field.array_length = tuple.Item2; - } - } - - int length = m.fields.Count; - for (int i = 0; i < length; i++) - { - var field = m.fields[i]; - if (!string.IsNullOrWhiteSpace(field.description)) - { - WriteComment(" ", field.description); - } - var type = field.type; - if (type == "uint8_t_mavlink_version") - { - type = "uint8_t"; - } - if (field.isArray) - { - header.WriteLine(" {0} {1}[{2}] = {{ 0 }};", type, field.name, field.array_length); - } - else - { - header.WriteLine(" {0} {1} = 0;", type, field.name); - } - } - - // mavlink packs the fields in order of descending size (but not including the extension fields. - var sortedFields = m.fields; - int extensionPos = m.ExtensionPos; - if (extensionPos == 0) - { - extensionPos = m.fields.Count; - } - sortedFields = new List(m.fields.Take(extensionPos)); - sortedFields = sortedFields.OrderByDescending(x => typeSize[x.type]).ToList(); - sortedFields.AddRange(m.fields.Skip(extensionPos)); - - m.fields = sortedFields; - - header.WriteLine(" virtual std::string toJSon();"); - header.WriteLine("protected:"); - - header.WriteLine(" virtual int pack(char* buffer) const;"); - impl.WriteLine("int MavLink{0}::pack(char* buffer) const {{", name); - - int offset = 0; - for (int i = 0; i < length; i++) - { - var field = m.fields[i]; - var type = field.type; - if (type == "uint8_t_mavlink_version") - { - type = "uint8_t"; - } - int size = typeSize[type]; - if (field.isArray) - { - // it is an array. - impl.WriteLine(" pack_{0}_array({1}, buffer, reinterpret_cast(&this->{2}[0]), {3});", type, field.array_length, field.name, offset); - size *= field.array_length; - } - else - { - impl.WriteLine(" pack_{0}(buffer, reinterpret_cast(&this->{1}), {2});", type, field.name, offset); - } - offset += size; - } - impl.WriteLine(" return {0};", offset); - impl.WriteLine("}"); - impl.WriteLine(""); - - header.WriteLine(" virtual int unpack(const char* buffer);"); - impl.WriteLine("int MavLink{0}::unpack(const char* buffer) {{", name); - - offset = 0; - for (int i = 0; i < length; i++) - { - var field = m.fields[i]; - var type = field.type; - if (type == "uint8_t_mavlink_version") - { - type = "uint8_t"; - } - int size = typeSize[type]; - if (field.isArray) - { - // it is an array. - impl.WriteLine(" unpack_{0}_array({1}, buffer, reinterpret_cast<{0}*>(&this->{2}[0]), {3});", type, field.array_length, field.name, offset); - size *= field.array_length; - } - else - { - impl.WriteLine(" unpack_{0}(buffer, reinterpret_cast<{0}*>(&this->{1}), {2});", type, field.name, offset); - } - offset += size; - } - impl.WriteLine(" return {0};", offset); - impl.WriteLine("}"); - impl.WriteLine(""); - - - impl.WriteLine("std::string MavLink{0}::toJSon() {{", name); - impl.WriteLine(" std::ostringstream ss;"); - - impl.WriteLine(" ss << \"{{ \\\"name\\\": \\\"{0}\\\", \\\"id\\\": {1}, \\\"timestamp\\\":\" << timestamp << \", \\\"msg\\\": {{\";", m.name, id); - - for (int i = 0; i < length; i++) - { - var field = m.fields[i]; - var type = field.type; - if (type == "uint8_t_mavlink_version") - { - type = "uint8_t"; - } - if (i == 0) - { - impl.Write(" ss << \"\\\"{0}\\\":\" ", field.name); - } - else - { - impl.Write(" ss << \", \\\"{0}\\\":\" ", field.name); - } - if (field.isArray) - { - if (type == "char") - { - impl.Write(" << \"\\\"\" << "); - impl.Write("char_array_tostring({1}, reinterpret_cast<{0}*>(&this->{2}[0]))", type, field.array_length, field.name); - impl.Write(" << \"\\\"\""); - } - else - { - impl.Write(" << \"[\" << "); - impl.Write("{0}_array_tostring({1}, reinterpret_cast<{0}*>(&this->{2}[0]))", type, field.array_length, field.name); - impl.Write(" << \"]\""); - } - } - else if (type == "int8_t") - { - // so that stream doesn't try and treat this as a char. - impl.Write(" << static_cast(this->{0})", field.name); - } - else if (type == "uint8_t") - { - // so that stream doesn't try and treat this as a char. - impl.Write(" << static_cast(this->{0})", field.name); - } - else if (type == "float") - { - impl.Write(" << float_tostring(this->{0})", field.name); - } - else - { - impl.Write(" << this->{0}", field.name); - } - - impl.WriteLine(";"); - } - impl.WriteLine(" ss << \"} },\";"); - - impl.WriteLine(" return ss.str();"); - impl.WriteLine("}"); - impl.WriteLine(""); - - header.WriteLine("};"); - header.WriteLine(""); - } - } - - public void GenerateDecodeMethod() - { - impl.WriteLine("MavLinkMessageBase* MavLinkMessageBase::lookup(const MavLinkMessage& msg) {"); - impl.WriteLine(" MavLinkMessageBase* result = nullptr;"); - impl.WriteLine(" switch (static_cast(msg.msgid)) {"); - foreach (var m in definitions.messages) - { - int id = int.Parse(m.id); - if (id > 255) - { - // these require mavlink 2... - continue; - } - impl.WriteLine(" case MavLinkMessageIds::MAVLINK_MSG_ID_{0}:", m.name); - string name = CamelCase(m.name); - - impl.WriteLine(" result = new MavLink{0}();", name); - impl.WriteLine(" break;"); - } - - impl.WriteLine(" default:"); - impl.WriteLine(" break;"); - impl.WriteLine(" }"); - impl.WriteLine(" if (result != nullptr) {"); - impl.WriteLine(" result->decode(msg);"); - impl.WriteLine(" }"); - impl.WriteLine(" return result;"); - impl.WriteLine("}"); - } - - public Tuple ParseArrayType(string type) - { - int i = type.IndexOf('['); - int k = type.IndexOf(']', i); - if (k > i) - { - string name = type.Substring(0, i); - i++; - string index = type.Substring(i, k - i); - int j = 0; - int.TryParse(index, out j); - return new Tuple(name, j); - } - throw new Exception("Invalid array type specification: " + type); - } - - public static Dictionary typeSize = new Dictionary() - { - { "float" , 4}, - {"double" , 8 }, - {"char" , 1}, - {"int8_t" , 1}, - {"uint8_t" , 1}, - {"uint8_t_mavlink_version" , 1 }, - {"int16_t" , 2 }, - {"uint16_t" , 2 }, - {"int32_t" , 4 }, - {"uint32_t" , 4 }, - {"int64_t" , 8 }, - {"uint64_t" , 8 } - }; - - private void GenerateEnums() - { - - header.WriteLine("enum class MavLinkMessageIds {"); - bool first = true; - foreach (var m in definitions.messages) - { - int id = int.Parse(m.id); - if (id > 255) - { - // these require mavlink 2... - continue; - } - if (!first) - { - header.WriteLine(","); - } - first = false; - header.Write(" MAVLINK_MSG_ID_{0} = {1}", m.name, m.id); - } - header.WriteLine(""); - - header.WriteLine("};"); - - - foreach (var e in definitions.enums) - { - Console.WriteLine("generating enum class {0}", e.name); - - if (!string.IsNullOrWhiteSpace(e.description)) - { - WriteComment("", e.description); - } - header.WriteLine("enum class {0} {{", e.name); - - bool zeroUsed = false; - - int length = e.entries.Count; - for (int i = 0; i < length; i++) - { - var field = e.entries[i]; - if (!string.IsNullOrWhiteSpace(field.description)) - { - WriteComment(" ", field.description); - } - header.Write(" {0}", field.name); - if (!zeroUsed || field.value != 0) - { - header.Write(" = {0}", field.value); - } - if (!zeroUsed && field.value == 0) - { - zeroUsed = true; - } - if (i + 1 < length) - { - header.Write(","); - } - header.WriteLine(); - } - - header.WriteLine("};"); - header.WriteLine(""); - } - } - - private void WriteComment(string indent, string description) - { - int lineLength = 0; - foreach (string word in description.Split(new char[] { '\r', '\n', ' ', '\t' }, StringSplitOptions.RemoveEmptyEntries)) - { - if (lineLength > 80) - { - header.WriteLine(); - lineLength = 0; - } - if (lineLength == 0) - { - header.Write(indent + "// "); - lineLength = indent.Length + 3; - } - else - { - lineLength++; - header.Write(" "); - } - header.Write(word); - lineLength += word.Length; - } - if (lineLength > 0) - { - header.WriteLine(); - } - } - - class UniqueList - { - HashSet existing = new HashSet(); - - public string Add(String baseName) - { - string name = baseName; - int index = 2; - while (existing.Contains(name)) - { - name = baseName + index++; - } - existing.Add(name); - return name; - } - } - } -} diff --git a/MavLinkCom/common_utils/ThreadUtils.cpp b/MavLinkCom/common_utils/ThreadUtils.cpp deleted file mode 100644 index 80d8b7f9bb..0000000000 --- a/MavLinkCom/common_utils/ThreadUtils.cpp +++ /dev/null @@ -1,70 +0,0 @@ -#include "ThreadUtils.hpp" -#include "StrictMode.hpp" -#include - -#ifdef _WIN32 -#include // SetThreadPriority and GetCurrentThread -#else -#include -#endif - -using namespace mavlink_utils; - -// make the current thread run with maximum priority. -bool CurrentThread::setMaximumPriority() -{ -#ifdef _WIN32 - HANDLE thread = GetCurrentThread(); - // THREAD_PRIORITY_HIGHEST is too high and makes animation a bit jumpy. - int rc = SetThreadPriority(GetCurrentThread(), THREAD_PRIORITY_ABOVE_NORMAL); - if (rc == 0) { - rc = GetLastError(); - return false; - } - return true; -#elif defined(__APPLE__) - // TODO: How to handle POSIX thread priorities on OSX? - return true; -#else - int policy; - struct sched_param param; - int err = pthread_getschedparam(pthread_self(), &policy, ¶m); - if (err != 0) return false; - int maxPriority = sched_get_priority_max(policy); - err = pthread_setschedprio(pthread_self(), maxPriority); - return err == 0; -#endif -} - -#ifdef _WIN32 -typedef HRESULT(WINAPI* SetThreadDescriptionFunction)(_In_ HANDLE hThread, _In_ PCWSTR lpThreadDescription); -static SetThreadDescriptionFunction setThreadDescriptionFunction = nullptr; -#endif - -// setThreadName is a helper function that is useful when debugging because your threads -// show up in the debugger with the name you set which makes it easier to find the threads -// that you are interested in. -bool CurrentThread::setThreadName(const std::string& name) -{ -#ifdef _WIN32 - // unfortunately this is only available on Windows 10, and AirSim is not limited to that. - if (setThreadDescriptionFunction == nullptr) { - HINSTANCE hGetProcIDDLL = LoadLibrary(L"Kernel32"); - FARPROC func = GetProcAddress(hGetProcIDDLL, "SetThreadDescription"); - if (func != nullptr) { - setThreadDescriptionFunction = (SetThreadDescriptionFunction)func; - } - } - if (setThreadDescriptionFunction != nullptr) { - std::wstring_convert, wchar_t> converter; - std::wstring wide_path = converter.from_bytes(name); - auto rc = (*setThreadDescriptionFunction)(GetCurrentThread(), wide_path.c_str()); - return S_OK == rc; - } - return false; -#elif defined(__APPLE__) - return 0 == pthread_setname_np(name.c_str()); -#else - return 0 == pthread_setname_np(pthread_self(), name.c_str()); -#endif -} diff --git a/MavLinkCom/src/impl/MavLinkFtpClientImpl.cpp b/MavLinkCom/src/impl/MavLinkFtpClientImpl.cpp deleted file mode 100644 index 6a504c247a..0000000000 --- a/MavLinkCom/src/impl/MavLinkFtpClientImpl.cpp +++ /dev/null @@ -1,816 +0,0 @@ -// Copyright (c) Microsoft Corporation. All rights reserved. -// Licensed under the MIT License. - -#include "MavLinkFtpClientImpl.hpp" -#include -#include "Utils.hpp" -#include "FileSystem.hpp" -#include - -using namespace mavlink_utils; -using namespace mavlinkcom; -using namespace mavlinkcom_impl; -using milliseconds = std::chrono::milliseconds; - -#define MAXIMUM_ROUND_TRIP_TIME 200 // 200 milliseconds should be plenty of time for single round trip to remote node. -#define TIMEOUT_INTERVAL 10 // 10 * MAXIMUM_ROUND_TRIP_TIME means we have a problem. - -// These definitions are copied from PX4 implementation - -/// @brief This is the payload which is in mavlink_file_transfer_protocol_t.payload. We pad the structure ourselves to -/// 32 bit alignment to avoid usage of any pack pragmas. -struct FtpPayload -{ - uint16_t seq_number; ///< sequence number for message - uint8_t session; ///< Session id for read and write commands - uint8_t opcode; ///< Command opcode - uint8_t size; ///< Size of data - uint8_t req_opcode; ///< Request opcode returned in kRspAck, kRspNak message - uint8_t burst_complete; ///< Only used if req_opcode=kCmdBurstreadFile - 1: set of burst packets complete, 0: More burst packets coming. - uint8_t padding; ///< 32 bit aligment padding - uint32_t offset; ///< Offsets for List and Read commands - uint8_t data; ///< command data, varies by Opcode -}; - -void setPayloadFilename(FtpPayload* payload, const char* filename) -{ - - const size_t maxFileName = 251 - sizeof(FtpPayload); - size_t len = strlen(filename); - if (len > maxFileName) { - len = maxFileName; - } - strncpy(reinterpret_cast(&payload->data), filename, len); - payload->size = static_cast(len); -} - -/// @brief Command opcodes -enum Opcode : uint8_t -{ - kCmdNone, ///< ignored, always acked - kCmdTerminateSession, ///< Terminates open Read session - kCmdResetSessions, ///< Terminates all open Read sessions - kCmdListDirectory, ///< List files in from - kCmdOpenFileRO, ///< Opens file at for reading, returns - kCmdReadFile, ///< Reads bytes from in - kCmdCreateFile, ///< Creates file at for writing, returns - kCmdWriteFile, ///< Writes bytes to in - kCmdRemoveFile, ///< Remove file at - kCmdCreateDirectory, ///< Creates directory at - kCmdRemoveDirectory, ///< Removes Directory at , must be empty - kCmdOpenFileWO, ///< Opens file at for writing, returns - kCmdTruncateFile, ///< Truncate file at to length - kCmdRename, ///< Rename to - kCmdCalcFileCRC32, ///< Calculate CRC32 for file at - kCmdBurstreadFile, ///< Burst download session file - - kRspAck = 128, ///< Ack response - kRspNak ///< Nak response -}; - -/// @brief Error codes returned in Nak response PayloadHeader.data[0]. -enum ErrorCode : uint8_t -{ - kErrNone, - kErrFail, ///< Unknown failure - kErrFailErrno, ///< Command failed, errno sent back in PayloadHeader.data[1] - kErrInvalidDataSize, ///< PayloadHeader.size is invalid - kErrInvalidSession, ///< Session is not currently open - kErrNoSessionsAvailable, ///< All available Sessions in use - kErrEOF, ///< Offset past end of file for List and Read commands - kErrRetriesExhausted, - kErrUnknownCommand ///< Unknown command opcode -}; - -static const char kDirentFile = 'F'; ///< Identifies File returned from List command -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 localSystemId, int localComponentId) - : MavLinkNodeImpl(localSystemId, localComponentId) -{ -} - -MavLinkFtpClientImpl::~MavLinkFtpClientImpl() -{ -} - -bool MavLinkFtpClientImpl::isSupported() -{ - // request capabilities, it will respond with AUTOPILOT_VERSION. - MavLinkAutopilotVersion ver; - assertNotPublishingThread(); - if (!getCapabilities().wait(5000, &ver)) { - throw std::runtime_error(Utils::stringf("Five second timeout waiting for response to mavlink command MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES")); - } - return (ver.capabilities & static_cast(MAV_PROTOCOL_CAPABILITY::MAV_PROTOCOL_CAPABILITY_FTP)) != 0; -} - -void MavLinkFtpClientImpl::subscribe() -{ - if (subscription_ == 0) { - subscription_ = getConnection()->subscribe([=](std::shared_ptr connection, const MavLinkMessage& msg) { - unused(connection); - handleResponse(msg); - }); - } -} - -void MavLinkFtpClientImpl::list(MavLinkFtpProgress& progress, const std::string& remotePath, std::vector& files) -{ - if (waiting_) { - cancel(); - } - ensureConnection(); - progress_ = &progress; - files_ = &files; - command_ = FtpCommandList; - remote_file_ = remotePath; - size_t len = remote_file_.size(); - if (len > 1 && remote_file_[len - 1] == '/') { - // must trim trailing slashes so PX4 doesn't hang! - remote_file_ = remote_file_.substr(0, len - 1); - } - file_index_ = 0; - runStateMachine(); - progress.complete = true; - files_ = nullptr; - progress_ = nullptr; -} - -void MavLinkFtpClientImpl::get(MavLinkFtpProgress& progress, const std::string& remotePath, const std::string& localPath) -{ - if (waiting_) { - cancel(); - } - ensureConnection(); - progress_ = &progress; - command_ = FtpCommandGet; - local_file_ = localPath; - remote_file_ = remotePath; - bytes_read_ = 0; - file_size_ = 0; - remote_file_open_ = false; - - runStateMachine(); - progress_ = nullptr; - progress.complete = true; -} - -void MavLinkFtpClientImpl::put(MavLinkFtpProgress& progress, const std::string& remotePath, const std::string& localPath) -{ - local_file_ = localPath; - remote_file_ = remotePath; - - if (waiting_) { - cancel(); - } - ensureConnection(); - progress_ = &progress; - command_ = FtpCommandPut; - - if (openSourceFile()) { - remote_file_open_ = false; - runStateMachine(); - } - progress_ = nullptr; - progress.complete = true; -} - -void MavLinkFtpClientImpl::remove(MavLinkFtpProgress& progress, const std::string& remotePath) -{ - remote_file_ = remotePath; - - if (waiting_) { - cancel(); - } - ensureConnection(); - progress_ = &progress; - command_ = FtpCommandRemove; - runStateMachine(); - progress_ = nullptr; - progress.complete = true; -} - -void MavLinkFtpClientImpl::mkdir(MavLinkFtpProgress& progress, const std::string& remotePath) -{ - remote_file_ = remotePath; - - if (waiting_) { - cancel(); - } - ensureConnection(); - progress_ = &progress; - command_ = FtpCommandMkdir; - runStateMachine(); - progress_ = nullptr; - progress.complete = true; -} - -void MavLinkFtpClientImpl::rmdir(MavLinkFtpProgress& progress, const std::string& remotePath) -{ - remote_file_ = remotePath; - - if (waiting_) { - cancel(); - } - ensureConnection(); - progress_ = &progress; - command_ = FtpCommandRmdir; - runStateMachine(); - progress_ = nullptr; - progress.complete = true; -} - -void MavLinkFtpClientImpl::runStateMachine() -{ - waiting_ = true; - retries_ = 0; - total_time_ = milliseconds::zero(); - messages_ = 0; - - progress_->cancel = false; - progress_->error = 0; - progress_->current = 0; - progress_->goal = 0; - progress_->complete = false; - progress_->longest_delay = 0; - progress_->message_count = 0; - - int before = 0; - subscribe(); - nextStep(); - - const int monitorInterval = 30; // milliseconds between monitoring progress. - double rate = 0; - double totalSleep = 0; - - while (waiting_) { - std::this_thread::sleep_for(std::chrono::milliseconds(monitorInterval)); - totalSleep += monitorInterval; - - int after = 0; - { - std::lock_guard guard(mutex_); - after = messages_; - if (messages_ > 0) { - rate = static_cast(total_time_.count()) / static_cast(messages_); - if (progress_ != nullptr) { - progress_->average_rate = rate; - } - } - } - - if (before == after) { - if (totalSleep > (MAXIMUM_ROUND_TRIP_TIME * TIMEOUT_INTERVAL)) { - Utils::log("ftp command timeout, not getting a response, so retrying\n", Utils::kLogLevelWarn); - retry(); - totalSleep = 0; - } - } - else { - totalSleep = 0; - } - before = after; - } - waiting_ = false; -} - -void MavLinkFtpClientImpl::nextStep() -{ - switch (command_) { - case FtpCommandList: - listDirectory(); - break; - case FtpCommandGet: - readFile(); - break; - case FtpCommandPut: - writeFile(); - break; - case FtpCommandRemove: - removeFile(); - break; - case FtpCommandMkdir: - mkdir(); - break; - case FtpCommandRmdir: - rmdir(); - break; - default: - break; - } -} - -void MavLinkFtpClientImpl::removeFile() -{ - MavLinkFileTransferProtocol ftp; - FtpPayload* payload = reinterpret_cast(&ftp.payload[0]); - ftp.target_component = getTargetComponentId(); - ftp.target_system = getTargetSystemId(); - payload->opcode = kCmdRemoveFile; - setPayloadFilename(payload, remote_file_.c_str()); - sendMessage(ftp); - recordMessageSent(); -} - -void MavLinkFtpClientImpl::mkdir() -{ - MavLinkFileTransferProtocol ftp; - FtpPayload* payload = reinterpret_cast(&ftp.payload[0]); - ftp.target_component = getTargetComponentId(); - ftp.target_system = getTargetSystemId(); - payload->opcode = kCmdCreateDirectory; - setPayloadFilename(payload, remote_file_.c_str()); - sendMessage(ftp); - recordMessageSent(); -} - -void MavLinkFtpClientImpl::rmdir() -{ - MavLinkFileTransferProtocol ftp; - FtpPayload* payload = reinterpret_cast(&ftp.payload[0]); - ftp.target_component = getTargetComponentId(); - ftp.target_system = getTargetSystemId(); - payload->opcode = kCmdRemoveDirectory; - setPayloadFilename(payload, remote_file_.c_str()); - sendMessage(ftp); - recordMessageSent(); -} - -void MavLinkFtpClientImpl::listDirectory() -{ - MavLinkFileTransferProtocol ftp; - FtpPayload* payload = reinterpret_cast(&ftp.payload[0]); - ftp.target_component = getTargetComponentId(); - ftp.target_system = getTargetSystemId(); - payload->opcode = kCmdListDirectory; - setPayloadFilename(payload, remote_file_.c_str()); - payload->offset = file_index_; - sendMessage(ftp); - recordMessageSent(); -} - -bool MavLinkFtpClientImpl::openSourceFile() -{ - file_ptr_ = fopen(local_file_.c_str(), "rb"); - if (file_ptr_ == nullptr) { - if (progress_ != nullptr) { - progress_->error = errno; - progress_->message = Utils::stringf("Error opening file '%s' for reading, rc=%d", remote_file_.c_str(), progress_->error); - } - return false; - } - fseek(file_ptr_, 0, SEEK_END); - long pos = ftell(file_ptr_); - fseek(file_ptr_, 0, SEEK_SET); - file_size_ = static_cast(pos); - if (progress_ != nullptr) { - progress_->goal = file_size_; - } - return true; -} - -bool MavLinkFtpClientImpl::createLocalFile() -{ - if (file_ptr_ == nullptr) { - auto path = FileSystem::getFullPath(local_file_); - if (FileSystem::isDirectory(path)) { - // 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(normalize(remote_file_)); - local_file_ = FileSystem::combine(path, remote); - } - else { - // check if directory exists. - FileSystem::removeLeaf(path); - if (FileSystem::isDirectory(path)) { - // perfect. - } - else if (FileSystem::exists(path)) { - if (progress_ != nullptr) { - progress_->error = errno; - progress_->message = Utils::stringf("Error opening file because '%s' is not a directory", path.c_str()); - } - return false; - } - else { - if (progress_ != nullptr) { - progress_->error = errno; - progress_->message = Utils::stringf("Error opening file because '%s' should be a directory but it was not found", path.c_str()); - } - return false; - } - } - - file_ptr_ = fopen(local_file_.c_str(), "wb"); - if (file_ptr_ == nullptr) { - if (progress_ != nullptr) { - progress_->error = errno; - progress_->message = Utils::stringf("Error opening file '%s' for writing, rc=%d", local_file_.c_str(), errno); - } - return false; - } - file_size_ = 0; - } - return true; -} - -void MavLinkFtpClientImpl::readFile() -{ - if (!remote_file_open_) { - MavLinkFileTransferProtocol ftp; - FtpPayload* payload = reinterpret_cast(&ftp.payload[0]); - ftp.target_component = getTargetComponentId(); - ftp.target_system = getTargetSystemId(); - payload->opcode = kCmdOpenFileRO; - setPayloadFilename(payload, remote_file_.c_str()); - sendMessage(ftp); - recordMessageSent(); - } - else { - if (createLocalFile()) { - // use last_message_ so we preserve the sessionid. - FtpPayload* payload = reinterpret_cast(&last_message_.payload[0]); - payload->opcode = kCmdReadFile; - payload->offset = bytes_read_; - last_message_.target_component = getTargetComponentId(); - last_message_.target_system = getTargetSystemId(); - sendMessage(last_message_); - recordMessageSent(); - if (progress_ != nullptr) { - progress_->current = bytes_read_; - } - } - else { - // could not create the local file, so stop. - waiting_ = false; - } - } -} - -void MavLinkFtpClientImpl::writeFile() -{ - - if (!remote_file_open_) { - MavLinkFileTransferProtocol ftp; - FtpPayload* payload = reinterpret_cast(&ftp.payload[0]); - ftp.target_component = getTargetComponentId(); - ftp.target_system = getTargetSystemId(); - payload->opcode = kCmdOpenFileWO; - strcpy(reinterpret_cast(&payload->data), remote_file_.c_str()); - payload->size = static_cast(remote_file_.size()); - sendMessage(ftp); - recordMessageSent(); - } - else { - // must use last_message_ so we preserve the session id. - MavLinkFileTransferProtocol& ftp = last_message_; - FtpPayload* payload = reinterpret_cast(&ftp.payload[0]); - payload->opcode = kCmdWriteFile; - payload->seq_number = sequence_; - fseek(file_ptr_, bytes_written_, SEEK_SET); - uint8_t* data = &payload->data; - size_t bytes = fread(data, 1, 251 - 12, file_ptr_); - if (progress_ != nullptr) { - progress_->current = bytes_written_; - } - if (bytes == 0) { - success_ = true; - reset(); - int err = ferror(file_ptr_); - if (err != 0) { - if (progress_ != nullptr) { - progress_->error = err; - progress_->message = Utils::stringf("error reading local file, errno=%d", err); - } - } - fclose(file_ptr_); - file_ptr_ = nullptr; - waiting_ = false; - } - else { - payload->offset = bytes_written_; - payload->size = static_cast(bytes); - ftp.target_component = getTargetComponentId(); - ftp.target_system = getTargetSystemId(); - sendMessage(ftp); - recordMessageSent(); - } - } -} - -void MavLinkFtpClientImpl::cancel() -{ - // todo: wait for any pending responses from PX4 so we can safely start a new command. - reset(); -} - -void MavLinkFtpClientImpl::close() -{ - if (file_ptr_ != nullptr) { - reset(); - fclose(file_ptr_); - file_ptr_ = nullptr; - } - if (subscription_ != 0) { - getConnection()->unsubscribe(subscription_); - subscription_ = 0; - } -} - -void MavLinkFtpClientImpl::reset() -{ - MavLinkFileTransferProtocol ftp; - ftp.target_component = getTargetComponentId(); - ftp.target_system = getTargetSystemId(); - FtpPayload* payload = reinterpret_cast(&ftp.payload[0]); - payload->opcode = kCmdResetSessions; - sendMessage(ftp); - recordMessageSent(); -} - -void MavLinkFtpClientImpl::handleListResponse() -{ - FtpPayload* payload = reinterpret_cast(&last_message_.payload[0]); - - if (payload->offset != file_index_) { - // todo: error handling here? sequence is out of order... - Utils::log(Utils::stringf("list got offset %d, but expecting file index %d\n", payload->offset, file_index_), Utils::kLogLevelError); - retry(); - return; - } - - if (payload->offset == 0 && payload->size == 0) { - // directory must be empty then, can't do nextStep because - // it will just loop for ever re-requesting zero offset into - // empty directory. - reset(); - success_ = true; - waiting_ = false; - return; - } - - // result should be a list of null terminated file names. - uint8_t* data = &payload->data; - for (int offset = 0; offset < payload->size;) { - uint8_t dirType = data[offset]; - offset++; - retries_ = 0; - file_index_++; - int len = 0; - std::string name = reinterpret_cast(&data[offset]); - if (dirType == kDirentSkip) { - // skipping this entry - } - else if (dirType == kDirentFile) { - size_t i = name.find_last_of('\t'); - MavLinkFileInfo info; - info.is_directory = false; - len = static_cast(name.size()); - if (i > 0) { - // remove the file size field. - std::string size(name.begin() + i + 1, name.end()); - info.size = Utils::to_integer(size); - name.erase(name.begin() + i, name.end()); - } - info.name = name; - //printf("%s\n", name.c_str()); - if (files_ != nullptr) { - files_->push_back(info); - } - } - else if (dirType == kDirentDir) { - MavLinkFileInfo info; - info.is_directory = true; - info.name = name; - len = static_cast(name.size()); - if (files_ != nullptr) { - files_->push_back(info); - } - } - offset += len + 1; - } - if (progress_ != nullptr) { - progress_->current = file_index_; - } - // request the next batch. - nextStep(); -} - -void MavLinkFtpClientImpl::handleReadResponse() -{ - FtpPayload* payload = reinterpret_cast(&last_message_.payload[0]); - if (payload->req_opcode == kCmdOpenFileRO) { - remote_file_open_ = true; - bytes_read_ = 0; - retries_ = 0; - sequence_ = payload->seq_number; - uint32_t* size = reinterpret_cast(&payload->data); - file_size_ = static_cast(*size); - if (progress_ != nullptr) { - progress_->goal = file_size_; - } - nextStep(); - } - else if (payload->req_opcode == kCmdReadFile) { - int seq = static_cast(payload->seq_number); - if (seq != sequence_ + 1) { - Utils::log(Utils::stringf("packet %d is out of sequence, expecting number %d\n", seq, sequence_ + 1), Utils::kLogLevelError); - // perhaps this was a late response after we did a retry, so ignore it. - return; - } - else if (file_ptr_ != nullptr) { - sequence_ = payload->seq_number; - fwrite(&payload->data, payload->size, 1, file_ptr_); - bytes_read_ += payload->size; - retries_ = 0; - nextStep(); - } - } -} - -void MavLinkFtpClientImpl::handleWriteResponse() -{ - FtpPayload* payload = reinterpret_cast(&last_message_.payload[0]); - if (payload->req_opcode == kCmdOpenFileWO) { - remote_file_open_ = true; - bytes_written_ = 0; - retries_ = 0; - sequence_ = payload->seq_number; - nextStep(); - } - else if (payload->req_opcode == kCmdWriteFile) { - int seq = static_cast(payload->seq_number); - if (seq != sequence_ + 1) { - Utils::log(Utils::stringf("packet %d is out of sequence, expecting number %d\n", seq, sequence_ + 1), Utils::kLogLevelError); - // perhaps this was a late response after we did a retry, so ignore it. - return; - } - - uint32_t* size = reinterpret_cast(&payload->data); - // payload->size contains the bytes_written from PX4, so that's how much we advance. - bytes_written_ += static_cast(*size); - retries_ = 0; - sequence_++; - nextStep(); - } -} - -void MavLinkFtpClientImpl::handleRemoveResponse() -{ - success_ = true; - waiting_ = false; -} - -void MavLinkFtpClientImpl::handleRmdirResponse() -{ - success_ = true; - waiting_ = false; -} - -void MavLinkFtpClientImpl::handleMkdirResponse() -{ - success_ = true; - waiting_ = false; -} - -void MavLinkFtpClientImpl::handleResponse(const MavLinkMessage& msg) -{ - if (msg.msgid == static_cast(MavLinkMessageIds::MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL)) { - - last_message_.decode(msg); - recordMessageReceived(); - - FtpPayload* payload = reinterpret_cast(&last_message_.payload[0]); - if (payload->opcode == kRspNak) { - - // reached the end of the list or the file. - if (file_ptr_ != nullptr) { - fclose(file_ptr_); - file_ptr_ = nullptr; - } - - int error = static_cast(payload->data); - if (error == kErrEOF) { - // end of file or directory listing. - success_ = true; - error = 0; - } - else { - success_ = false; - if (progress_ != nullptr) { - if (error == kErrFailErrno) { - const uint8_t* data = &(payload->data); - error = static_cast(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; - waiting_ = false; - reset(); - } - else if (payload->opcode == kRspAck) { - if (progress_ != nullptr) { - progress_->message_count++; - } - // success, data should be following... - switch (payload->req_opcode) { - case kCmdListDirectory: - handleListResponse(); - break; - case kCmdOpenFileRO: - case kCmdReadFile: - handleReadResponse(); - break; - case kCmdOpenFileWO: - case kCmdWriteFile: - handleWriteResponse(); - break; - case kCmdResetSessions: - // ack on this cmd is a noop - break; - case kCmdRemoveFile: - handleRemoveResponse(); - break; - case kCmdRemoveDirectory: - handleRmdirResponse(); - break; - case kCmdCreateDirectory: - handleMkdirResponse(); - break; - default: - // todo: how to handle this? For now we ignore it and let the watchdog kick in and do a retry. - Utils::log(Utils::stringf("Unexpected ACK with req_opcode=%d\n", static_cast(payload->req_opcode)), Utils::kLogLevelWarn); - break; - } - } - } -} - -void MavLinkFtpClientImpl::MavLinkFtpClientImpl::retry() -{ - retries_++; - if (retries_ < 10) { - Utils::log(Utils::stringf("retry %d\n", retries_), Utils::kLogLevelWarn); - nextStep(); - } - else { - // give up then. - errorCode_ = kErrRetriesExhausted; - success_ = false; - waiting_ = false; - reset(); - } -} - -void MavLinkFtpClientImpl::recordMessageSent() -{ - // tell watchdog we are sending a request - std::lock_guard guard(mutex_); - start_time_ = std::chrono::duration_cast(std::chrono::system_clock::now().time_since_epoch()); -} - -void MavLinkFtpClientImpl::recordMessageReceived() -{ - std::lock_guard guard(mutex_); - messages_++; - milliseconds endTime = std::chrono::duration_cast(std::chrono::system_clock::now().time_since_epoch()); - milliseconds duration = endTime - start_time_; - total_time_ += duration; - if (progress_ != nullptr && duration.count() > progress_->longest_delay) { - progress_->longest_delay = static_cast(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; -} diff --git a/MavLinkLog.cpp b/MavLinkLog.cpp new file mode 100644 index 0000000000..64a6e9150a --- /dev/null +++ b/MavLinkLog.cpp @@ -0,0 +1,220 @@ +// Copyright (c) Microsoft Corporation. All rights reserved. +// Licensed under the MIT License. + +#include "MavLinkLog.hpp" +#include "Utils.hpp" +#include + +using namespace mavlinkcom; +using namespace mavlink_utils; + +#define MAVLINK_STX_MAVLINK1 0xFE // marker for old protocol +#define MAVLINK_STX 253 // marker for mavlink 2 + +uint64_t MavLinkFileLog::getTimeStamp() +{ + return std::chrono::duration_cast(std::chrono::high_resolution_clock::now().time_since_epoch()).count(); +} + +MavLinkFileLog::MavLinkFileLog() +{ + ptr_ = nullptr; + reading_ = false; + writing_ = false; + json_ = false; +} + +MavLinkFileLog::~MavLinkFileLog() +{ + close(); +} + +bool MavLinkFileLog::isOpen() +{ + return reading_ || writing_; +} +void MavLinkFileLog::openForReading(const std::string& filename) +{ + close(); + file_name_ = filename; + ptr_ = fopen(filename.c_str(), "rb"); + if (ptr_ == nullptr) { + throw std::runtime_error(Utils::stringf("Could not open the file %s, error=%d", filename.c_str(), errno)); + } + + fseek(ptr_, 0, SEEK_SET); + reading_ = true; + writing_ = false; +} +void MavLinkFileLog::openForWriting(const std::string& filename, bool json) +{ + close(); + json_ = json; + file_name_ = filename; + ptr_ = fopen(filename.c_str(), "wb"); + if (ptr_ == nullptr) { + throw std::runtime_error(Utils::stringf("Could not open the file %s, error=%d", filename.c_str(), errno)); + } + if (json) { + fprintf(ptr_, "{ \"rows\": [\n"); + } + reading_ = false; + writing_ = true; +} + +void MavLinkFileLog::close() +{ + FILE* temp = ptr_; + if (json_ && ptr_ != nullptr) { + fprintf(ptr_, " {}\n"); // so that trailing comma on last row isn't a problem. + fprintf(ptr_, "]}\n"); + } + ptr_ = nullptr; + if (temp != nullptr) { + fclose(temp); + } + reading_ = false; + writing_ = false; +} + +uint64_t FlipEndianness(uint64_t v) +{ + uint64_t result = 0; + uint64_t shift = v; + const int size = sizeof(uint64_t); + for (int i = 0; i < size; i++) { + uint64_t low = (shift & 0xff); + result = (result << 8) + low; + shift >>= 8; + } + return result; +} + +void MavLinkFileLog::write(const mavlinkcom::MavLinkMessage& msg, uint64_t timestamp) +{ + if (ptr_ != nullptr) { + if (reading_) { + throw std::runtime_error("Log file was opened for reading"); + } + if (json_) { + MavLinkMessageBase* strongTypedMsg = MavLinkMessageBase::lookup(msg); + if (strongTypedMsg != nullptr) { + strongTypedMsg->timestamp = timestamp; + std::string line = strongTypedMsg->toJSon(); + { + std::lock_guard lock(log_lock_); + fprintf(ptr_, " %s\n", line.c_str()); + } + delete strongTypedMsg; + } + } + else { + if (timestamp == 0) { + timestamp = getTimeStamp(); + } + // for compatibility with QGroundControl we have to save the time field in big endian. + // todo: mavlink2 support? + timestamp = FlipEndianness(timestamp); + + uint8_t magic = msg.magic; + if (magic != MAVLINK_STX_MAVLINK1) { + // has to be one or the other! + magic = MAVLINK_STX; + } + + std::lock_guard lock(log_lock_); + fwrite(×tamp, sizeof(uint64_t), 1, ptr_); + fwrite(&magic, 1, 1, ptr_); + fwrite(&msg.len, 1, 1, ptr_); + fwrite(&msg.seq, 1, 1, ptr_); + fwrite(&msg.sysid, 1, 1, ptr_); + fwrite(&msg.compid, 1, 1, ptr_); + + if (magic == MAVLINK_STX_MAVLINK1) { + uint8_t msgid = msg.msgid & 0xff; // truncate to mavlink 2 msgid + fwrite(&msgid, 1, 1, ptr_); + } + else { + // 24 bits. + uint8_t msgid = msg.msgid & 0xFF; + fwrite(&msgid, 1, 1, ptr_); + msgid = (msg.msgid >> 8) & 0xFF; + fwrite(&msgid, 1, 1, ptr_); + msgid = (msg.msgid >> 16) & 0xFF; + fwrite(&msgid, 1, 1, ptr_); + } + + fwrite(&msg.payload64, 1, msg.len, ptr_); + fwrite(&msg.checksum, sizeof(uint16_t), 1, ptr_); + } + } +} + +bool MavLinkFileLog::read(mavlinkcom::MavLinkMessage& msg, uint64_t& timestamp) +{ + if (ptr_ != nullptr) { + if (writing_) { + throw std::runtime_error("Log file was opened for writing"); + } + uint64_t time; + + size_t s = fread(&time, 1, sizeof(uint64_t), ptr_); + if (s < sizeof(uint64_t)) { + int hr = errno; + return false; + } + + timestamp = FlipEndianness(time); + + s = fread(&msg.magic, 1, 1, ptr_); + if (s == 0) { + return false; + } + s = fread(&msg.len, 1, 1, ptr_); + if (s == 0) { + return false; + } + s = fread(&msg.seq, 1, 1, ptr_); + if (s == 0) { + return false; + } + s = fread(&msg.sysid, 1, 1, ptr_); + if (s == 0) { + return false; + } + s = fread(&msg.compid, 1, 1, ptr_); + if (s == 0) { + return false; + } + + if (msg.magic == MAVLINK_STX_MAVLINK1) { + uint8_t msgid = 0; + s = fread(&msgid, 1, 1, ptr_); + msg.msgid = msgid; + } + else { + // 24 bits. + uint8_t msgid = 0; + s = fread(&msgid, 1, 1, ptr_); + msg.msgid = msgid; + s = fread(&msgid, 1, 1, ptr_); + msg.msgid |= (msgid << 8); + s = fread(&msgid, 1, 1, ptr_); + msg.msgid |= (msgid << 16); + } + + if (s < 1) { + return false; + } + s = fread(&msg.payload64, 1, msg.len, ptr_); + if (s < msg.len) { + return false; + } + s = fread(&msg.checksum, 1, sizeof(uint16_t), ptr_); + if (s < sizeof(uint16_t)) { + return false; + } + return true; + } + return false; +}