Add some mavlink telemetry for debugging.

AirLib/include/control/DroneControlBase.hpp

@@ -115,6 +115,8 @@ class DroneControlBase {
     virtual bool setImageForCamera(int camera_id, ImageType type, const vector<uint8_t>& image);
     virtual vector<uint8_t> getImageForCamera(int camera_id, ImageType type);
 
+	// optional telemetry
+	virtual void reportTelemetry(long renderTime) {}
 
     DroneControlBase() = default;
     virtual ~DroneControlBase() = default;

AirLib/include/control/MavLinkDroneControl.hpp

@@ -54,6 +54,9 @@ class MavLinkDroneControl : public DroneControlBase {
     float getTakeoffZ() override;
     float getDistanceAccuracy() override;
 
+	// optional telemetry
+	void reportTelemetry(long renderTime) override;
+
 protected:  //keep low level commands hidden from outside as they have no safety checks
     void commandRollPitchZ(float pitch, float roll, float z, float yaw) override;
     void commandVelocity(float vx, float vy, float vz, const YawMode& yaw_mode) override;

AirLib/include/control/MavLinkHelper.h

@@ -81,6 +81,7 @@ class MavLinkHelper : public ControllerBase
     virtual void update(real_T dt) override;
     //*** End: UpdatableState implementation ***//
 
+	void reportTelemetry(float renderTime);
     struct impl;
     std::unique_ptr<impl> pimpl_;
 };

AirLib/src/control/MavLinkDroneControl.cpp

@@ -39,6 +39,16 @@ MavLinkDroneControl::MavLinkDroneControl(int local_system_id, int local_componen
     drone_->connect(connection);
 }
 
+void MavLinkDroneControl::reportTelemetry(long renderTime)
+{
+	MavLinkTelemetry telemetry;
+	auto con = drone_->getConnection();
+	if (con != nullptr) {
+		con->getTelemetry(telemetry);
+		telemetry.renderTime = renderTime;				
+		drone_->sendMessage(telemetry);
+	}
+}
 
 shared_ptr<mavlinkcom::MavLinkConnection> MavLinkDroneControl::createConnection(const Parameters& params)
 {

AirLib/src/control/MavLinkHelper.cpp

@@ -654,6 +654,16 @@ struct MavLinkHelper::impl {
         }
     }
 
+	void reportTelemetry(float renderTime)
+	{
+		if (logviewer_proxy_ == nullptr || connection_ == nullptr) {
+			return;
+		}
+		MavLinkTelemetry data;
+		connection_->getTelemetry(data);
+		data.renderTime = static_cast<long>(renderTime * 1000000);// microseconds
+		logviewer_proxy_->sendMessage(data);
+	}
 };
 
 //empty constructor required for pimpl
@@ -757,7 +767,6 @@ DroneControlBase* MavLinkHelper::createOrGetDroneControl()
     return pimpl_->createOrGetDroneControl();
 }
 
-
 //*** Start: UpdatableState implementation ***//
 void MavLinkHelper::reset()
 {
@@ -769,5 +778,10 @@ void MavLinkHelper::update(real_T dt)
 }
 //*** End: UpdatableState implementation ***//
 
+void MavLinkHelper::reportTelemetry(float renderTime)
+{
+	return pimpl_->reportTelemetry(renderTime);
+}
+
 }} //namespace
 #endif

LogViewer/Networking/Mavlink/MavlinkChannel.cs

@@ -179,6 +179,21 @@ public class MavlinkChannel
         IPort port;
         byte seqno;
 
+        public MavlinkChannel()
+        {
+            // plug in our custom mavlink_simulator_telemetry message
+            int id = MAVLink.mavlink_telemetry.MessageId;
+            Type info = MAVLink.MAVLINK_MESSAGE_INFO[id];
+            if (info != null && typeof(MAVLink.mavlink_telemetry) != info)
+            {
+                throw new Exception("The custom messageid " + id + " is already defined, so we can't use it for mavlink_simulator_telemetry");
+            }
+            else
+            {
+                MAVLink.MAVLINK_MESSAGE_INFO[id] = typeof(MAVLink.mavlink_telemetry);
+            }
+        }
+
         public void Start(IPort port)
         {
             this.port = port;

LogViewer/Networking/Mavlink/MavlinkTypes.cs

@@ -5816,5 +5816,21 @@ public struct mavlink_debug_t
 
         };
 
+
+
+        // custom message from the simulator 
+        [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 24)]
+        public struct mavlink_telemetry
+        {
+            public const int MessageId = 204;
+            public int messagesSent;       // number of messages sent since the last telemetry message
+            public int messagesReceived;   // number of messages received since the last telemetry message
+            public int messagesHandled;    // number of messages handled since the last telemetry message
+            public int crcErrors;          // # crc errors detected in mavlink stream since the last telemetry message
+            public int handlerMicroseconds; // total time spent in the handlers in microseconds since the last telemetry message
+            public int renderTime;          // total time spent rendering frames since the last message
+        };
+
+
     }
 }

MavLinkCom/include/MavLinkConnection.hpp

@@ -111,6 +111,11 @@ namespace mavlinkcom {
         // Send the given already encoded message, assuming the compid and sysid have been set by the caller.
         void sendMessage(const MavLinkMessage& msg);
 
+		// get the next telemetry snapshot, then clear the internal counters and start over.  This way each snapshot
+		// gives you a picture of what happened in whatever timeslice you decide to call this method.  This is packaged
+		// in a mavlink message so you can easily send it to the LogViewer.
+		void getTelemetry(MavLinkTelemetry& result);
+
     protected:
         void startListening(const std::string& nodeName, std::shared_ptr<Port> connectedPort);
 

MavLinkCom/include/MavLinkMessageBase.hpp

@@ -104,6 +104,24 @@ namespace mavlinkcom
 
 		friend class mavlinkcom_impl::MavLinkNodeImpl;
 	};
+
+
+	// The location of a landing area captured from a downward facing camera
+	class MavLinkTelemetry : public MavLinkMessageBase {
+	public:
+		const static uint8_t kMessageId = 204; // in the user range 180-229.
+		MavLinkTelemetry() { msgid = kMessageId; }
+		long messagesSent;		 // number of messages sent since the last telemetry message
+		long messagesReceived;	 // number of messages received since the last telemetry message
+		long messagesHandled;	 // number of messages handled since the last telemetry message
+		long crcErrors;			 // # crc errors detected in mavlink stream since the last telemetry message
+		long handlerMicroseconds; // total time spent in the handlers in microseconds since the last telemetry message
+		long renderTime;         // total time spent rendering frames since the last telemetry message
+	protected:
+		virtual int pack(char* buffer) const;
+		virtual int unpack(const char* buffer);
+	};
+
 }
 
 #endif

MavLinkCom/src/MavLinkConnection.cpp

@@ -109,5 +109,14 @@ void MavLinkConnection::join(std::shared_ptr<MavLinkConnection> remote, bool sub
 	pImpl->join(remote);
 }
 
+// get the next telemetry snapshot, then clear the internal counters and start over.  This way each snapshot
+// gives you a picture of what happened in whatever timeslice you decide to call this method.
+void MavLinkConnection::getTelemetry(MavLinkTelemetry& result)
+{
+	pImpl->getTelemetry(result);
+}
+
+
+
 //MavLinkConnection::MavLinkConnection(MavLinkConnection&&) = default;
 //MavLinkConnection& MavLinkConnection::operator=(MavLinkConnection&&) = default;

MavLinkCom/src/MavLinkMessageBase.cpp

@@ -217,3 +217,23 @@ void MavLinkMessageBase::unpack_float_array(int len, const char* buffer, float*
 		field++;
 	}
 }
+
+int MavLinkTelemetry::pack(char* buffer) const {
+	pack_int32_t(buffer, reinterpret_cast<const int32_t*>(&this->messagesSent), 0);
+	pack_int32_t(buffer, reinterpret_cast<const int32_t*>(&this->messagesReceived), 4);
+	pack_int32_t(buffer, reinterpret_cast<const int32_t*>(&this->messagesHandled), 8);
+	pack_int32_t(buffer, reinterpret_cast<const int32_t*>(&this->crcErrors), 12);
+	pack_int32_t(buffer, reinterpret_cast<const int32_t*>(&this->handlerMicroseconds), 16);
+	pack_int32_t(buffer, reinterpret_cast<const int32_t*>(&this->renderTime), 20);
+	return 24;
+}
+
+int MavLinkTelemetry::unpack(const char* buffer) {
+	unpack_int32_t(buffer, reinterpret_cast<int32_t*>(&this->messagesSent), 0);
+	unpack_int32_t(buffer, reinterpret_cast<int32_t*>(&this->messagesReceived), 4);
+	unpack_int32_t(buffer, reinterpret_cast<int32_t*>(&this->messagesHandled), 8);
+	unpack_int32_t(buffer, reinterpret_cast<int32_t*>(&this->crcErrors), 12);
+	unpack_int32_t(buffer, reinterpret_cast<int32_t*>(&this->handlerMicroseconds), 16);
+	unpack_int32_t(buffer, reinterpret_cast<int32_t*>(&this->renderTime), 20);
+	return 24;
+}

MavLinkCom/src/impl/MavLinkConnectionImpl.cpp

@@ -24,6 +24,13 @@ using namespace mavlinkcom_impl;
 
 MavLinkConnectionImpl::MavLinkConnectionImpl()
 {
+	// add our custom telemetry message length.
+	telemetry_.crcErrors = 0;
+	telemetry_.handlerMicroseconds = 0;
+	telemetry_.messagesHandled = 0;
+	telemetry_.messagesReceived = 0;
+	telemetry_.messagesSent = 0;
+	telemetry_.renderTime = 0;
 	closed = true;
 }
 std::string MavLinkConnectionImpl::getName() {
@@ -156,10 +163,16 @@ void MavLinkConnectionImpl::sendMessage(const MavLinkMessage& msg)
 		{
 			sendLog_->write(msg);
 		}
-		const mavlink_message_t& m = reinterpret_cast<const mavlink_message_t&>(msg);
-		std::lock_guard<std::mutex> guard(buffer_mutex);
-		unsigned len = mavlink_msg_to_send_buffer(message_buf, &m);
-		port->write(message_buf, len);
+		{
+			const mavlink_message_t& m = reinterpret_cast<const mavlink_message_t&>(msg);
+			std::lock_guard<std::mutex> guard(buffer_mutex);
+			unsigned len = mavlink_msg_to_send_buffer(message_buf, &m);
+			port->write(message_buf, len);
+		}
+		{
+			std::lock_guard<std::mutex> guard(telemetry_mutex_);
+			telemetry_.messagesSent++;
+		}
 	}
 }
 
@@ -178,6 +191,9 @@ void MavLinkConnectionImpl::sendMessage(const MavLinkMessageBase& msg)
 	// calculate checksum
 	uint8_t crc_extra = mavlink_message_crcs[m.msgid];
 	int msglen = mavlink_message_lengths[m.msgid];
+	if (m.msgid == MavLinkTelemetry::kMessageId) {
+		msglen = 24; // 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));
 	}
@@ -266,7 +282,8 @@ void MavLinkConnectionImpl::readPackets()
 		int count = safePort->read(buffer, MAXBUFFER);
 		if (count <= 0) {
 			// error, so just try again...
-			mavlink_errors_++;
+			std::lock_guard<std::mutex> guard(telemetry_mutex_);
+			telemetry_.crcErrors++;
 			continue;
 		}
 		for (int i = 0; i < count; i++)
@@ -277,9 +294,8 @@ void MavLinkConnectionImpl::readPackets()
 				continue;
 			}
 			else if (frame_state == MAVLINK_FRAMING_BAD_CRC) {
-				mavlink_errors_++;
-				Utils::logError(Utils::stringf("MavLink message %d, seqno %x has bad CRC %x received, expecting %x!", 
-					static_cast<int>(msgBuffer.msgid), static_cast<int>(msgBuffer.seq), static_cast<int>(buffer[i]), static_cast<int>(msgBuffer.checksum)).c_str());
+				std::lock_guard<std::mutex> guard(telemetry_mutex_);
+				telemetry_.crcErrors++;
 			}
 			else if (frame_state == MAVLINK_FRAMING_OK)
 			{
@@ -293,24 +309,24 @@ void MavLinkConnectionImpl::readPackets()
 
 				if (con_ != nullptr && !closed)
 				{
+					{
+						std::lock_guard<std::mutex> guard(telemetry_mutex_);
+						telemetry_.messagesReceived++;
+					}
 					// queue event for publishing.
                     {
                         std::lock_guard<std::mutex> guard(msg_queue_mutex_);
                         MavLinkMessage& message = reinterpret_cast<MavLinkMessage&>(msg);
                         msg_queue_.push(message);
-                        size_t size = msg_queue_.size();
-                        if (size > max_queue_length_) {
-                            max_queue_length_ = size;
-                        }
                     }
 					if (waiting_for_msg_) {
 						msg_available_.post();
 					}
 				}
 			}
 			else {
-				mavlink_errors_++;
-				Utils::logError("Unknown frame_state %d", frame_state);
+				std::lock_guard<std::mutex> guard(telemetry_mutex_);
+				telemetry_.crcErrors++;
 			}
 		}	
 
@@ -351,6 +367,8 @@ void MavLinkConnectionImpl::drainQueue()
 			snapshot_stale = false;
 		}
 		auto end = snapshot.end();
+
+		auto startTime = std::chrono::system_clock::now();
 		std::shared_ptr<MavLinkConnection> sharedPtr = std::shared_ptr<MavLinkConnection>(this->con_);
 		for (auto ptr = snapshot.begin(); ptr != end; ptr++)
 		{
@@ -361,6 +379,15 @@ void MavLinkConnectionImpl::drainQueue()
 				Utils::logError("MavLinkConnection subscriber threw exception: %s", e.what());
 			}
 		}
+
+		{
+			auto endTime = std::chrono::system_clock::now();
+			auto diff = endTime - startTime;
+			long microseconds = static_cast<long>(std::chrono::duration_cast<std::chrono::microseconds>(diff).count());
+			std::lock_guard<std::mutex> guard(telemetry_mutex_);
+			telemetry_.messagesHandled++;
+			telemetry_.handlerMicroseconds += microseconds;
+		}
 	}
 }
 
@@ -376,3 +403,16 @@ void MavLinkConnectionImpl::publishPackets()
 	}
 }
 
+
+void MavLinkConnectionImpl::getTelemetry(MavLinkTelemetry& result)
+{
+	std::lock_guard<std::mutex> guard(telemetry_mutex_);
+	result = telemetry_;
+	// reset counters 
+	telemetry_.crcErrors = 0;
+	telemetry_.handlerMicroseconds = 0;
+	telemetry_.messagesHandled = 0;
+	telemetry_.messagesReceived = 0;
+	telemetry_.messagesSent = 0;
+	telemetry_.renderTime = 0;
+}

MavLinkCom/src/impl/MavLinkConnectionImpl.hpp

@@ -18,6 +18,7 @@ using namespace mavlinkcom;
 
 namespace mavlinkcom_impl {
 
+	// See MavLinkConnection.hpp for definitions of these methods.
 	class MavLinkConnectionImpl
 	{
 	public:
@@ -43,6 +44,8 @@ namespace mavlinkcom_impl {
 		void unsubscribe(int id);		
 		uint8_t getNextSequence();
 		void join(std::shared_ptr<MavLinkConnection> remote, bool subscribeToLeft = true, bool subscribeToRight = true);
+		void getTelemetry(MavLinkTelemetry& result);
+
 	private:
 		static std::shared_ptr<MavLinkConnection> createConnection(const std::string& nodeName, Port* port);
         void joinLeftSubscriber(std::shared_ptr<MavLinkConnection> remote, std::shared_ptr<MavLinkConnection>con, const MavLinkMessage& msg);
@@ -78,8 +81,8 @@ namespace mavlinkcom_impl {
 		std::mutex msg_queue_mutex_;
 		MavLinkSemaphore msg_available_;
 		bool waiting_for_msg_ = false;
-		size_t max_queue_length_ = 0;
-		long mavlink_errors_ = 0;
+		std::mutex telemetry_mutex_;
+		MavLinkTelemetry telemetry_;
 	};
 }
 

Unreal/Plugins/AirSim/Source/MavMultiRotor.cpp

@@ -143,8 +143,20 @@ void MavMultiRotor::updateRenderedState()
 	mav_.getStatusMessages(mav_messages_);
 }
 
-void MavMultiRotor::updateRendering()
+void MavMultiRotor::updateRendering(float dt)
 {
+	if (total_ == 0) {
+		real_T total_ = 0;
+		telemetry_start_ = std::chrono::system_clock::now();
+	}
+	total_ += dt;
+	auto now = std::chrono::system_clock::now();
+	if (std::chrono::duration<double>(now - telemetry_start_).count() > kTelemetryInterval)
+	{ 
+		mav_.reportTelemetry(total_);
+		total_ = 0;
+		telemetry_start_ = now;
+	}
 	//update rotor animations
 	for (unsigned int i = 0; i < vehicle_.vertexCount(); ++i) {
 		vehicle_pawn_->setRotorSpeed(i, rotor_speeds_[i] * rotor_directions_[i]);