From de44dba05dcc3cb2a996e632f4da2b74c2d2bacc Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Thu, 12 Nov 2020 16:21:52 +0100 Subject: [PATCH] mavlink: add events interface protocol --- .gitmodules | 3 + QGCExternalLibs.pri | 13 +++ libs/libevents/definitions.cpp | 27 +++++++ libs/libevents/libevents | 1 + libs/libevents/libevents_definitions.h | 37 +++++++++ libs/mavlink/include/mavlink/v2.0 | 2 +- qgroundcontrol.pro | 6 ++ src/Vehicle/CompInfoEvents.cc | 23 ++++++ src/Vehicle/CompInfoEvents.h | 31 +++++++ src/Vehicle/ComponentInformationManager.cc | 33 ++++++-- src/Vehicle/ComponentInformationManager.h | 9 ++- src/Vehicle/EventHandler.cc | 104 ++++++++++++++++++++++++ src/Vehicle/EventHandler.h | 53 ++++++++++++ src/Vehicle/HealthAndArmingChecks.cc | 125 +++++++++++++++++++++++++++++ src/Vehicle/HealthAndArmingChecks.h | 84 +++++++++++++++++++ src/Vehicle/Vehicle.cc | 90 +++++++++++++++++++++ src/Vehicle/Vehicle.h | 15 ++++ 17 files changed, 646 insertions(+), 10 deletions(-) create mode 100644 libs/libevents/definitions.cpp create mode 160000 libs/libevents/libevents create mode 100644 libs/libevents/libevents_definitions.h create mode 100644 src/Vehicle/CompInfoEvents.cc create mode 100644 src/Vehicle/CompInfoEvents.h create mode 100644 src/Vehicle/EventHandler.cc create mode 100644 src/Vehicle/EventHandler.h create mode 100644 src/Vehicle/HealthAndArmingChecks.cc create mode 100644 src/Vehicle/HealthAndArmingChecks.h diff --git a/.gitmodules b/.gitmodules index c41ef18..8b3046b 100644 --- a/.gitmodules +++ b/.gitmodules @@ -13,3 +13,6 @@ [submodule "libs/xz-embedded"] path = libs/xz-embedded url = https://github.com/Auterion/xz-embedded.git +[submodule "libs/libevents/libevents"] + path = libs/libevents/libevents + url = https://github.com/mavlink/libevents.git diff --git a/QGCExternalLibs.pri b/QGCExternalLibs.pri index bda5a64..283d005 100644 --- a/QGCExternalLibs.pri +++ b/QGCExternalLibs.pri @@ -102,6 +102,19 @@ INCLUDEPATH += libs/eigen DEFINES += NOMINMAX # +# [REQUIRED] Events submodule +HEADERS+= \ + libs/libevents/libevents/libs/cpp/protocol/receive.h \ + libs/libevents/libevents/libs/cpp/parse/parser.h \ + libs/libevents/libevents/libs/cpp/generated/events_generated.h \ + libs/libevents/libevents_definitions.h +SOURCES += \ + libs/libevents/libevents/libs/cpp/protocol/receive.cpp \ + libs/libevents/libevents/libs/cpp/parse/parser.cpp \ + libs/libevents/definitions.cpp +INCLUDEPATH += libs/libevents + +# # [REQUIRED] shapelib library INCLUDEPATH += libs/shapelib SOURCES += \ diff --git a/libs/libevents/definitions.cpp b/libs/libevents/definitions.cpp new file mode 100644 index 0000000..fc06c7b --- /dev/null +++ b/libs/libevents/definitions.cpp @@ -0,0 +1,27 @@ +/**************************************************************************** + * + * (c) 2020 QGROUNDCONTROL PROJECT + * + * QGroundControl is licensed according to the terms in the file + * COPYING.md in the root of the source code directory. + * + ****************************************************************************/ + +#include "libevents_definitions.h" + +#include + +QGC_LOGGING_CATEGORY(EventsLog, "EventsLog"); + +void qgc_events_parser_debug_printf(const char *fmt, ...) { + char msg[256]; + va_list argptr; + va_start(argptr, fmt); + vsnprintf(msg, sizeof(msg), fmt, argptr); + va_end(argptr); + msg[sizeof(msg)-1] = '\0'; + int len = strlen(msg); + if (len > 0) msg[len-1] = '\0'; // remove newline + qCDebug(EventsLog) << msg; +} + diff --git a/libs/libevents/libevents b/libs/libevents/libevents new file mode 160000 index 0000000..b3df80a --- /dev/null +++ b/libs/libevents/libevents @@ -0,0 +1 @@ +Subproject commit b3df80adf5e9a1ffd3520a699d751acddd07763c diff --git a/libs/libevents/libevents_definitions.h b/libs/libevents/libevents_definitions.h new file mode 100644 index 0000000..d84bb91 --- /dev/null +++ b/libs/libevents/libevents_definitions.h @@ -0,0 +1,37 @@ +/**************************************************************************** + * + * (c) 2020 QGROUNDCONTROL PROJECT + * + * QGroundControl is licensed according to the terms in the file + * COPYING.md in the root of the source code directory. + * + ****************************************************************************/ + +/* + * This header defines the events::EventType type. + * + */ + +#pragma once + +#include +#include + +#include + +Q_DECLARE_LOGGING_CATEGORY(EventsLog) + +void qgc_events_parser_debug_printf(const char *fmt, ...); + +//#define LIBEVENTS_PARSER_DEBUG_PRINTF qgc_events_parser_debug_printf +#define LIBEVENTS_DEBUG_PRINTF qgc_events_parser_debug_printf + +#include "MAVLinkProtocol.h" + +namespace events +{ +using EventType = mavlink_event_t; +} // namespace events + + + diff --git a/libs/mavlink/include/mavlink/v2.0 b/libs/mavlink/include/mavlink/v2.0 index 9e07c7d..aef7041 160000 --- a/libs/mavlink/include/mavlink/v2.0 +++ b/libs/mavlink/include/mavlink/v2.0 @@ -1 +1 @@ -Subproject commit 9e07c7d0b6eb91f0fd84f911dc91e68e865ba7ed +Subproject commit aef704108f3c1e7520338c34171c28565211a607 diff --git a/qgroundcontrol.pro b/qgroundcontrol.pro index df317b0..49df025 100644 --- a/qgroundcontrol.pro +++ b/qgroundcontrol.pro @@ -684,12 +684,15 @@ HEADERS += \ src/Terrain/TerrainQuery.h \ src/TerrainTile.h \ src/Vehicle/CompInfo.h \ + src/Vehicle/CompInfoEvents.h \ src/Vehicle/CompInfoParam.h \ src/Vehicle/CompInfoGeneral.h \ src/Vehicle/ComponentInformationCache.h \ src/Vehicle/ComponentInformationManager.h \ + src/Vehicle/EventHandler.h \ src/Vehicle/FTPManager.h \ src/Vehicle/GPSRTKFactGroup.h \ + src/Vehicle/HealthAndArmingChecks.h \ src/Vehicle/InitialConnectStateMachine.h \ src/Vehicle/MAVLinkLogManager.h \ src/Vehicle/MAVLinkStreamConfig.h \ @@ -919,12 +922,15 @@ SOURCES += \ src/Terrain/TerrainQuery.cc \ src/TerrainTile.cc\ src/Vehicle/CompInfo.cc \ + src/Vehicle/CompInfoEvents.cc \ src/Vehicle/CompInfoParam.cc \ src/Vehicle/CompInfoGeneral.cc \ src/Vehicle/ComponentInformationCache.cc \ src/Vehicle/ComponentInformationManager.cc \ + src/Vehicle/EventHandler.cc \ src/Vehicle/FTPManager.cc \ src/Vehicle/GPSRTKFactGroup.cc \ + src/Vehicle/HealthAndArmingChecks.cc \ src/Vehicle/InitialConnectStateMachine.cc \ src/Vehicle/MAVLinkLogManager.cc \ src/Vehicle/MAVLinkStreamConfig.cc \ diff --git a/src/Vehicle/CompInfoEvents.cc b/src/Vehicle/CompInfoEvents.cc new file mode 100644 index 0000000..348f42c --- /dev/null +++ b/src/Vehicle/CompInfoEvents.cc @@ -0,0 +1,23 @@ +/**************************************************************************** + * + * (c) 2020 QGROUNDCONTROL PROJECT + * + * QGroundControl is licensed according to the terms in the file + * COPYING.md in the root of the source code directory. + * + ****************************************************************************/ + +#include "CompInfoEvents.h" +#include "Vehicle.h" + +CompInfoEvents::CompInfoEvents(uint8_t compId, Vehicle* vehicle, QObject* parent) + : CompInfo(COMP_METADATA_TYPE_EVENTS, compId, vehicle, parent) +{ + +} + +void CompInfoEvents::setJson(const QString& metadataJsonFileName, const QString& translationJsonFileName) +{ + vehicle->setEventsMetadata(compId, metadataJsonFileName, translationJsonFileName); +} + diff --git a/src/Vehicle/CompInfoEvents.h b/src/Vehicle/CompInfoEvents.h new file mode 100644 index 0000000..c70d1ad --- /dev/null +++ b/src/Vehicle/CompInfoEvents.h @@ -0,0 +1,31 @@ +/**************************************************************************** + * + * (c) 2020 QGROUNDCONTROL PROJECT + * + * QGroundControl is licensed according to the terms in the file + * COPYING.md in the root of the source code directory. + * + ****************************************************************************/ + +#pragma once + +#include "CompInfo.h" + +#include + +class FactMetaData; +class Vehicle; +class FirmwarePlugin; + +class CompInfoEvents : public CompInfo +{ + Q_OBJECT + +public: + CompInfoEvents(uint8_t compId, Vehicle* vehicle, QObject* parent = nullptr); + + // Overrides from CompInfo + void setJson(const QString& metadataJsonFileName, const QString& translationJsonFileName) override; + +private: +}; diff --git a/src/Vehicle/ComponentInformationManager.cc b/src/Vehicle/ComponentInformationManager.cc index dbc7369..f5384aa 100644 --- a/src/Vehicle/ComponentInformationManager.cc +++ b/src/Vehicle/ComponentInformationManager.cc @@ -14,6 +14,7 @@ #include "JsonHelper.h" #include "CompInfoGeneral.h" #include "CompInfoParam.h" +#include "CompInfoEvents.h" #include "QGCFileDownload.h" #include "QGCApplication.h" @@ -23,16 +24,17 @@ QGC_LOGGING_CATEGORY(ComponentInformationManagerLog, "ComponentInformationManagerLog") -ComponentInformationManager::StateFn ComponentInformationManager::_rgStates[]= { +const ComponentInformationManager::StateFn ComponentInformationManager::_rgStates[]= { ComponentInformationManager::_stateRequestCompInfoGeneral, ComponentInformationManager::_stateRequestCompInfoGeneralComplete, ComponentInformationManager::_stateRequestCompInfoParam, + ComponentInformationManager::_stateRequestCompInfoEvents, ComponentInformationManager::_stateRequestAllCompInfoComplete }; -int ComponentInformationManager::_cStates = sizeof(ComponentInformationManager::_rgStates) / sizeof(ComponentInformationManager::_rgStates[0]); +const int ComponentInformationManager::_cStates = sizeof(ComponentInformationManager::_rgStates) / sizeof(ComponentInformationManager::_rgStates[0]); -RequestMetaDataTypeStateMachine::StateFn RequestMetaDataTypeStateMachine::_rgStates[]= { +const RequestMetaDataTypeStateMachine::StateFn RequestMetaDataTypeStateMachine::_rgStates[]= { RequestMetaDataTypeStateMachine::_stateRequestCompInfo, RequestMetaDataTypeStateMachine::_stateRequestMetaDataJson, RequestMetaDataTypeStateMachine::_stateRequestMetaDataJsonFallback, @@ -40,7 +42,7 @@ RequestMetaDataTypeStateMachine::StateFn RequestMetaDataTypeStateMachine::_rgSta RequestMetaDataTypeStateMachine::_stateRequestComplete, }; -int RequestMetaDataTypeStateMachine::_cStates = sizeof(RequestMetaDataTypeStateMachine::_rgStates) / sizeof(RequestMetaDataTypeStateMachine::_rgStates[0]); +const int RequestMetaDataTypeStateMachine::_cStates = sizeof(RequestMetaDataTypeStateMachine::_rgStates) / sizeof(RequestMetaDataTypeStateMachine::_rgStates[0]); ComponentInformationManager::ComponentInformationManager(Vehicle* vehicle) : _vehicle (vehicle) @@ -49,6 +51,7 @@ ComponentInformationManager::ComponentInformationManager(Vehicle* vehicle) { _compInfoMap[MAV_COMP_ID_AUTOPILOT1][COMP_METADATA_TYPE_GENERAL] = new CompInfoGeneral (MAV_COMP_ID_AUTOPILOT1, vehicle, this); _compInfoMap[MAV_COMP_ID_AUTOPILOT1][COMP_METADATA_TYPE_PARAMETER] = new CompInfoParam (MAV_COMP_ID_AUTOPILOT1, vehicle, this); + _compInfoMap[MAV_COMP_ID_AUTOPILOT1][COMP_METADATA_TYPE_EVENTS] = new CompInfoEvents (MAV_COMP_ID_AUTOPILOT1, vehicle, this); } int ComponentInformationManager::stateCount(void) const @@ -121,6 +124,18 @@ void ComponentInformationManager::_stateRequestCompInfoParam(StateMachine* state } } +void ComponentInformationManager::_stateRequestCompInfoEvents(StateMachine* stateMachine) +{ + ComponentInformationManager* compMgr = static_cast(stateMachine); + + if (compMgr->_isCompTypeSupported(COMP_METADATA_TYPE_EVENTS)) { + compMgr->_requestTypeStateMachine.request(compMgr->_compInfoMap[MAV_COMP_ID_AUTOPILOT1][COMP_METADATA_TYPE_EVENTS]); + } else { + qCDebug(ComponentInformationManagerLog) << "_stateRequestCompInfoEvents skipping, not supported"; + compMgr->advance(); + } +} + void ComponentInformationManager::_stateRequestAllCompInfoComplete(StateMachine* stateMachine) { ComponentInformationManager* compMgr = static_cast(stateMachine); @@ -187,7 +202,15 @@ void RequestMetaDataTypeStateMachine::statesCompleted(void) const QString RequestMetaDataTypeStateMachine::typeToString(void) { - return _compInfo->type == COMP_METADATA_TYPE_GENERAL ? "COMP_METADATA_TYPE_GENERAL" : "COMP_METADATA_TYPE_PARAM"; + switch (_compInfo->type) { + case COMP_METADATA_TYPE_GENERAL: return "COMP_METADATA_TYPE_GENERAL"; + case COMP_METADATA_TYPE_PARAMETER: return "COMP_METADATA_TYPE_PARAMETER"; + case COMP_METADATA_TYPE_COMMANDS: return "COMP_METADATA_TYPE_COMMANDS"; + case COMP_METADATA_TYPE_PERIPHERALS: return "COMP_METADATA_TYPE_PERIPHERALS"; + case COMP_METADATA_TYPE_EVENTS: return "COMP_METADATA_TYPE_EVENTS"; + default: break; + } + return "Unknown"; } static void _requestMessageResultHandler(void* resultHandlerData, MAV_RESULT result, Vehicle::RequestMessageResultHandlerFailureCode_t failureCode, const mavlink_message_t &message) diff --git a/src/Vehicle/ComponentInformationManager.h b/src/Vehicle/ComponentInformationManager.h index 5e2bdc1..cf88cc6 100644 --- a/src/Vehicle/ComponentInformationManager.h +++ b/src/Vehicle/ComponentInformationManager.h @@ -69,8 +69,8 @@ private: QElapsedTimer _downloadStartTime; - static StateFn _rgStates[]; - static int _cStates; + static const StateFn _rgStates[]; + static const int _cStates; }; class ComponentInformationManager : public StateMachine @@ -110,6 +110,7 @@ private: static void _stateRequestCompInfoGeneral (StateMachine* stateMachine); static void _stateRequestCompInfoGeneralComplete(StateMachine* stateMachine); static void _stateRequestCompInfoParam (StateMachine* stateMachine); + static void _stateRequestCompInfoEvents (StateMachine* stateMachine); static void _stateRequestAllCompInfoComplete (StateMachine* stateMachine); Vehicle* _vehicle = nullptr; @@ -120,8 +121,8 @@ private: QMap> _compInfoMap; - static StateFn _rgStates[]; - static int _cStates; + static const StateFn _rgStates[]; + static const int _cStates; friend class RequestMetaDataTypeStateMachine; }; diff --git a/src/Vehicle/EventHandler.cc b/src/Vehicle/EventHandler.cc new file mode 100644 index 0000000..7a84141 --- /dev/null +++ b/src/Vehicle/EventHandler.cc @@ -0,0 +1,104 @@ +/**************************************************************************** + * + * (c) 2020 QGROUNDCONTROL PROJECT + * + * QGroundControl is licensed according to the terms in the file + * COPYING.md in the root of the source code directory. + * + ****************************************************************************/ + +#include + +#include "EventHandler.h" +#include "QGCLoggingCategory.h" + +Q_DECLARE_METATYPE(QSharedPointer); + + +EventHandler::EventHandler(QObject* parent, const QString& profile, handle_event_f handleEventCB, + send_request_event_message_f sendRequestCB, + uint8_t ourSystemId, uint8_t ourComponentId, uint8_t systemId, uint8_t componentId) + : QObject(parent), _timer(parent), + _handleEventCB(handleEventCB), + _sendRequestCB(sendRequestCB), + _compid(componentId) +{ + auto error_cb = [componentId](int num_events_lost) { + qCWarning(EventsLog) << "Events got lost:" << num_events_lost << "comp_id:" << componentId; + }; + + auto timeout_cb = [this](int timeout_ms) { + if (timeout_ms < 0) { + _timer.stop(); + } else { + _timer.setSingleShot(true); + _timer.start(timeout_ms); + } + }; + + _parser.setProfile(profile.toStdString()); + + _parser.formatters().url = [](const std::string& content, const std::string& link) { + return ""+content+""; }; + + events::ReceiveProtocol::Callbacks callbacks{error_cb, _sendRequestCB, + std::bind(&EventHandler::gotEvent, this, std::placeholders::_1), timeout_cb}; + _protocol = new events::ReceiveProtocol(callbacks, ourSystemId, ourComponentId, systemId, componentId); + + connect(&_timer, &QTimer::timeout, this, [this]() { _protocol->timerEvent(); }); + + qRegisterMetaType>("ParsedEvent"); +} + +EventHandler::~EventHandler() +{ + delete _protocol; +} + +void EventHandler::gotEvent(const mavlink_event_t& event) +{ + if (!_parser.hasDefinitions()) { + if (_pendingEvents.size() > 50) { // limit size (not expected to happen) + _pendingEvents.clear(); + } + qCDebug(EventsLog) << "No metadata, queuing event, ID:" << event.id << "num pending:" << _pendingEvents.size(); + _pendingEvents.push_back(event); + return; + } + + std::unique_ptr parsed_event = _parser.parse(event); + if (parsed_event == nullptr) { + qCWarning(EventsLog) << "Got Event w/o known metadata: ID:" << event.id << "comp id:" << _compid; + return; + } + + qCDebug(EventsLog) << "Got Event: ID:" << parsed_event->id() << "namespace:" << parsed_event->eventNamespace().c_str() << + "name:" << parsed_event->name().c_str() << "msg:" << parsed_event->message().c_str(); + + _handleEventCB(std::move(parsed_event)); +} + +void EventHandler::handleEvents(const mavlink_message_t& message) +{ + _protocol->processMessage(message); +} + +void EventHandler::setMetadata(const QString &metadataJsonFileName, const QString &translationJsonFileName) +{ + auto translate = [](const std::string& s) { + // TODO: use translation file + return s; + }; + if (_parser.loadDefinitionsFile(metadataJsonFileName.toStdString(), translate)) { + if (_parser.hasDefinitions()) { + // do we have queued events? + for (const auto& event : _pendingEvents) { + gotEvent(event); + } + _pendingEvents.clear(); + } + } else { + qCWarning(EventsLog) << "Failed to load events JSON metadata file"; + } +} + diff --git a/src/Vehicle/EventHandler.h b/src/Vehicle/EventHandler.h new file mode 100644 index 0000000..ae794f6 --- /dev/null +++ b/src/Vehicle/EventHandler.h @@ -0,0 +1,53 @@ +/**************************************************************************** + * + * (c) 2020 QGROUNDCONTROL PROJECT + * + * QGroundControl is licensed according to the terms in the file + * COPYING.md in the root of the source code directory. + * + ****************************************************************************/ + +#pragma once + +#include +#include +#include + +#include + +#include "HealthAndArmingChecks.h" + +#include +#include +#include + +class EventHandler : public QObject +{ + Q_OBJECT +public: + using send_request_event_message_f = std::function; + using handle_event_f = std::function)>; + + EventHandler(QObject* parent, const QString& profile, handle_event_f handleEventCB, + send_request_event_message_f sendRequestCB, + uint8_t ourSystemId, uint8_t ourComponentId, uint8_t systemId, uint8_t componentId); + ~EventHandler(); + + + void handleEvents(const mavlink_message_t& message); + + void setMetadata(const QString& metadataJsonFileName, const QString& translationJsonFileName); + + HealthAndArmingCheckHandler& healthAndArmingChecks() { return _healthAndArmingChecks; } +private: + void gotEvent(const mavlink_event_t& event); + + events::ReceiveProtocol* _protocol{nullptr}; + QTimer _timer; + events::parser::Parser _parser; + HealthAndArmingCheckHandler _healthAndArmingChecks; + QVector _pendingEvents; ///< stores incoming events until we have the metadata loaded + handle_event_f _handleEventCB; + send_request_event_message_f _sendRequestCB; + const uint8_t _compid; +}; diff --git a/src/Vehicle/HealthAndArmingChecks.cc b/src/Vehicle/HealthAndArmingChecks.cc new file mode 100644 index 0000000..ff8be64 --- /dev/null +++ b/src/Vehicle/HealthAndArmingChecks.cc @@ -0,0 +1,125 @@ +/**************************************************************************** + * + * (c) 2020 QGROUNDCONTROL PROJECT + * + * QGroundControl is licensed according to the terms in the file + * COPYING.md in the root of the source code directory. + * + ****************************************************************************/ + +#include "HealthAndArmingChecks.h" + +#include + +QGC_LOGGING_CATEGORY(HealthAndArmingChecks, "HealthAndArmingChecks"); + +using health_component_t = events::common::enums::health_component_t; +using navigation_mode_category_t = events::common::enums::navigation_mode_category_t; + +void HealthAndArmingCheckHandler::handleEvent(const events::parser::ParsedEvent& event) +{ + Type type; + if (event.eventNamespace() == "common" && event.name() == "arming_check_summary") { + type = Type::ArmingCheckSummary; + } else if (event.eventNamespace() == "common" && event.name() == "health_summary") { + type = Type::HealthSummary; + } else { + type = Type::Other; + } + + // the expected order of receiving is: + // - ArmingCheckSummary + // - N Other + // - HealthSummary + + if (type != _expectedEvent) { + if (_expectedEvent == Type::Other && type == Type::HealthSummary) { + // all good + } else if (type == Type::ArmingCheckSummary) { + qCDebug(HealthAndArmingChecks) << "Unexpected ArmingCheckSummary event, resetting. Expected:" << (int)_expectedEvent; + // accept & reset + } else { + qCDebug(HealthAndArmingChecks) << "Unexpected event, resetting. Expected:" << (int)_expectedEvent + << "Got:" << (int)type; + _expectedEvent = Type::ArmingCheckSummary; + return; + } + } + + switch (type) { + case Type::ArmingCheckSummary: + reset(); + if (event.id() == (uint32_t)events::common::event_id_t::arming_check_summary) { + ArmingCheckSummary &arming = _results[_currentResult].arming; + events::common::decode_arming_check_summary(event.eventData(), arming.error, arming.warning, arming.canArm); + _expectedEvent = Type::Other; + } + break; + case Type::Other: { + Check check; + check.type = event.group() == "health" ? CheckType::Health : CheckType::ArmingCheck; + check.message = QString::fromStdString(event.message()); + check.description = QString::fromStdString(event.description()); + check.affectedModes = (events::common::enums::navigation_mode_category_t)event.argumentValue(0).value.val_uint8_t; + check.affectedHealthComponentIndex = event.argumentValue(1).value.val_uint8_t; + check.logLevel = events::externalLogLevel(event.eventData().log_levels); + _results[_currentResult].checks.append(check); + } + break; + case Type::HealthSummary: + if (event.id() == (uint32_t)events::common::event_id_t::health_summary) { + HealthSummary &health = _results[_currentResult].health; + events::common::decode_health_summary(event.eventData(), health.isPresent, health.error, health.warning); + _currentResult = (_currentResult + 1) % 2; + emit update(); + testReport(); + } + reset(); + break; + } +} + +void HealthAndArmingCheckHandler::reset() +{ + _results[_currentResult].reset(); + _expectedEvent = Type::ArmingCheckSummary; +} + +void HealthAndArmingCheckHandler::testReport() +{ + // just for testing... +// qWarning() << "Got Health/Arming checks update"; +// qWarning() << "Arming possible in current mode: " << (results().arming.canArm & navigation_mode_category_t::current); +// qWarning() << "Can a mission be flown: " << (results().arming.canArm & navigation_mode_category_t::mission); +// qWarning() << "Autonomous (e.g. Takeoff) flight possible: " << (results().arming.canArm & navigation_mode_category_t::autonomous); +// +// QString gps_icon_color; +// if ((results().health.error & health_component_t::sensor_gps) || +// (results().health.error & health_component_t::global_position_estimate) || +// (results().arming.error & health_component_t::sensor_gps) || +// (results().arming.error & health_component_t::global_position_estimate)) { +// gps_icon_color = "red"; +// } else if((results().health.warning & health_component_t::sensor_gps) || +// (results().health.warning & health_component_t::global_position_estimate) || +// (results().arming.warning & health_component_t::sensor_gps) || +// (results().arming.warning & health_component_t::global_position_estimate)) { +// gps_icon_color = "yellow"; +// } else { +// gps_icon_color = "green"; +// } +// qWarning() << "GPS/Position icon color: " << gps_icon_color; +// +// // display events that are relevant for current mode: +// qWarning() << "Current flight mode:"; +// for (const auto& check : results().checks) { +// if (check.affectedModes & navigation_mode_category_t::current) { +// qWarning() << " " << (int)check.logLevel << check.message; +// } +// } +// qWarning() << "Other flight modes:"; +// for (const auto& check : results().checks) { +// if (!(check.affectedModes & navigation_mode_category_t::current)) { +// qWarning() << " " << (int)check.logLevel << check.message; +// } +// } +} diff --git a/src/Vehicle/HealthAndArmingChecks.h b/src/Vehicle/HealthAndArmingChecks.h new file mode 100644 index 0000000..1ea15d9 --- /dev/null +++ b/src/Vehicle/HealthAndArmingChecks.h @@ -0,0 +1,84 @@ +/**************************************************************************** + * + * (c) 2020 QGROUNDCONTROL PROJECT + * + * QGroundControl is licensed according to the terms in the file + * COPYING.md in the root of the source code directory. + * + ****************************************************************************/ + +#pragma once + +#include +#include +#include + +#include +#include + +Q_DECLARE_LOGGING_CATEGORY(HealthAndArmingChecks) + +class HealthAndArmingCheckHandler : public QObject +{ + Q_OBJECT +public: + + enum class CheckType { + ArmingCheck, + Health + }; + struct Check { + CheckType type; + QString message; + QString description; + events::common::enums::navigation_mode_category_t affectedModes; + uint8_t affectedHealthComponentIndex; ///< index for events::common::enums::health_component_t, can be 0xff + events::Log logLevel; + }; + + struct HealthSummary { + events::common::enums::health_component_t isPresent; + events::common::enums::health_component_t error; + events::common::enums::health_component_t warning; + }; + struct ArmingCheckSummary { + events::common::enums::health_component_t error; + events::common::enums::health_component_t warning; + events::common::enums::navigation_mode_category_t canArm; + }; + + struct Results { + HealthSummary health{}; + ArmingCheckSummary arming{}; + QVector checks{}; + + void reset() { + health = {}; + arming = {}; + checks.clear(); + } + }; + + void handleEvent(const events::parser::ParsedEvent& event); + + const Results& results() const { return _results[(_currentResult + 1) % 2]; } + +signals: + void update(); +private: + + enum class Type { + ArmingCheckSummary, + Other, + HealthSummary, + }; + + void reset(); + + void testReport(); + + Type _expectedEvent{Type::ArmingCheckSummary}; + Results _results[2]; ///< store the last full set and currently updating one + int _currentResult{0}; ///< index for the currently updating/adding results +}; + diff --git a/src/Vehicle/Vehicle.cc b/src/Vehicle/Vehicle.cc index 84b4653..df01cd3 100644 --- a/src/Vehicle/Vehicle.cc +++ b/src/Vehicle/Vehicle.cc @@ -49,6 +49,7 @@ #include "ComponentInformationManager.h" #include "InitialConnectStateMachine.h" #include "VehicleBatteryFactGroup.h" +#include "EventHandler.h" #ifdef QT_DEBUG #include "MockLink.h" #endif @@ -740,6 +741,12 @@ void Vehicle::_mavlinkMessageReceived(LinkInterface* link, mavlink_message_t mes _handleObstacleDistance(message); break; + case MAVLINK_MSG_ID_EVENT: + case MAVLINK_MSG_ID_CURRENT_EVENT_SEQUENCE: + case MAVLINK_MSG_ID_RESPONSE_EVENT_ERROR: + _eventHandler(message.compid).handleEvents(message); + break; + case MAVLINK_MSG_ID_SERIAL_CONTROL: { mavlink_serial_control_t ser; @@ -854,6 +861,12 @@ void Vehicle::_chunkedStatusTextCompleted(uint8_t compId) _chunkedStatusTextInfoMap.remove(compId); + // PX4 backwards compatibility: messages sent out ending with a tab are also sent as event + if (messageText.endsWith('\t') && px4Firmware()) { + qCDebug(VehicleLog) << "Dropping message (expected as event):" << messageText; + return; + } + bool skipSpoken = false; bool ardupilotPrearm = messageText.startsWith(QStringLiteral("PreArm")); bool px4Prearm = messageText.startsWith(QStringLiteral("preflight"), Qt::CaseInsensitive) && severity >= MAV_SEVERITY_CRITICAL; @@ -1501,6 +1514,83 @@ void Vehicle::_handlePing(LinkInterface* link, mavlink_message_t& message) } } +void Vehicle::_handleEvent(uint8_t comp_id, std::unique_ptr event) +{ + int severity = -1; + switch (events::externalLogLevel(event->eventData().log_levels)) { + case events::Log::Emergency: severity = MAV_SEVERITY_EMERGENCY; break; + case events::Log::Alert: severity = MAV_SEVERITY_ALERT; break; + case events::Log::Critical: severity = MAV_SEVERITY_CRITICAL; break; + case events::Log::Error: severity = MAV_SEVERITY_ERROR; break; + case events::Log::Warning: severity = MAV_SEVERITY_WARNING; break; + case events::Log::Notice: severity = MAV_SEVERITY_NOTICE; break; + case events::Log::Info: severity = MAV_SEVERITY_INFO; break; + default: break; + } + + // handle special groups & protocols + if (event->group() == "health" || event->group() == "arming_check") { + _events[comp_id]->healthAndArmingChecks().handleEvent(*event.get()); + // these are displayed separately + return; + } + if (event->group() == "calibration") { + emit calibrationEventReceived(id(), comp_id, severity, + QSharedPointer{new events::parser::ParsedEvent{*event}}); + // these are displayed separately + return; + } + + // show message according to the log level, don't show unknown event groups (might be part of a new protocol) + if (event->group() == "default" && severity != -1) { + std::string message = event->message(); + std::string description = event->description(); + if (message.size() > 0) { + // TODO: handle this properly in the UI (e.g. with an expand button to display the description, clickable URL's + params)... + QString msg = QString::fromStdString(message); + if (description.size() > 0) { + msg += "
" + QString::fromStdString(description).replace("\n", "
") + ""; + } + emit textMessageReceived(id(), comp_id, severity, msg); + } + } +} + +EventHandler& Vehicle::_eventHandler(uint8_t compid) +{ + auto eventData = _events.find(compid); + if (eventData == _events.end()) { + // add new component + + auto sendRequestEventMessageCB = [this](const mavlink_request_event_t& msg) { + SharedLinkInterfacePtr sharedLink = vehicleLinkManager()->primaryLink().lock(); + if (sharedLink) { + mavlink_message_t message; + mavlink_msg_request_event_encode_chan(_mavlink->getSystemId(), + _mavlink->getComponentId(), + sharedLink->mavlinkChannel(), + &message, + &msg); + sendMessageOnLinkThreadSafe(sharedLink.get(), message); + } + }; + + QString profile = "dev"; // TODO: should be configurable + + QSharedPointer eventHandler{new EventHandler(this, profile, + std::bind(&Vehicle::_handleEvent, this, compid, std::placeholders::_1), + sendRequestEventMessageCB, + _mavlink->getSystemId(), _mavlink->getComponentId(), _id, compid)}; + eventData = _events.insert(compid, eventHandler); + } + return *eventData->data(); +} + +void Vehicle::setEventsMetadata(uint8_t compid, const QString& metadataJsonFileName, const QString& translationJsonFileName) +{ + _eventHandler(compid).setMetadata(metadataJsonFileName, translationJsonFileName); +} + void Vehicle::_handleHeartbeat(mavlink_message_t& message) { if (message.compid != _defaultComponentId) { diff --git a/src/Vehicle/Vehicle.h b/src/Vehicle/Vehicle.h index 4aa25d2..38154b4 100644 --- a/src/Vehicle/Vehicle.h +++ b/src/Vehicle/Vehicle.h @@ -15,6 +15,7 @@ #include #include #include +#include #include "FactGroup.h" #include "QGCMAVLink.h" @@ -44,6 +45,7 @@ #include "RallyPointManager.h" #include "FTPManager.h" +class EventHandler; class UAS; class UASInterface; class FirmwarePlugin; @@ -71,6 +73,12 @@ class InitialConnectStateMachine; class AirspaceVehicleManager; #endif +namespace events { +namespace parser { +class ParsedEvent; +} +} + Q_DECLARE_LOGGING_CATEGORY(VehicleLog) class Vehicle : public FactGroup @@ -788,6 +796,8 @@ public: double loadProgress () const { return _loadProgress; } + void setEventsMetadata(uint8_t compid, const QString& metadataJsonFileName, const QString& translationJsonFileName); + public slots: void setVtolInFwdFlight (bool vtolInFwdFlight); void _offlineFirmwareTypeSettingChanged (QVariant varFirmwareType); // Should only be used by MissionControler to set firmware from Plan file @@ -819,6 +829,7 @@ signals: void toolIndicatorsChanged (); void modeIndicatorsChanged (); void textMessageReceived (int uasid, int componentid, int severity, QString text); + void calibrationEventReceived (int uasid, int componentid, int severity, QSharedPointer event); void checkListStateChanged (); void messagesReceivedChanged (); void messagesSentChanged (); @@ -955,6 +966,7 @@ private: void _handleOrbitExecutionStatus (const mavlink_message_t& message); void _handleGimbalOrientation (const mavlink_message_t& message); void _handleObstacleDistance (const mavlink_message_t& message); + void _handleEvent(uint8_t comp_id, std::unique_ptr event); // ArduPilot dialect messages #if !defined(NO_ARDUPILOT_DIALECT) void _handleCameraFeedback (const mavlink_message_t& message); @@ -982,6 +994,7 @@ private: void _chunkedStatusTextCompleted (uint8_t compId); void _setMessageInterval (int messageId, int rate); bool _initialConnectComplete () const; + EventHandler& _eventHandler (uint8_t compid); static void _rebootCommandResultHandler(void* resultHandlerData, int compId, MAV_RESULT commandResult, MavCmdResultFailureCode_t failureCode); @@ -1143,6 +1156,8 @@ private: QTimer _orbitTelemetryTimer; static const int _orbitTelemetryTimeoutMsecs = 3000; // No telemetry for this amount and orbit will go inactive + QMap> _events; ///< One protocol handler for each component ID + MAVLinkStreamConfig _mavlinkStreamConfig; // Chunked status text support