Add support for mavlink events interface

libraries/AP_Filesystem/AP_Filesystem_Sys.cpp

@@ -25,6 +25,7 @@
 #include <AP_Math/AP_Math.h>
 #include <AP_CANManager/AP_CANManager.h>
 #include <AP_Scheduler/AP_Scheduler.h>
+#include <AP_Logger/AP_Logger.h>
 #include <AP_Common/ExpandingString.h>
 
 extern const AP_HAL::HAL& hal;
@@ -33,13 +34,21 @@ 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"},
     {"dma.txt"},
     {"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);

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

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<queue_count; i++) {
+        if (!HAVE_PAYLOAD_SPACE(link.get_chan(), EVENT)) {
+            return;
+        }
+        const uint16_t seqno = mavlink_event_interface_queue[i].seqno;
+        if (packet.first_sequence <= packet.last_sequence) {
+            if (seqno >= 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(),

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.

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;

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<ARRAY_SIZE(map); i++) {
@@ -4107,6 +4108,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
@@ -5982,6 +5984,21 @@ void GCS_MAVLINK::send_received_message_deprecation_warning(const char * message
     send_text(MAV_SEVERITY_INFO, "Received message (%s) is deprecated", message);
 }
 
+void GCS_MAVLINK::send_component_information() const
+{
+    const char *general_metadata_url = "mftp:/@SYS/general_metadata.json";
+    const uint32_t general_metadata_checksum = 133761337;
+
+    mavlink_msg_component_information_send(
+        chan,
+        AP_HAL::millis(),
+        general_metadata_checksum,
+        general_metadata_url,
+        0,  // -1?
+        ""
+        );
+}
+
 bool GCS_MAVLINK::try_send_message(const enum ap_message id)
 {
     bool ret = true;
@@ -6382,6 +6399,11 @@ bool GCS_MAVLINK::try_send_message(const enum ap_message id)
     }
 #endif
 
+    case MSG_COMPONENT_INFORMATION:
+        CHECK_PAYLOAD_SIZE(COMPONENT_INFORMATION);
+        send_component_information();
+        break;
+
 #if AP_MAVLINK_MSG_UAVIONIX_ADSB_OUT_STATUS_ENABLED
     case MSG_UAVIONIX_ADSB_OUT_STATUS:
         CHECK_PAYLOAD_SIZE(UAVIONIX_ADSB_OUT_STATUS);

libraries/GCS_MAVLink/ap_message.h

@@ -97,5 +97,6 @@ enum ap_message : uint8_t {
 #if AP_MAVLINK_MSG_HIGHRES_IMU_ENABLED
     MSG_HIGHRES_IMU,
 #endif
+    MSG_COMPONENT_INFORMATION,
     MSG_LAST // MSG_LAST must be the last entry in this enum
 };