Add mavlinktest commands into the mavlink log generated.

MavLinkCom/MavLinkTest/main.cpp

@@ -14,7 +14,7 @@
 #include <vector>
 #include <string.h>
 #include <functional>
-
+#include <mutex>
 #include <map>
 #include <ctime>
 #include "UnitTests.h"
@@ -139,6 +139,7 @@ std::shared_ptr<MavLinkLog> inLogFile;
 std::shared_ptr<MavLinkLog> 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<std::mutex> 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<std::mutex> lock(logLock);
+							inLogFile->write(m);
+						}
 						break;
 					}
 				}

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

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<const char*>(msg.payload64));
 
 }
+void MavLinkMessageBase::encode(MavLinkMessage& msg, int seqno) const {
+	mavlink_message_t& m = reinterpret_cast<mavlink_message_t&>(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<char*>(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<const uint8_t*>(&m)) + 3, MAVLINK_CORE_HEADER_LEN);
+	crc_accumulate_buffer(&m.checksum, reinterpret_cast<const char*>(&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
 {

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<char*>(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<const uint8_t*>(&m)) + 3, MAVLINK_CORE_HEADER_LEN);
-	crc_accumulate_buffer(&m.checksum, reinterpret_cast<const char*>(&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<const MavLinkMessage&>(m));
+	MavLinkMessage m;
+	msg.encode(m, getNextSequence());
+	sendMessage(m);
 }
 
 int MavLinkConnectionImpl::subscribe(MessageHandler handler)