From b84c6fc0fad4d779a3028c54cfa1c498f849c2bc Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Thu, 23 Sep 2021 09:50:27 +1000 Subject: [PATCH 1/4] AP_Logger: support mavlink event interface --- libraries/AP_Logger/AP_Logger.cpp | 86 +++++++++++++++++++++++++++++++ libraries/AP_Logger/AP_Logger.h | 15 ++++++ 2 files changed, 101 insertions(+) diff --git a/libraries/AP_Logger/AP_Logger.cpp b/libraries/AP_Logger/AP_Logger.cpp index 89e54962a98d5..e63c868199f9e 100644 --- a/libraries/AP_Logger/AP_Logger.cpp +++ b/libraries/AP_Logger/AP_Logger.cpp @@ -842,6 +842,45 @@ bool AP_Logger::logging_started(void) { return false; } +void AP_Logger::handle_message_request_event(GCS_MAVLINK &link, const mavlink_message_t &msg) +{ + mavlink_request_event_t packet; + mavlink_msg_request_event_decode(&msg, &packet); + + // if (packet.target_system != gcs().sysid_this_mav()) { + // return; + // } + + // if (packet.target_component != gcs().compid_this_mav()) { + // return; + // } + + if (packet.target_system != 1) { + return; + } + + if (packet.target_component != 1) { + return; + } + + const uint8_t queue_count = queue_full ? ARRAY_SIZE(mavlink_event_interface_queue) : next_mavlink_event_interface_queue_entry; + for (uint8_t i=0; i= packet.first_sequence && seqno <= packet.last_sequence) { + send_mavlink_event_interface_queue_entry(mavlink_event_interface_queue[i], link); + } + } else { + if (seqno <= packet.last_sequence || seqno >= packet.first_sequence) { + send_mavlink_event_interface_queue_entry(mavlink_event_interface_queue[i], link); + } + } + } +} + void AP_Logger::handle_mavlink_msg(GCS_MAVLINK &link, const mavlink_message_t &msg) { switch (msg.msgid) { @@ -857,6 +896,9 @@ void AP_Logger::handle_mavlink_msg(GCS_MAVLINK &link, const mavlink_message_t &m case MAVLINK_MSG_ID_LOG_REQUEST_END: handle_log_message(link, msg); break; + case MAVLINK_MSG_ID_REQUEST_EVENT: + handle_message_request_event(link, msg); + break; } } @@ -1469,9 +1511,53 @@ void AP_Logger::start_io_thread(void) #undef FOR_EACH_BACKEND +void AP_Logger::queue_entry_to_event_message(mavlink_event_t &packet, const MAVLinkEventInterfaceQueue &entry) +{ + // note reordering of packet fields here; see mavlink_msg_event.h + packet = { + entry.mavlink_event_id, // event id (component metadata) + entry.time_boot_ms, + entry.seqno, + 0, // destination sysID + 0, // destination compid + entry.log_level, + { entry.args[0] } + }; +} + +void AP_Logger::send_mavlink_event_interface_queue_entry(const MAVLinkEventInterfaceQueue &entry) +{ + // note reordering of packet fields here; see mavlink_msg_event.h + mavlink_event_t packet {}; + queue_entry_to_event_message(packet, entry); + gcs().send_to_active_channels(MAVLINK_MSG_ID_EVENT, (const char*)&packet); +} + +void AP_Logger::send_mavlink_event_interface_queue_entry(const MAVLinkEventInterfaceQueue &entry, GCS_MAVLINK &link) +{ + mavlink_event_t packet {}; + queue_entry_to_event_message(packet, entry); + link.send_message(MAVLINK_MSG_ID_EVENT, (const char*)&packet); +} + // Wrote an event packet void AP_Logger::Write_Event(LogEvent id) { + const struct MAVLinkEventInterfaceQueue queue_entry { + (uint8_t)id, + AP_HAL::millis(), + mavlink_event_interface_seq++, + 0, + { uint8_t(id) } + }; + mavlink_event_interface_queue[next_mavlink_event_interface_queue_entry++] = queue_entry; + if (next_mavlink_event_interface_queue_entry >= ARRAY_SIZE(mavlink_event_interface_queue)) { + next_mavlink_event_interface_queue_entry = 0; + queue_full = true; + } + + send_mavlink_event_interface_queue_entry(queue_entry); + const struct log_Event pkt{ LOG_PACKET_HEADER_INIT(LOG_EVENT_MSG), time_us : AP_HAL::micros64(), diff --git a/libraries/AP_Logger/AP_Logger.h b/libraries/AP_Logger/AP_Logger.h index e8a91d62da63d..ce659d5bb1d21 100644 --- a/libraries/AP_Logger/AP_Logger.h +++ b/libraries/AP_Logger/AP_Logger.h @@ -443,6 +443,21 @@ class AP_Logger IN_RC_FAILSAFE = 1U<<1, // true if the system is current in RC failsafe }; + struct MAVLinkEventInterfaceQueue { + uint32_t mavlink_event_id; + uint32_t time_boot_ms; + uint16_t seqno; + uint8_t log_level; + uint8_t args[1]; // uint8_t[40] in the mavlink message + } mavlink_event_interface_queue[5]; + uint8_t next_mavlink_event_interface_queue_entry; + bool queue_full; + uint16_t mavlink_event_interface_seq; + void queue_entry_to_event_message(mavlink_event_t &packet, const MAVLinkEventInterfaceQueue &entry); + void send_mavlink_event_interface_queue_entry(const MAVLinkEventInterfaceQueue &entry); + void send_mavlink_event_interface_queue_entry(const MAVLinkEventInterfaceQueue &entry, GCS_MAVLINK &link); + void handle_message_request_event(GCS_MAVLINK &link, const mavlink_message_t &msg); + /* * support for dynamic Write; user-supplies name, format, * labels and values in a single function call. From c1ca5036d6f7a142c70e4a1536ba9c878b8a2431 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Thu, 23 Sep 2021 09:50:27 +1000 Subject: [PATCH 2/4] GCS_MAVLink: support mavlink event interface --- libraries/GCS_MAVLink/GCS_Common.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp index 61daff9dc3d29..ef16ebd5b0320 100644 --- a/libraries/GCS_MAVLink/GCS_Common.cpp +++ b/libraries/GCS_MAVLink/GCS_Common.cpp @@ -4107,6 +4107,7 @@ void GCS_MAVLINK::handle_message(const mavlink_message_t &msg) case MAVLINK_MSG_ID_LOG_ERASE: case MAVLINK_MSG_ID_LOG_REQUEST_END: case MAVLINK_MSG_ID_REMOTE_LOG_BLOCK_STATUS: + case MAVLINK_MSG_ID_REQUEST_EVENT: AP::logger().handle_mavlink_msg(*this, msg); break; #endif From 9f5f1092b49acc99ac4de35ca957dd022905161c Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Wed, 21 Apr 2021 22:18:39 +1000 Subject: [PATCH 3/4] AP_FileSystem: add ability to emit generate_metadata.json --- libraries/AP_Filesystem/AP_Filesystem_Sys.cpp | 129 ++++++++++++++++++ libraries/AP_Filesystem/AP_Filesystem_Sys.h | 3 + 2 files changed, 132 insertions(+) diff --git a/libraries/AP_Filesystem/AP_Filesystem_Sys.cpp b/libraries/AP_Filesystem/AP_Filesystem_Sys.cpp index 5063267c04a65..9158f76f85982 100644 --- a/libraries/AP_Filesystem/AP_Filesystem_Sys.cpp +++ b/libraries/AP_Filesystem/AP_Filesystem_Sys.cpp @@ -25,6 +25,7 @@ #include #include #include +#include #include extern const AP_HAL::HAL& hal; @@ -33,6 +34,10 @@ struct SysFileList { const char* name; }; +#ifndef HAL_LOG_EVENT_METADATA +#define HAL_LOG_EVENT_METADATA 1 +#endif + static const SysFileList sysfs_file_list[] = { {"threads.txt"}, {"tasks.txt"}, @@ -40,6 +45,10 @@ static const SysFileList sysfs_file_list[] = { {"memory.txt"}, {"uarts.txt"}, {"timers.txt"}, + {"general_metadata.json"}, +#if HAL_LOG_EVENT_METADATA + {"events_metadata.json"}, +#endif #if HAL_MAX_CAN_PROTOCOL_DRIVERS {"can_log.txt"}, #endif @@ -66,6 +75,118 @@ int8_t AP_Filesystem_Sys::file_in_sysfs(const char *fname) { return -1; } +void AP_Filesystem_Sys::general_metadata(ExpandingString &str) +{ + const struct MetaDataInfo { + int type_id; + const char * uri; + } metadata[] = { + 3, // COMP_METADATA_TYPE_EVENTS +#if HAL_LOG_EVENT_METADATA + "mftp:/@SYS/events_metadata.json" +#else + "https://firmware.ardupilot.org/GITHASH/events_metadata.json" +#endif + }; + // a header to allow for machine parsers to determine format + str.printf( + "{" + " \"version\": 1," + " \"metadataTypes\": [" + ); + + // FIXME: too many [? + const char *joiner = ""; + for (const MetaDataInfo &info : metadata) { + str.printf("%s{\"type\": %d, \"uri\": \"%s\", \"fileCrc\": 133761337}", + joiner, + info.type_id, + info.uri); + joiner = ","; + } + + str.printf( + " ]}" + ); + +} + +#if HAL_LOG_EVENT_METADATA + +void AP_Filesystem_Sys::events_metadata(ExpandingString &str) +{ + static const struct { + LogEvent value; + const char *name; + const char *description; + } logevent_metadata[] { + { LogEvent::ARMED, "Armed", "Vehicle was armed" }, + { LogEvent::DISARMED, "Disarmed", "Vehicle was disarmed" }, + { LogEvent::SET_HOME, "SetHome", "Vehicle home location was set" }, + }; + + // FIXME: this is really the wrong schema for the LogError stuff. + static const struct { + LogErrorSubsystem value; + const char *name; + const char *description; + } logerror_metadata[] { + { LogErrorSubsystem::MAIN, "Main", "Bogus generic bucket for everything unclassified elsewhere" }, + }; + + // a header to allow for machine parsers to determine format + str.printf( + "{ " + " \"version\": 1, " + " \"components\": [ " + " { " + " \"component_id\": %u, " + " \"namespace\": \"common\", " + " \"enums\": [ " + " { " + " \"name\": \"ardupilot_event\", " + " \"type\": \"uint8_t\", " + " \"description\": \"Generic ArduPilot events from AP_Logger::LogEvent\", " + " \"entries\": [ ", + 1); // FIXME: should be mavlink component ID of autopilot + + const char *joiner = ""; + for (auto &x : logevent_metadata) { + str.printf( + "%s{\"value\":%u, \"name\":\"%s\", \"description\":\"%s\"}\n", + joiner, + (unsigned)x.value, + x.name, + x.description + ); + joiner = ","; + } + str.printf("]}, [{"); + str.printf( + " \"name\": \"ardupilot_errors\", " + " \"type\": \"uint8_t\", " + " \"description\": \"Generic ArduPilot errors from AP_Logger::LogErrorSubsystem\", " + " \"entries\": [ " + ); + + joiner = ""; + for (auto &x : logerror_metadata) { + str.printf( + "%s{\"value\":%u, \"name\":\"%s\", \"description\":\"%s\"}\n", + joiner, + (unsigned)x.value, + x.name, + x.description + ); + joiner = ","; + } + + str.printf("]}]"); + str.printf("]}]}"); +} + +#endif + int AP_Filesystem_Sys::open(const char *fname, int flags, bool allow_absolute_paths) { if ((flags & O_ACCMODE) != O_RDONLY) { @@ -120,6 +241,14 @@ int AP_Filesystem_Sys::open(const char *fname, int flags, bool allow_absolute_pa if (strcmp(fname, "timers.txt") == 0) { hal.util->timer_info(*r.str); } + if (strcmp(fname, "general_metadata.json") == 0) { + general_metadata(*r.str); + } +#if HAL_LOG_EVENT_METADATA + if (strcmp(fname, "events_metadata.json") == 0) { + events_metadata(*r.str); + } +#endif #if HAL_CANMANAGER_ENABLED if (strcmp(fname, "can_log.txt") == 0) { AP::can().log_retrieve(*r.str); diff --git a/libraries/AP_Filesystem/AP_Filesystem_Sys.h b/libraries/AP_Filesystem/AP_Filesystem_Sys.h index e9d186e70fc50..18bf2b67c91da 100644 --- a/libraries/AP_Filesystem/AP_Filesystem_Sys.h +++ b/libraries/AP_Filesystem/AP_Filesystem_Sys.h @@ -51,6 +51,9 @@ class AP_Filesystem_Sys : public AP_Filesystem_Backend uint32_t file_ofs; ExpandingString *str; } file[max_open_file]; + + void general_metadata(ExpandingString &str); + void events_metadata(ExpandingString &str); }; #endif // AP_FILESYSTEM_SYS_ENABLED From 0b8cde593c9e984398391d489a715ba904377434 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Wed, 21 Apr 2021 22:19:01 +1000 Subject: [PATCH 4/4] GCS_Common: add support for sending COMPONENT_INFORMATION --- libraries/GCS_MAVLink/GCS.h | 2 ++ libraries/GCS_MAVLink/GCS_Common.cpp | 21 +++++++++++++++++++++ libraries/GCS_MAVLink/ap_message.h | 1 + 3 files changed, 24 insertions(+) diff --git a/libraries/GCS_MAVLink/GCS.h b/libraries/GCS_MAVLink/GCS.h index 71725275b3e9e..64f7e85a90291 100644 --- a/libraries/GCS_MAVLink/GCS.h +++ b/libraries/GCS_MAVLink/GCS.h @@ -402,6 +402,8 @@ class GCS_MAVLINK void send_uavionix_adsb_out_status() const; void send_autopilot_state_for_gimbal_device() const; + void send_component_information() const; + // lock a channel, preventing use by MAVLink void lock(bool _lock) { _locked = _lock; diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp index ef16ebd5b0320..326b5fd72ca6b 100644 --- a/libraries/GCS_MAVLink/GCS_Common.cpp +++ b/libraries/GCS_MAVLink/GCS_Common.cpp @@ -1122,6 +1122,7 @@ ap_message GCS_MAVLINK::mavlink_id_to_ap_message_id(const uint32_t mavlink_id) c #if AP_MAVLINK_MSG_RELAY_STATUS_ENABLED { MAVLINK_MSG_ID_RELAY_STATUS, MSG_RELAY_STATUS}, #endif + { MAVLINK_MSG_ID_COMPONENT_INFORMATION, MSG_COMPONENT_INFORMATION}, }; for (uint8_t i=0; i