From a63caf9548b7663ea7a9fb7ee399b329163aadd9 Mon Sep 17 00:00:00 2001 From: Chris Lovett Date: Sun, 19 Mar 2017 16:58:21 -0700 Subject: [PATCH] Add mavlinktest commands into the mavlink log generated. --- MavLinkCom/MavLinkTest/main.cpp | 15 ++++++- MavLinkCom/include/MavLinkMessageBase.hpp | 5 ++- MavLinkCom/src/MavLinkMessageBase.cpp | 35 ++++++++++++++++ MavLinkCom/src/impl/MavLinkConnectionImpl.cpp | 40 ++----------------- 4 files changed, 55 insertions(+), 40 deletions(-) diff --git a/MavLinkCom/MavLinkTest/main.cpp b/MavLinkCom/MavLinkTest/main.cpp index 03c8165077..4df8aeb24a 100644 --- a/MavLinkCom/MavLinkTest/main.cpp +++ b/MavLinkCom/MavLinkTest/main.cpp @@ -14,7 +14,7 @@ #include #include #include - +#include #include #include #include "UnitTests.h" @@ -139,6 +139,7 @@ std::shared_ptr inLogFile; std::shared_ptr outLogFile; std::thread telemetry_thread; bool telemetry = false; +std::mutex logLock; #if defined(__cpp_lib_experimental_filesystem) @@ -1079,6 +1080,7 @@ int console() { MavLinkStatustext statustext; if (inLogFile != nullptr && inLogFile->isOpen()) { + std::lock_guard lock(logLock); inLogFile->write(message); } switch (message.msgid) { @@ -1178,6 +1180,7 @@ int console() { line = mavlink_utils::Utils::trim(line, ' '); + if (line.length() > 0) { if (line.length() == 0) @@ -1224,6 +1227,16 @@ int console() { { // found it! selected = command; + + if (inLogFile != nullptr && inLogFile->isOpen()) { + auto str = Utils::stringf("cmd:%s", line.c_str()); + MavLinkStatustext st; + strncpy(st.text, str.c_str(), 50); + MavLinkMessage m; + st.encode(m, 0); + std::lock_guard lock(logLock); + inLogFile->write(m); + } break; } } diff --git a/MavLinkCom/include/MavLinkMessageBase.hpp b/MavLinkCom/include/MavLinkMessageBase.hpp index 3bcf564066..63c2d5c0a0 100644 --- a/MavLinkCom/include/MavLinkMessageBase.hpp +++ b/MavLinkCom/include/MavLinkMessageBase.hpp @@ -50,6 +50,9 @@ namespace mavlinkcom // unpack the given message void decode(const MavLinkMessage& msg); + // encode this message into given message buffer + void encode(MavLinkMessage& msg, int seq) const; + // find what type of message this is and decode it on the heap (call delete when you are done with it). static MavLinkMessageBase* lookup(const MavLinkMessage& msg); virtual std::string toJSon() = 0; @@ -97,8 +100,6 @@ namespace mavlinkcom std::string uint16_t_array_tostring(int len, const uint16_t* field); std::string float_array_tostring(int len, const float* field); std::string float_tostring(float value); - - friend class mavlinkcom_impl::MavLinkConnectionImpl; }; // Base class for all strongly typed MavLinkCommand classes defined in MavLinkMessages.hpp diff --git a/MavLinkCom/src/MavLinkMessageBase.cpp b/MavLinkCom/src/MavLinkMessageBase.cpp index 7fc86d4b21..3886721889 100644 --- a/MavLinkCom/src/MavLinkMessageBase.cpp +++ b/MavLinkCom/src/MavLinkMessageBase.cpp @@ -15,6 +15,9 @@ STRICT_MODE_OFF #include "../mavlink/mavlink_helpers.h" STRICT_MODE_ON +static const uint8_t mavlink_message_crcs[256] = MAVLINK_MESSAGE_CRCS; +static const uint8_t mavlink_message_lengths[256] = MAVLINK_MESSAGE_LENGTHS; + using namespace mavlinkcom; void MavLinkMessageBase::decode(const MavLinkMessage& msg) @@ -24,7 +27,39 @@ void MavLinkMessageBase::decode(const MavLinkMessage& msg) unpack(reinterpret_cast(msg.payload64)); } +void MavLinkMessageBase::encode(MavLinkMessage& msg, int seqno) const { + mavlink_message_t& m = reinterpret_cast(msg); + m.magic = MAVLINK_STX; + m.msgid = this->msgid; + m.sysid = this->sysid; + m.compid = this->compid; + m.seq = seqno; + + // pack the payload buffer. + int len = this->pack(reinterpret_cast(m.payload64)); + // calculate checksum + uint8_t crc_extra = mavlink_message_crcs[m.msgid]; + int msglen = mavlink_message_lengths[m.msgid]; + if (m.msgid == MavLinkTelemetry::kMessageId) { + msglen = 28; // mavlink doesn't know about our custom telemetry message. + } + if (len != msglen) { + throw std::runtime_error(Utils::stringf("Message length %d doesn't match expected length%d\n", len, msglen)); + } + m.len = msglen; + m.checksum = crc_calculate((reinterpret_cast(&m)) + 3, MAVLINK_CORE_HEADER_LEN); + crc_accumulate_buffer(&m.checksum, reinterpret_cast(&m.payload64[0]), msglen); +#if MAVLINK_CRC_EXTRA + crc_accumulate(crc_extra, &m.checksum); +#endif + + // these macros use old style cast. + STRICT_MODE_OFF + mavlink_ck_a(&m) = (uint8_t)(m.checksum & 0xFF); + mavlink_ck_b(&m) = (uint8_t)(m.checksum >> 8); + STRICT_MODE_ON +} void MavLinkMessageBase::pack_uint8_t(char* buffer, const uint8_t* field, int offset) const { diff --git a/MavLinkCom/src/impl/MavLinkConnectionImpl.cpp b/MavLinkCom/src/impl/MavLinkConnectionImpl.cpp index 0d2cd55e32..5a70d225a0 100644 --- a/MavLinkCom/src/impl/MavLinkConnectionImpl.cpp +++ b/MavLinkCom/src/impl/MavLinkConnectionImpl.cpp @@ -16,9 +16,6 @@ STRICT_MODE_OFF #include "../mavlink/mavlink_types.h" STRICT_MODE_ON -static const uint8_t mavlink_message_crcs[256] = MAVLINK_MESSAGE_CRCS; -static const uint8_t mavlink_message_lengths[256] = MAVLINK_MESSAGE_LENGTHS; - using namespace mavlink_utils; using namespace mavlinkcom_impl; @@ -193,40 +190,9 @@ void MavLinkConnectionImpl::sendMessage(const MavLinkMessage& msg) void MavLinkConnectionImpl::sendMessage(const MavLinkMessageBase& msg) { - mavlink_message_t m; - m.magic = MAVLINK_STX; - m.msgid = msg.msgid; - m.sysid = msg.sysid; - m.compid = msg.compid; - m.seq = getNextSequence(); - - // pack the payload buffer. - int len = msg.pack(reinterpret_cast(m.payload64)); - - // calculate checksum - uint8_t crc_extra = mavlink_message_crcs[m.msgid]; - int msglen = mavlink_message_lengths[m.msgid]; - if (m.msgid == MavLinkTelemetry::kMessageId) { - msglen = 28; // mavlink doesn't know about our custom telemetry message. - } - if (len != msglen) { - throw std::runtime_error(Utils::stringf("Message length %d doesn't match expected length%d\n", len, msglen)); - } - m.len = msglen; - m.checksum = crc_calculate((reinterpret_cast(&m)) + 3, MAVLINK_CORE_HEADER_LEN); - crc_accumulate_buffer(&m.checksum, reinterpret_cast(&m.payload64[0]), msglen); -#if MAVLINK_CRC_EXTRA - crc_accumulate(crc_extra, &m.checksum); -#endif - - // these macros use old style cast. - STRICT_MODE_OFF - mavlink_ck_a(&m) = (uint8_t)(m.checksum & 0xFF); - mavlink_ck_b(&m) = (uint8_t)(m.checksum >> 8); - STRICT_MODE_ON - - // send the message, now that it is ready - sendMessage(reinterpret_cast(m)); + MavLinkMessage m; + msg.encode(m, getNextSequence()); + sendMessage(m); } int MavLinkConnectionImpl::subscribe(MessageHandler handler)