From 03412a5079db9bcc927ede6da4acc878416e0090 Mon Sep 17 00:00:00 2001 From: Chris Lovett Date: Tue, 21 Feb 2017 02:22:29 -0800 Subject: [PATCH] Add some mavlink telemetry for debugging. --- AirLib/include/control/DroneControlBase.hpp | 2 + .../include/control/MavLinkDroneControl.hpp | 3 + AirLib/include/control/MavLinkHelper.h | 1 + AirLib/src/control/MavLinkDroneControl.cpp | 10 +++ AirLib/src/control/MavLinkHelper.cpp | 16 ++++- .../Networking/Mavlink/MavlinkChannel.cs | 15 ++++ LogViewer/Networking/Mavlink/MavlinkTypes.cs | 16 +++++ MavLinkCom/include/MavLinkConnection.hpp | 5 ++ MavLinkCom/include/MavLinkMessageBase.hpp | 18 +++++ MavLinkCom/src/MavLinkConnection.cpp | 9 +++ MavLinkCom/src/MavLinkMessageBase.cpp | 20 ++++++ MavLinkCom/src/impl/MavLinkConnectionImpl.cpp | 68 +++++++++++++++---- MavLinkCom/src/impl/MavLinkConnectionImpl.hpp | 7 +- .../Plugins/AirSim/Source/MavMultiRotor.cpp | 14 +++- Unreal/Plugins/AirSim/Source/MavMultiRotor.h | 8 ++- .../AirSim/Source/SimModeWorldBase.cpp | 2 +- Unreal/Plugins/AirSim/Source/VehicleBase.h | 2 +- docs/install_boost.md | 2 + 18 files changed, 195 insertions(+), 23 deletions(-) diff --git a/AirLib/include/control/DroneControlBase.hpp b/AirLib/include/control/DroneControlBase.hpp index 3bd3e95ab4..9a173b988a 100644 --- a/AirLib/include/control/DroneControlBase.hpp +++ b/AirLib/include/control/DroneControlBase.hpp @@ -115,6 +115,8 @@ class DroneControlBase { virtual bool setImageForCamera(int camera_id, ImageType type, const vector& image); virtual vector getImageForCamera(int camera_id, ImageType type); + // optional telemetry + virtual void reportTelemetry(long renderTime) {} DroneControlBase() = default; virtual ~DroneControlBase() = default; diff --git a/AirLib/include/control/MavLinkDroneControl.hpp b/AirLib/include/control/MavLinkDroneControl.hpp index bf3ff78c84..bdf19d6804 100644 --- a/AirLib/include/control/MavLinkDroneControl.hpp +++ b/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; diff --git a/AirLib/include/control/MavLinkHelper.h b/AirLib/include/control/MavLinkHelper.h index 8fe8565775..02e48df9b2 100644 --- a/AirLib/include/control/MavLinkHelper.h +++ b/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 pimpl_; }; diff --git a/AirLib/src/control/MavLinkDroneControl.cpp b/AirLib/src/control/MavLinkDroneControl.cpp index d5b9f11a5c..0153eb0628 100644 --- a/AirLib/src/control/MavLinkDroneControl.cpp +++ b/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 MavLinkDroneControl::createConnection(const Parameters& params) { diff --git a/AirLib/src/control/MavLinkHelper.cpp b/AirLib/src/control/MavLinkHelper.cpp index ee707b95a4..a5f04ff0a1 100644 --- a/AirLib/src/control/MavLinkHelper.cpp +++ b/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(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 diff --git a/LogViewer/Networking/Mavlink/MavlinkChannel.cs b/LogViewer/Networking/Mavlink/MavlinkChannel.cs index a10d53817c..25459a78f9 100644 --- a/LogViewer/Networking/Mavlink/MavlinkChannel.cs +++ b/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; diff --git a/LogViewer/Networking/Mavlink/MavlinkTypes.cs b/LogViewer/Networking/Mavlink/MavlinkTypes.cs index 42715a1923..e0abf8f3c8 100644 --- a/LogViewer/Networking/Mavlink/MavlinkTypes.cs +++ b/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 + }; + + } } \ No newline at end of file diff --git a/MavLinkCom/include/MavLinkConnection.hpp b/MavLinkCom/include/MavLinkConnection.hpp index 42705eadb7..f21b2f9e15 100644 --- a/MavLinkCom/include/MavLinkConnection.hpp +++ b/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 connectedPort); diff --git a/MavLinkCom/include/MavLinkMessageBase.hpp b/MavLinkCom/include/MavLinkMessageBase.hpp index 3e2a2c904c..3dd9bece25 100644 --- a/MavLinkCom/include/MavLinkMessageBase.hpp +++ b/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 diff --git a/MavLinkCom/src/MavLinkConnection.cpp b/MavLinkCom/src/MavLinkConnection.cpp index 6b0929c3e6..56f140a980 100644 --- a/MavLinkCom/src/MavLinkConnection.cpp +++ b/MavLinkCom/src/MavLinkConnection.cpp @@ -109,5 +109,14 @@ void MavLinkConnection::join(std::shared_ptr 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; diff --git a/MavLinkCom/src/MavLinkMessageBase.cpp b/MavLinkCom/src/MavLinkMessageBase.cpp index 220264961a..445e70ee47 100644 --- a/MavLinkCom/src/MavLinkMessageBase.cpp +++ b/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(&this->messagesSent), 0); + pack_int32_t(buffer, reinterpret_cast(&this->messagesReceived), 4); + pack_int32_t(buffer, reinterpret_cast(&this->messagesHandled), 8); + pack_int32_t(buffer, reinterpret_cast(&this->crcErrors), 12); + pack_int32_t(buffer, reinterpret_cast(&this->handlerMicroseconds), 16); + pack_int32_t(buffer, reinterpret_cast(&this->renderTime), 20); + return 24; +} + +int MavLinkTelemetry::unpack(const char* buffer) { + unpack_int32_t(buffer, reinterpret_cast(&this->messagesSent), 0); + unpack_int32_t(buffer, reinterpret_cast(&this->messagesReceived), 4); + unpack_int32_t(buffer, reinterpret_cast(&this->messagesHandled), 8); + unpack_int32_t(buffer, reinterpret_cast(&this->crcErrors), 12); + unpack_int32_t(buffer, reinterpret_cast(&this->handlerMicroseconds), 16); + unpack_int32_t(buffer, reinterpret_cast(&this->renderTime), 20); + return 24; +} \ No newline at end of file diff --git a/MavLinkCom/src/impl/MavLinkConnectionImpl.cpp b/MavLinkCom/src/impl/MavLinkConnectionImpl.cpp index eddf4e7369..a3c8a1d2d2 100644 --- a/MavLinkCom/src/impl/MavLinkConnectionImpl.cpp +++ b/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(msg); - std::lock_guard 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(msg); + std::lock_guard guard(buffer_mutex); + unsigned len = mavlink_msg_to_send_buffer(message_buf, &m); + port->write(message_buf, len); + } + { + std::lock_guard 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 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(msgBuffer.msgid), static_cast(msgBuffer.seq), static_cast(buffer[i]), static_cast(msgBuffer.checksum)).c_str()); + std::lock_guard guard(telemetry_mutex_); + telemetry_.crcErrors++; } else if (frame_state == MAVLINK_FRAMING_OK) { @@ -293,15 +309,15 @@ void MavLinkConnectionImpl::readPackets() if (con_ != nullptr && !closed) { + { + std::lock_guard guard(telemetry_mutex_); + telemetry_.messagesReceived++; + } // queue event for publishing. { std::lock_guard guard(msg_queue_mutex_); MavLinkMessage& message = reinterpret_cast(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(); @@ -309,8 +325,8 @@ void MavLinkConnectionImpl::readPackets() } } else { - mavlink_errors_++; - Utils::logError("Unknown frame_state %d", frame_state); + std::lock_guard 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 sharedPtr = std::shared_ptr(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(std::chrono::duration_cast(diff).count()); + std::lock_guard guard(telemetry_mutex_); + telemetry_.messagesHandled++; + telemetry_.handlerMicroseconds += microseconds; + } } } @@ -376,3 +403,16 @@ void MavLinkConnectionImpl::publishPackets() } } + +void MavLinkConnectionImpl::getTelemetry(MavLinkTelemetry& result) +{ + std::lock_guard 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; +} diff --git a/MavLinkCom/src/impl/MavLinkConnectionImpl.hpp b/MavLinkCom/src/impl/MavLinkConnectionImpl.hpp index 02448fee22..2dba375264 100644 --- a/MavLinkCom/src/impl/MavLinkConnectionImpl.hpp +++ b/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 remote, bool subscribeToLeft = true, bool subscribeToRight = true); + void getTelemetry(MavLinkTelemetry& result); + private: static std::shared_ptr createConnection(const std::string& nodeName, Port* port); void joinLeftSubscriber(std::shared_ptr remote, std::shared_ptrcon, 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_; }; } diff --git a/Unreal/Plugins/AirSim/Source/MavMultiRotor.cpp b/Unreal/Plugins/AirSim/Source/MavMultiRotor.cpp index 758ddf0a3e..085c128ab5 100644 --- a/Unreal/Plugins/AirSim/Source/MavMultiRotor.cpp +++ b/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(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]); diff --git a/Unreal/Plugins/AirSim/Source/MavMultiRotor.h b/Unreal/Plugins/AirSim/Source/MavMultiRotor.h index 68a2390210..633a173db2 100644 --- a/Unreal/Plugins/AirSim/Source/MavMultiRotor.h +++ b/Unreal/Plugins/AirSim/Source/MavMultiRotor.h @@ -10,7 +10,7 @@ #include "control/DroneControlBase.hpp" #include "VehicleBase.h" #include "FlyingPawn.h" - +#include class MavMultiRotor : public VehicleBase { @@ -41,7 +41,7 @@ class MavMultiRotor : public VehicleBase virtual void beginPlay() override; virtual void endPlay() override; virtual void updateRenderedState() override; - virtual void updateRendering() override; + virtual void updateRendering(float dt) override; //PhysicsBody interface //this just wrapped around MultiRotor physics body @@ -64,7 +64,9 @@ class MavMultiRotor : public VehicleBase std::vector mav_messages_; msr::airlib::Environment environment_; AFlyingPawn* vehicle_pawn_; - + real_T total_ = 0; + std::chrono::time_point telemetry_start_; + const real_T kTelemetryInterval = 0.1f; real_T rotor_speeds_[4]; int rotor_directions_[4]; real_T rotor_thrusts_[4]; diff --git a/Unreal/Plugins/AirSim/Source/SimModeWorldBase.cpp b/Unreal/Plugins/AirSim/Source/SimModeWorldBase.cpp index c3e090fea9..66be01e190 100644 --- a/Unreal/Plugins/AirSim/Source/SimModeWorldBase.cpp +++ b/Unreal/Plugins/AirSim/Source/SimModeWorldBase.cpp @@ -68,7 +68,7 @@ void ASimModeWorldBase::Tick(float DeltaSeconds) //perfom any expensive rendering update outside of lock region for (auto& vehicle : vehicles_) - vehicle->updateRendering(); + vehicle->updateRendering(DeltaSeconds); Super::Tick(DeltaSeconds); } diff --git a/Unreal/Plugins/AirSim/Source/VehicleBase.h b/Unreal/Plugins/AirSim/Source/VehicleBase.h index 802431514f..2842a2a215 100644 --- a/Unreal/Plugins/AirSim/Source/VehicleBase.h +++ b/Unreal/Plugins/AirSim/Source/VehicleBase.h @@ -16,5 +16,5 @@ class VehicleBase : public msr::airlib::UpdatableObject //called when physics gets updated (must be fast, avoid rendering) virtual void updateRenderedState() = 0; //called when render changes are required - virtual void updateRendering() = 0; + virtual void updateRendering(float dt) = 0; }; diff --git a/docs/install_boost.md b/docs/install_boost.md index 5148d1878a..77cdda9ad9 100644 --- a/docs/install_boost.md +++ b/docs/install_boost.md @@ -22,3 +22,5 @@ cd boost_1_63_0 ./bootstrap.sh ./b2 variant=release link=static runtime-link=shared threading=multi ```` + +If you want to debug the boost binaries use variant=debug instead. \ No newline at end of file