From 38f3b8d356fea8b2e36c3dd64f716011661a27b4 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Mon, 9 Nov 2020 11:51:27 +0100 Subject: [PATCH] mavlink & system: add events interface - sending protocol - uorb event message & template methods for argument packing - libevents submodule to send common events and handle json files - cmake maintains a list of all (PX4) source files for the current build (PX4 modules + libs), which is used to extract event metadata and generate a json file --- .gitmodules | 3 + CMakeLists.txt | 7 + Tools/astyle/files_to_check_code_style.sh | 1 + Tools/compress.py | 20 ++ cmake/px4_add_board.cmake | 8 +- cmake/px4_add_library.cmake | 3 + cmake/px4_add_module.cmake | 3 + cmake/px4_list_make_absolute.cmake | 58 +++++ msg/CMakeLists.txt | 1 + msg/event.msg | 11 + msg/tools/uorb_rtps_message_ids.yaml | 2 + platforms/common/CMakeLists.txt | 1 + platforms/common/events.cpp | 70 ++++++ .../common/include/libevents_definitions.h | 48 ++++ .../include/px4_platform_common/events.h | 132 ++++++++++ src/lib/component_information/CMakeLists.txt | 17 ++ src/lib/events/CMakeLists.txt | 95 +++++++ src/lib/events/libevents | 1 + src/modules/logger/logger.cpp | 73 ++++++ src/modules/logger/logger.h | 8 + src/modules/mavlink/CMakeLists.txt | 1 + src/modules/mavlink/mavlink_events.cpp | 232 ++++++++++++++++++ src/modules/mavlink/mavlink_events.h | 146 +++++++++++ src/modules/mavlink/mavlink_main.cpp | 53 ++++ src/modules/mavlink/mavlink_main.h | 4 + src/modules/mavlink/mavlink_receiver.cpp | 9 + src/modules/mavlink/mavlink_receiver.h | 1 + src/systemcmds/topic_listener/CMakeLists.txt | 2 +- 28 files changed, 1007 insertions(+), 3 deletions(-) create mode 100755 Tools/compress.py create mode 100644 cmake/px4_list_make_absolute.cmake create mode 100644 msg/event.msg create mode 100644 platforms/common/events.cpp create mode 100644 platforms/common/include/libevents_definitions.h create mode 100644 platforms/common/include/px4_platform_common/events.h create mode 100644 src/lib/events/CMakeLists.txt create mode 160000 src/lib/events/libevents create mode 100644 src/modules/mavlink/mavlink_events.cpp create mode 100644 src/modules/mavlink/mavlink_events.h diff --git a/.gitmodules b/.gitmodules index b019469bf9..90518643a9 100644 --- a/.gitmodules +++ b/.gitmodules @@ -70,3 +70,6 @@ [submodule "src/lib/crypto/monocypher"] path = src/lib/crypto/monocypher url = https://github.com/PX4/Monocypher.git +[submodule "src/lib/events/libevents"] + path = src/lib/events/libevents + url = https://github.com/mavlink/libevents.git diff --git a/CMakeLists.txt b/CMakeLists.txt index a694590e1e..814fbe2bbf 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -129,6 +129,10 @@ define_property(GLOBAL PROPERTY PX4_MODULE_PATHS BRIEF_DOCS "PX4 module paths" FULL_DOCS "List of paths to all PX4 modules" ) +define_property(GLOBAL PROPERTY PX4_SRC_FILES + BRIEF_DOCS "src files from all PX4 modules & libs" + FULL_DOCS "SRC files from px4_add_{module,library}" + ) #============================================================================= # configuration @@ -417,6 +421,9 @@ foreach(module ${config_module_list}) add_subdirectory(src/${module}) endforeach() +# add events lib after modules and libs as it needs to know all source files (PX4_SRC_FILES) +add_subdirectory(src/lib/events EXCLUDE_FROM_ALL) + # must be the last module before firmware add_subdirectory(src/lib/parameters EXCLUDE_FROM_ALL) target_link_libraries(parameters_interface INTERFACE parameters) diff --git a/Tools/astyle/files_to_check_code_style.sh b/Tools/astyle/files_to_check_code_style.sh index 3eec9a2855..d39c4c397d 100755 --- a/Tools/astyle/files_to_check_code_style.sh +++ b/Tools/astyle/files_to_check_code_style.sh @@ -16,6 +16,7 @@ exec find boards msg src platforms test \ -path src/drivers/uavcannode_gps_demo/libcanard -prune -o \ -path src/drivers/uavcan/uavcan_drivers/kinetis/driver/include/uavcan_kinetis -prune -o \ -path src/lib/ecl -prune -o \ + -path src/lib/events/libevents -prune -o \ -path src/lib/matrix -prune -o \ -path src/lib/parameters/uthash -prune -o \ -path src/modules/gyro_fft/CMSIS_5 -prune -o \ diff --git a/Tools/compress.py b/Tools/compress.py new file mode 100755 index 0000000000..1cee672746 --- /dev/null +++ b/Tools/compress.py @@ -0,0 +1,20 @@ +#!/usr/bin/env python + +import argparse +import lzma + +parser = argparse.ArgumentParser(description="""Compress a file with xz""") +parser.add_argument('filename', metavar='file', help='Input file (output: file.xz)') + +args = parser.parse_args() +filename = args.filename + +def save_compressed(filename): + #create xz compressed version + xz_filename=filename+'.xz' + with lzma.open(xz_filename, 'wt', preset=9) as f: + with open(filename, 'r') as content_file: + f.write(content_file.read()) + +save_compressed(filename) + diff --git a/cmake/px4_add_board.cmake b/cmake/px4_add_board.cmake index ba41d44cf6..9eddceb907 100644 --- a/cmake/px4_add_board.cmake +++ b/cmake/px4_add_board.cmake @@ -215,8 +215,12 @@ function(px4_add_board) set(config_romfs_extra_dependencies) # additional embedded metadata if (NOT CONSTRAINED_FLASH AND NOT EXTERNAL_METADATA AND NOT ${PX4_BOARD_LABEL} STREQUAL "test") - list(APPEND romfs_extra_files ${PX4_BINARY_DIR}/parameters.json.xz) - list(APPEND romfs_extra_dependencies parameters_xml) + list(APPEND romfs_extra_files + ${PX4_BINARY_DIR}/parameters.json.xz + ${PX4_BINARY_DIR}/events/all_events.json.xz) + list(APPEND romfs_extra_dependencies + parameters_xml + events_json) endif() list(APPEND romfs_extra_files ${PX4_BINARY_DIR}/component_general.json.xz) list(APPEND romfs_extra_dependencies component_general_json) diff --git a/cmake/px4_add_library.cmake b/cmake/px4_add_library.cmake index d77af09281..8d56a69ae4 100644 --- a/cmake/px4_add_library.cmake +++ b/cmake/px4_add_library.cmake @@ -30,6 +30,7 @@ # POSSIBILITY OF SUCH DAMAGE. # ############################################################################ +include(px4_list_make_absolute) #============================================================================= # @@ -52,4 +53,6 @@ function(px4_add_library target) endif() set_property(GLOBAL APPEND PROPERTY PX4_MODULE_PATHS ${CMAKE_CURRENT_SOURCE_DIR}) + px4_list_make_absolute(ABS_SRCS ${CMAKE_CURRENT_SOURCE_DIR} ${ARGN}) + set_property(GLOBAL APPEND PROPERTY PX4_SRC_FILES ${ABS_SRCS}) endfunction() diff --git a/cmake/px4_add_module.cmake b/cmake/px4_add_module.cmake index ba3a74d9ec..fb2944b179 100644 --- a/cmake/px4_add_module.cmake +++ b/cmake/px4_add_module.cmake @@ -30,6 +30,7 @@ # POSSIBILITY OF SUCH DAMAGE. # ############################################################################ +include(px4_list_make_absolute) #============================================================================= # @@ -156,6 +157,8 @@ function(px4_add_module) target_link_libraries(${MODULE} PRIVATE prebuild_targets parameters_interface px4_layer px4_platform systemlib) set_property(GLOBAL APPEND PROPERTY PX4_MODULE_LIBRARIES ${MODULE}) set_property(GLOBAL APPEND PROPERTY PX4_MODULE_PATHS ${CMAKE_CURRENT_SOURCE_DIR}) + px4_list_make_absolute(ABS_SRCS ${CMAKE_CURRENT_SOURCE_DIR} ${SRCS}) + set_property(GLOBAL APPEND PROPERTY PX4_SRC_FILES ${ABS_SRCS}) endif() # set defaults if not set diff --git a/cmake/px4_list_make_absolute.cmake b/cmake/px4_list_make_absolute.cmake new file mode 100644 index 0000000000..e087040719 --- /dev/null +++ b/cmake/px4_list_make_absolute.cmake @@ -0,0 +1,58 @@ +############################################################################ +# +# Copyright (c) 2020 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +# cmake include guard +if(px4_list_make_absolute_included) + return() +endif(px4_list_make_absolute_included) +set(px4_list_make_absolute_included true) + +#============================================================================= +# +# px4_list_make_absolute +# +# prepend a prefix to each element in a list, if the element is not an abolute +# path +# +function(px4_list_make_absolute var prefix) + set(list_var "") + foreach(f ${ARGN}) + if(IS_ABSOLUTE ${f}) + list(APPEND list_var "${f}") + else() + list(APPEND list_var "${prefix}/${f}") + endif() + endforeach(f) + set(${var} "${list_var}" PARENT_SCOPE) +endfunction() + diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index b66c6e41b5..88c2c217ae 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -55,6 +55,7 @@ set(msg_files distance_sensor.msg ekf2_timestamps.msg ekf_gps_drift.msg + event.msg esc_report.msg esc_status.msg estimator_event_flags.msg diff --git a/msg/event.msg b/msg/event.msg new file mode 100644 index 0000000000..e8fe119ca7 --- /dev/null +++ b/msg/event.msg @@ -0,0 +1,11 @@ +# Events interface +uint64 timestamp # time since system start (microseconds) + +uint32 id # Event ID +uint16 sequence # Sequence number +uint8[25] arguments # (optional) arguments, depend on event id + +uint8 log_levels # Log levels: 4 bits MSB: internal, 4 bits LSB: external + +uint8 ORB_QUEUE_LENGTH = 8 + diff --git a/msg/tools/uorb_rtps_message_ids.yaml b/msg/tools/uorb_rtps_message_ids.yaml index f72e3e0985..4aba00b4d4 100644 --- a/msg/tools/uorb_rtps_message_ids.yaml +++ b/msg/tools/uorb_rtps_message_ids.yaml @@ -341,6 +341,8 @@ rtps: id: 158 - msg: estimator_event_flags id: 159 + - msg: event + id: 160 ########## multi topics: begin ########## - msg: actuator_controls_0 id: 180 diff --git a/platforms/common/CMakeLists.txt b/platforms/common/CMakeLists.txt index 53fea9fbc1..c9803450c3 100644 --- a/platforms/common/CMakeLists.txt +++ b/platforms/common/CMakeLists.txt @@ -41,6 +41,7 @@ endif() add_library(px4_platform board_identity.c + events.cpp external_reset_lockout.cpp i2c.cpp i2c_spi_buses.cpp diff --git a/platforms/common/events.cpp b/platforms/common/events.cpp new file mode 100644 index 0000000000..b710f5a67c --- /dev/null +++ b/platforms/common/events.cpp @@ -0,0 +1,70 @@ +/**************************************************************************** + * + * Copyright (c) 2020 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include + +#include +#include +#include +#include + +static orb_advert_t orb_event_pub = nullptr; +static pthread_mutex_t publish_event_mutex = PTHREAD_MUTEX_INITIALIZER; +static uint16_t event_sequence{events::initial_event_sequence}; + +namespace events +{ + +void send(EventType &event) +{ + event.timestamp = hrt_absolute_time(); + + // We need some synchronization here because: + // - modifying orb_event_pub + // - the update of event_sequence needs to be atomic + // - we need to ensure ordering of the sequence numbers: the sequence we set here + // has to be the one published next. + pthread_mutex_lock(&publish_event_mutex); + event.sequence = ++event_sequence; // Set the sequence here so we're able to detect uORB queue overflows + + if (orb_event_pub != nullptr) { + orb_publish(ORB_ID(event), orb_event_pub, &event); + + } else { + orb_event_pub = orb_advertise_queue(ORB_ID(event), &event, event_s::ORB_QUEUE_LENGTH); + } + + pthread_mutex_unlock(&publish_event_mutex); +} + +} /* namespace events */ diff --git a/platforms/common/include/libevents_definitions.h b/platforms/common/include/libevents_definitions.h new file mode 100644 index 0000000000..4706a20693 --- /dev/null +++ b/platforms/common/include/libevents_definitions.h @@ -0,0 +1,48 @@ +/**************************************************************************** + * + * Copyright (c) 2020 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/* + * This header defines the events::EventType type. + */ + +#pragma once + +#include + +namespace events +{ +using EventType = event_s; +} // namespace events + + + diff --git a/platforms/common/include/px4_platform_common/events.h b/platforms/common/include/px4_platform_common/events.h new file mode 100644 index 0000000000..a5aed0b732 --- /dev/null +++ b/platforms/common/include/px4_platform_common/events.h @@ -0,0 +1,132 @@ +/**************************************************************************** + * + * Copyright (c) 2020 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +// Console event output can be enabled if needed. +// Note: this will only print custom (PX4) events, not from common. +// Helpful for debugging and to ensure the system is not spammed with events. +// It does not print arguments. +#if 0 +#include +#define CONSOLE_PRINT_EVENT(log_level, id, str) PX4_INFO_RAW("Event 0x%08x: %s\n", id, str) +#else +#define CONSOLE_PRINT_EVENT(log_level, id, str) +#endif + +#include +#include + +#include + +namespace events +{ + +namespace util +{ + +// source: https://gist.github.com/ruby0x1/81308642d0325fd386237cfa3b44785c +constexpr uint32_t val_32_const = 0x811c9dc5; +constexpr uint32_t prime_32_const = 0x1000193; +constexpr uint64_t val_64_const = 0xcbf29ce484222325; +constexpr uint64_t prime_64_const = 0x100000001b3; +inline constexpr uint32_t hash_32_fnv1a_const(const char *const str, const uint32_t value = val_32_const) noexcept +{ + return (str[0] == '\0') ? value : hash_32_fnv1a_const(&str[1], (value ^ uint32_t(str[0])) * prime_32_const); +} + +template +inline constexpr void fillEventArguments(uint8_t *buf, T arg) +{ + // This assumes we're on little-endian + memcpy(buf, &arg, sizeof(T)); +} + +template +inline constexpr void fillEventArguments(uint8_t *buf, T arg, Args... args) +{ + fillEventArguments(buf, arg); + fillEventArguments(buf + sizeof(T), args...); +} + +constexpr unsigned sizeofArguments() { return 0; } + +template +constexpr unsigned sizeofArguments(const T &t, const Args &... args) +{ + return sizeof(T) + sizeofArguments(args...); +} + +} // namespace util + + +/** + * publish/send an event + */ +void send(EventType &event); + +/** + * Generate event ID from an event name + */ +template +constexpr uint32_t ID(const char (&name)[N]) +{ + // Note: the generated ID must match with the python generator under Tools/px4events + uint32_t component_id = 1u << 24; // autopilot component + return (0xffffff & util::hash_32_fnv1a_const(name)) | component_id; +} + +template +inline void send(uint32_t id, const LogLevels &log_levels, const char *message, Args... args) +{ + EventType e{}; + e.log_levels = ((uint8_t)log_levels.internal << 4) | (uint8_t)log_levels.external; + e.id = id; + static_assert(util::sizeofArguments(args...) <= sizeof(e.arguments), "Too many arguments"); + util::fillEventArguments((uint8_t *)e.arguments, args...); + CONSOLE_PRINT_EVENT(e.log_level_external, e.id, message); + send(e); +} + +inline void send(uint32_t id, const LogLevels &log_levels, const char *message) +{ + EventType e{}; + e.log_levels = ((uint8_t)log_levels.internal << 4) | (uint8_t)log_levels.external; + e.id = id; + CONSOLE_PRINT_EVENT(e.log_level_external, e.id, message); + send(e); +} + +static constexpr uint16_t initial_event_sequence = 65535 - 10; // initialize with a high number so it wraps soon + +} // namespace events diff --git a/src/lib/component_information/CMakeLists.txt b/src/lib/component_information/CMakeLists.txt index 8e08b29ae8..09f524335c 100644 --- a/src/lib/component_information/CMakeLists.txt +++ b/src/lib/component_information/CMakeLists.txt @@ -36,9 +36,11 @@ # 1: COMP_METADATA_TYPE_PARAMETER # 2: COMP_METADATA_TYPE_COMMANDS # 3: COMP_METADATA_TYPE_PERIPHERALS +# 4: COMP_METADATA_TYPE_EVENTS set(comp_metadata_types) set(comp_metadata_board "${PX4_BOARD_VENDOR}_${PX4_BOARD_MODEL}_${PX4_BOARD_LABEL}") set(s3_url "https://px4-travis.s3.amazonaws.com") + set(comp_metadata_param_uri_board "${s3_url}/Firmware/{version}/${comp_metadata_board}/parameters.json.xz") list(FIND config_romfs_extra_dependencies "parameters_xml" index) if (${index} EQUAL -1) @@ -52,6 +54,19 @@ endif() # arguments: type, metadata file, uri, fallback uri, optional translation uri list(APPEND comp_metadata_types "--type" "1,${PX4_BINARY_DIR}/parameters.json.xz,${comp_metadata_param_uri},${comp_metadata_param_uri_fallback},") + +set(comp_metadata_events_uri_board "${s3_url}/Firmware/{version}/${comp_metadata_board}/all_events.json.xz") +list(FIND config_romfs_extra_dependencies "events_json" index) +if (${index} EQUAL -1) + set(comp_metadata_events_uri ${comp_metadata_events_uri_board}) + # use generic URL as fallback + set(comp_metadata_events_uri_fallback "${s3_url}/Firmware/{version}/_general/all_events.json.xz") +else() + set(comp_metadata_events_uri "mftp://etc/extras/all_events.json.xz") + set(comp_metadata_events_uri_fallback ${comp_metadata_events_uri_board}) +endif() +list(APPEND comp_metadata_types "--type" "4,${PX4_BINARY_DIR}/events/all_events.json.xz,${comp_metadata_events_uri},${comp_metadata_events_uri_fallback},") + set(component_general_json ${PX4_BINARY_DIR}/component_general.json) add_custom_command(OUTPUT ${component_general_json} ${component_general_json}.xz COMMAND ${PYTHON_EXECUTABLE} ${CMAKE_CURRENT_SOURCE_DIR}/generate_component_general.py @@ -63,6 +78,8 @@ add_custom_command(OUTPUT ${component_general_json} ${component_general_json}.xz generate_component_general.py ${PX4_BINARY_DIR}/parameters.json.xz parameters_xml + ${PX4_BINARY_DIR}/events/all_events.json.xz + events_json ver_gen COMMENT "Generating component_general.json" ) diff --git a/src/lib/events/CMakeLists.txt b/src/lib/events/CMakeLists.txt new file mode 100644 index 0000000000..1c4e991e5f --- /dev/null +++ b/src/lib/events/CMakeLists.txt @@ -0,0 +1,95 @@ +############################################################################ +# +# Copyright (c) 2020 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +px4_add_git_submodule(TARGET git_libevents PATH "libevents") + +get_property(all_px4_src_files GLOBAL PROPERTY PX4_SRC_FILES) + +set(generated_events_dir ${PX4_BINARY_DIR}/events) +set(generated_events_px4_file ${generated_events_dir}/px4.json) +add_custom_command(OUTPUT ${generated_events_px4_file} + COMMAND ${CMAKE_COMMAND} -E make_directory ${generated_events_dir} + COMMAND ${PYTHON_EXECUTABLE} ${PX4_SOURCE_DIR}/Tools/px_process_events.py + --src-path ${all_px4_src_files} + --json ${generated_events_px4_file} #--verbose + DEPENDS + ${all_px4_src_files} + ${PX4_SOURCE_DIR}/Tools/px_process_events.py + ${PX4_SOURCE_DIR}/Tools/px4events/jsonout.py + ${PX4_SOURCE_DIR}/Tools/px4events/srcparser.py + ${PX4_SOURCE_DIR}/Tools/px4events/srcscanner.py + COMMENT "Generating px4 event json file from source" +) +add_custom_target(events_px4_json DEPENDS ${generated_events_px4_file}) + +set(generated_events_file ${generated_events_dir}/all_events.json) +add_custom_command(OUTPUT ${generated_events_file} ${generated_events_file}.xz + COMMAND ${PYTHON_EXECUTABLE} libevents/scripts/combine.py + ${generated_events_px4_file} + libevents/events/common.json + --output ${generated_events_file} + COMMAND ${PYTHON_EXECUTABLE} libevents/scripts/validate.py + ${generated_events_file} + COMMAND ${PYTHON_EXECUTABLE} ${PX4_SOURCE_DIR}/Tools/compress.py + ${generated_events_file} + DEPENDS + ${generated_events_px4_file} + libevents/events/common.json + libevents/scripts/combine.py + libevents/scripts/validate.py + ${PX4_SOURCE_DIR}/Tools/compress.py + WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR} + COMMENT "Generating combined event json file" +) +add_custom_target(events_json DEPENDS ${generated_events_file}) + +# generate header from common.json +# (we don't add the definitions from the source as they are not needed and to +# avoid additional dependencies) +set(generated_events_header ${generated_events_dir}/events_generated.h) +add_custom_command(OUTPUT ${generated_events_header} + COMMAND ${CMAKE_COMMAND} -E make_directory ${generated_events_dir} + COMMAND ${PYTHON_EXECUTABLE} libevents/scripts/generate.py + --template libevents/libs/cpp/templates/events_generated.h.jinja + --output ${generated_events_dir} + libevents/events/common.json + DEPENDS + libevents/events/common.json + libevents/libs/cpp/templates/events_generated.h.jinja + libevents/scripts/generate.py + WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR} + COMMENT "Generating px4 event header file" +) +add_custom_target(events_header DEPENDS ${generated_events_header}) +add_dependencies(prebuild_targets events_header) + diff --git a/src/lib/events/libevents b/src/lib/events/libevents new file mode 160000 index 0000000000..5fd19be22d --- /dev/null +++ b/src/lib/events/libevents @@ -0,0 +1 @@ +Subproject commit 5fd19be22dcbeaccb8afeba146f3a7c931a9b5fb diff --git a/src/modules/logger/logger.cpp b/src/modules/logger/logger.cpp index e748dd5b25..10e22febcf 100644 --- a/src/modules/logger/logger.cpp +++ b/src/modules/logger/logger.cpp @@ -55,6 +55,7 @@ #include #include #include +#include #include #include #include @@ -363,6 +364,7 @@ Logger::Logger(LogWriter::Backend backend, size_t buffer_size, uint32_t log_inte ModuleParams(nullptr), _log_mode(log_mode), _log_name_timestamp(log_name_timestamp), + _event_subscription(ORB_ID::event), _writer(backend, buffer_size), _log_interval(log_interval) { @@ -578,6 +580,10 @@ void Logger::run() } } + if (_event_subscription.get_topic()->o_size > max_msg_size) { + max_msg_size = _event_subscription.get_topic()->o_size; + } + max_msg_size += sizeof(ulog_message_data_header_s); if (sizeof(ulog_message_logging_s) > (size_t)max_msg_size) { @@ -756,6 +762,9 @@ void Logger::run() } } + // check for new events + handle_event_updates(total_bytes); + // check for new logging message(s) log_message_s log_message; @@ -923,6 +932,66 @@ void Logger::debug_print_buffer(uint32_t &total_bytes, hrt_abstime &timer_start) #endif /* DBGPRINT */ } +bool Logger::handle_event_updates(uint32_t &total_bytes) +{ + bool data_written = false; + + while (_event_subscription.updated()) { + event_s *orb_event = (event_s *)(_msg_buffer + sizeof(ulog_message_data_header_s)); + _event_subscription.copy(orb_event); + + // Important: we can only access single-byte values in orb_event (it's not necessarily aligned) + if (events::internalLogLevel(orb_event->log_levels) == events::LogLevelInternal::Disabled) { + ++_event_sequence_offset; // skip this event + + } else { + // adjust sequence number + uint16_t updated_sequence; + memcpy(&updated_sequence, &orb_event->sequence, sizeof(updated_sequence)); + updated_sequence -= _event_sequence_offset; + memcpy(&orb_event->sequence, &updated_sequence, sizeof(updated_sequence)); + + size_t msg_size = sizeof(ulog_message_data_header_s) + _event_subscription.get_topic()->o_size_no_padding; + uint16_t write_msg_size = static_cast(msg_size - ULOG_MSG_HEADER_LEN); + uint16_t write_msg_id = _event_subscription.msg_id; + //write one byte after another (because of alignment) + _msg_buffer[0] = (uint8_t)write_msg_size; + _msg_buffer[1] = (uint8_t)(write_msg_size >> 8); + _msg_buffer[2] = static_cast(ULogMessageType::DATA); + _msg_buffer[3] = (uint8_t)write_msg_id; + _msg_buffer[4] = (uint8_t)(write_msg_id >> 8); + + // full log + if (write_message(LogType::Full, _msg_buffer, msg_size)) { + +#ifdef DBGPRINT + total_bytes += msg_size; +#endif /* DBGPRINT */ + + data_written = true; + } + + // mission log: only warnings or higher + if (events::internalLogLevel(orb_event->log_levels) <= events::LogLevelInternal::Warning) { + if (_writer.is_started(LogType::Mission)) { + memcpy(&updated_sequence, &orb_event->sequence, sizeof(updated_sequence)); + updated_sequence -= _event_sequence_offset_mission; + memcpy(&orb_event->sequence, &updated_sequence, sizeof(updated_sequence)); + + if (write_message(LogType::Mission, _msg_buffer, msg_size)) { + data_written = true; + } + } + + } else { + ++_event_sequence_offset_mission; // skip this event + } + } + } + + return data_written; +} + void Logger::publish_logger_status() { if (hrt_elapsed_time(&_logger_status_last) >= 1_s) { @@ -1573,6 +1642,8 @@ void Logger::write_formats(LogType type) write_format(type, *sub.get_topic(), written_formats, msg, i); } + write_format(type, *_event_subscription.get_topic(), written_formats, msg, sub_count); + _writer.unlock(); } @@ -1597,6 +1668,8 @@ void Logger::write_all_add_logged_msg(LogType type) } } + write_add_logged_msg(type, _event_subscription); // always add, even if not valid + _writer.unlock(); if (!added_subscriptions) { diff --git a/src/modules/logger/logger.h b/src/modules/logger/logger.h index f73497b513..7bf990b828 100644 --- a/src/modules/logger/logger.h +++ b/src/modules/logger/logger.h @@ -313,6 +313,11 @@ private: void publish_logger_status(); + /** + * Check for events and log them + */ + bool handle_event_updates(uint32_t &total_bytes); + uint8_t *_msg_buffer{nullptr}; int _msg_buffer_len{0}; @@ -331,6 +336,9 @@ private: int _num_subscriptions{0}; MissionSubscription _mission_subscriptions[MAX_MISSION_TOPICS_NUM] {}; ///< additional data for mission subscriptions int _num_mission_subs{0}; + LoggerSubscription _event_subscription; ///< Subscription for the event topic (handled separately) + uint16_t _event_sequence_offset{0}; ///< event sequence offset to account for skipped (not logged) messages + uint16_t _event_sequence_offset_mission{0}; LogWriter _writer; uint32_t _log_interval{0}; diff --git a/src/modules/mavlink/CMakeLists.txt b/src/modules/mavlink/CMakeLists.txt index 2556b17ea8..62f2075acb 100644 --- a/src/modules/mavlink/CMakeLists.txt +++ b/src/modules/mavlink/CMakeLists.txt @@ -46,6 +46,7 @@ px4_add_module( SRCS mavlink.c mavlink_command_sender.cpp + mavlink_events.cpp mavlink_ftp.cpp mavlink_log_handler.cpp mavlink_main.cpp diff --git a/src/modules/mavlink/mavlink_events.cpp b/src/modules/mavlink/mavlink_events.cpp new file mode 100644 index 0000000000..016e8e6bb8 --- /dev/null +++ b/src/modules/mavlink/mavlink_events.cpp @@ -0,0 +1,232 @@ +/**************************************************************************** + * + * Copyright (c) 2019 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include "mavlink_events.h" +#include "mavlink_main.h" + +#include +#include + +namespace events +{ + +EventBuffer::EventBuffer(int capacity) + : _capacity(capacity) +{ + pthread_mutex_init(&_mutex, nullptr); +} + +EventBuffer::~EventBuffer() +{ + delete[](_events); + pthread_mutex_destroy(&_mutex); +} + +int EventBuffer::init() +{ + if (_events) { return 0; } + + _events = new Event[_capacity]; + + if (!_events) { + return -ENOMEM; + } + + return 0; +} + +void EventBuffer::insert_event(const Event &event) +{ + pthread_mutex_lock(&_mutex); + _events[_next] = event; + _next = (_next + 1) % _capacity; + + if (_size < _capacity) { + ++_size; + } + + _latest_sequence.store(event.sequence); + pthread_mutex_unlock(&_mutex); +} + +uint16_t EventBuffer::get_oldest_sequence_after(uint16_t sequence) const +{ + pthread_mutex_lock(&_mutex); + uint16_t sequence_ret = _latest_sequence.load(); + uint16_t min_diff = UINT16_MAX; + + for (int i = 0; i < _size; ++i) { + uint16_t event_seq = _events[i].sequence; + uint16_t diff = event_seq - sequence; + + // this handles wrap-arounds correctly + if (event_seq != sequence && diff < min_diff) { + min_diff = diff; + sequence_ret = event_seq; + } + } + + pthread_mutex_unlock(&_mutex); + return sequence_ret; +} +bool EventBuffer::get_event(uint16_t sequence, Event &event) const +{ + pthread_mutex_lock(&_mutex); + + for (int count = 0; count < _size; ++count) { + int index = (_next - 1 - count + _size) % _size; + + if (_events[index].sequence == sequence) { + event = _events[index]; + pthread_mutex_unlock(&_mutex); + return true; + } + } + + pthread_mutex_unlock(&_mutex); + return false; +} + +int EventBuffer::size() const +{ + pthread_mutex_lock(&_mutex); + int size = _size; + pthread_mutex_unlock(&_mutex); + return size; +} + +SendProtocol::SendProtocol(EventBuffer &buffer, Mavlink &mavlink) + : _buffer(buffer), _latest_sequence(buffer.get_latest_sequence()), _mavlink(mavlink) +{ +} + +void SendProtocol::update(const hrt_abstime &now) +{ + // check for new events in the buffer + uint16_t buffer_sequence = _buffer.get_latest_sequence(); + int num_drops = 0; + + while (_latest_sequence != buffer_sequence) { + // only send if enough tx buffer space available + if (_mavlink.get_free_tx_buf() < MAVLINK_MSG_ID_EVENT_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) { + break; + } + + PX4_DEBUG("Changed seq: %i, latest: %i (mavlink instance: %i)", buffer_sequence, _latest_sequence, + _mavlink.get_instance_id()); + ++_latest_sequence; + Event e; + + if (_buffer.get_event(_latest_sequence, e)) { + send_event(e); + + } else { + if (num_drops == 0) { // avoid console spamming + // This happens if either an event dropped in uORB or update() is not called fast enough + PX4_WARN("Event dropped (%i, %i)", (int)_latest_sequence, buffer_sequence); + } + + ++num_drops; + } + } + + if (num_drops > 1) { + PX4_WARN("Dropped %i events (seq=%i)", num_drops, _latest_sequence); + } + + if (now - _last_current_sequence_sent > current_sequence_interval) { + send_current_sequence(now); + } +} + +void SendProtocol::handle_request_event(const mavlink_message_t &msg) const +{ + mavlink_request_event_t request_event; + mavlink_msg_request_event_decode(&msg, &request_event); + Event e; + + const uint16_t end_sequence = request_event.last_sequence + 1; + + for (uint16_t sequence = request_event.first_sequence; sequence != end_sequence; ++sequence) { + if (_buffer.get_event(sequence, e)) { + PX4_DEBUG("sending requested event %i", sequence); + send_event(e); + + } else { + mavlink_response_event_error_t event_error{}; + event_error.target_system = msg.sysid; + event_error.target_component = msg.compid; + event_error.sequence = sequence; + event_error.sequence_oldest_available = _buffer.get_oldest_sequence_after(sequence); + event_error.reason = MAV_EVENT_ERROR_REASON_UNAVAILABLE; + PX4_DEBUG("Event unavailable (seq=%i oldest=%i)", sequence, event_error.sequence_oldest_available); + mavlink_msg_response_event_error_send_struct(_mavlink.get_channel(), &event_error); + } + } +} + +void SendProtocol::send_event(const Event &event) const +{ + mavlink_event_t event_msg{}; + event_msg.event_time_boot_ms = event.timestamp_ms; + event_msg.destination_component = MAV_COMP_ID_ALL; + event_msg.destination_system = 0; + event_msg.id = event.id; + event_msg.sequence = event.sequence; + event_msg.log_levels = event.log_levels; + static_assert(sizeof(event_msg.arguments) >= sizeof(event.arguments), "MAVLink message arguments buffer too small"); + memcpy(&event_msg.arguments, event.arguments, sizeof(event.arguments)); + mavlink_msg_event_send_struct(_mavlink.get_channel(), &event_msg); + +} + +void SendProtocol::on_gcs_connected() +{ + send_current_sequence(hrt_absolute_time()); +} + +void SendProtocol::send_current_sequence(const hrt_abstime &now) +{ + // only send if enough tx buffer space available + if (_mavlink.get_free_tx_buf() < MAVLINK_MSG_ID_CURRENT_EVENT_SEQUENCE_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) { + return; + } + + _last_current_sequence_sent = now; + mavlink_current_event_sequence_t current_event_seq; + current_event_seq.sequence = _buffer.get_latest_sequence(); + current_event_seq.flags = _buffer.size() == 0 ? MAV_EVENT_CURRENT_SEQUENCE_FLAGS_RESET : 0; + mavlink_msg_current_event_sequence_send_struct(_mavlink.get_channel(), ¤t_event_seq); +} + +} /* namespace events */ diff --git a/src/modules/mavlink/mavlink_events.h b/src/modules/mavlink/mavlink_events.h new file mode 100644 index 0000000000..6e1dfe0d8d --- /dev/null +++ b/src/modules/mavlink/mavlink_events.h @@ -0,0 +1,146 @@ +/**************************************************************************** + * + * Copyright (c) 2019 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +#include + +#include "mavlink_bridge_header.h" +#include +#include +#include +#include + +using namespace time_literals; + +class Mavlink; + +namespace events +{ + +/** + * @struct Event + * Stores all data required for sending an event. This needs to be kept as small as possible, + * so that we can buffer more events + */ +struct Event { + uint32_t timestamp_ms; + uint32_t id; + uint16_t sequence; + uint8_t log_levels; ///< Log levels: 4 bits MSB: internal, 4 bits LSB: external + uint8_t arguments[MAX_ARGUMENTS_SIZE]; +}; + +/** + * @class EventBuffer + * Event buffer that can be shared between threads and multiple SendProtocol instances. + * All methods are thread-safe. + */ +class EventBuffer +{ +public: + + /** + * Create an event buffer. Required memory: sizeof(Event) * capacity. + * @param capacity maximum number of buffered events + */ + EventBuffer(int capacity = 20); + ~EventBuffer(); + + int init(); + + uint16_t get_latest_sequence() const { return _latest_sequence.load(); } + uint16_t get_oldest_sequence_after(uint16_t sequence) const; + + /** + * Insert a new event. It's expect to have a later sequence number than the + * already inserted events. + */ + void insert_event(const Event &event); + + bool get_event(uint16_t sequence, Event &event) const; + + int size() const; +private: + ::px4::atomic _latest_sequence{events::initial_event_sequence}; + + Event *_events{nullptr}; ///< stored events, ringbuffer + int _capacity; + int _next{0}; ///< next element to use + int _size{0}; + + mutable pthread_mutex_t _mutex; +}; + +/** + * @class SendProtocol + * Handles sending of events + */ +class SendProtocol +{ +public: + SendProtocol(EventBuffer &buffer, Mavlink &mavlink); + + /** + * Handle sending of new events by checking the event buffer. Should be called + * regularly. + * @param now current time + */ + void update(const hrt_abstime &now); + + /** + * Handle mavlink_request_event_t message. Can be called from another thread than + * the rest of the class and is therefore thread-safe. + */ + void handle_request_event(const mavlink_message_t &msg) const; + + /** + * Should be called whenever a GCS is connected + */ + void on_gcs_connected(); + +private: + + void send_event(const Event &event) const; + void send_current_sequence(const hrt_abstime &now); + + static constexpr hrt_abstime current_sequence_interval{3_s}; + + EventBuffer &_buffer; + uint16_t _latest_sequence; + hrt_abstime _last_current_sequence_sent{0}; + Mavlink &_mavlink; +}; + + +} /* namespace events */ diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index c06c3d0633..c828ae90ef 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -54,6 +54,7 @@ #include #include +#include #include "mavlink_receiver.h" #include "mavlink_main.h" @@ -78,6 +79,7 @@ #define MAIN_LOOP_DELAY 10000 ///< 100 Hz @ 1000 bytes/s data rate static pthread_mutex_t mavlink_module_mutex = PTHREAD_MUTEX_INITIALIZER; +events::EventBuffer *Mavlink::_event_buffer = nullptr; Mavlink *mavlink_module_instances[MAVLINK_COMM_NUM_BUFFERS] {}; @@ -329,6 +331,9 @@ Mavlink::destroy_all_instances() delete inst_to_del; } + delete _event_buffer; + _event_buffer = nullptr; + printf("\n"); PX4_INFO("all instances stopped"); return OK; @@ -2240,6 +2245,14 @@ Mavlink::task_main(int argc, char *argv[]) _receiver.start(); + /* Events subscription: only the first MAVLink instance should check */ + uORB::Subscription event_sub{ORB_ID(event)}; + const bool should_check_events = _instance_id == 0; + uint16_t event_sequence_offset = 0; // offset to account for skipped events, not sent via MAVLink + // ensure topic exists, otherwise we might lose first queued events + orb_advertise_queue(ORB_ID(event), nullptr, event_s::ORB_QUEUE_LENGTH); + event_sub.subscribe(); + _mavlink_start_time = hrt_absolute_time(); while (!_task_should_exit) { @@ -2448,6 +2461,30 @@ Mavlink::task_main(int argc, char *argv[]) } } + /* handle new events */ + if (should_check_events) { + event_s orb_event; + + while (event_sub.update(&orb_event)) { + if (events::externalLogLevel(orb_event.log_levels) == events::LogLevel::Disabled) { + ++event_sequence_offset; // skip this event + + } else { + events::Event e; + e.id = orb_event.id; + e.timestamp_ms = orb_event.timestamp / 1000; + e.sequence = orb_event.sequence - event_sequence_offset; + e.log_levels = orb_event.log_levels; + static_assert(sizeof(e.arguments) == sizeof(orb_event.arguments), + "uorb message event: arguments size mismatch"); + memcpy(e.arguments, orb_event.arguments, sizeof(orb_event.arguments)); + _event_buffer->insert_event(e); + } + } + } + + _events.update(t); + /* pass messages from other UARTs */ if (_forwarding_on) { @@ -2732,6 +2769,22 @@ Mavlink::start(int argc, char *argv[]) MavlinkULog::initialize(); MavlinkCommandSender::initialize(); + if (!_event_buffer) { + _event_buffer = new events::EventBuffer(); + int ret; + + if (_event_buffer && (ret = _event_buffer->init()) != 0) { + PX4_ERR("EventBuffer init failed (%i)", ret); + delete _event_buffer; + _event_buffer = nullptr; + } + + if (!_event_buffer) { + PX4_ERR("EventBuffer alloc failed"); + return 1; + } + } + // Wait for the instance count to go up one // before returning to the shell int ic = Mavlink::instance_count(); diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index 2cc5961571..e6d4e92e15 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -79,6 +79,7 @@ #include #include "mavlink_command_sender.h" +#include "mavlink_events.h" #include "mavlink_messages.h" #include "mavlink_receiver.h" #include "mavlink_shell.h" @@ -500,6 +501,7 @@ public: if (_mavlink_ulog) { _mavlink_ulog_stop_requested.store(true); } } + const events::SendProtocol &get_events_protocol() const { return _events; }; bool ftp_enabled() const { return _ftp_on; } bool hash_check_enabled() const { return _param_mav_hash_chk_en.get(); } @@ -568,6 +570,8 @@ private: MavlinkShell *_mavlink_shell{nullptr}; MavlinkULog *_mavlink_ulog{nullptr}; + static events::EventBuffer *_event_buffer; + events::SendProtocol _events{*_event_buffer, *this}; px4::atomic_bool _mavlink_ulog_stop_requested{false}; diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 2267467b9e..14d57b362b 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -288,6 +288,10 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) handle_message_gimbal_device_information(msg); break; + case MAVLINK_MSG_ID_REQUEST_EVENT: + handle_message_request_event(msg); + break; + default: break; } @@ -2834,6 +2838,11 @@ void MavlinkReceiver::CheckHeartbeats(const hrt_abstime &t, bool force) } } +void MavlinkReceiver::handle_message_request_event(mavlink_message_t *msg) +{ + _mavlink->get_events_protocol().handle_request_event(*msg); +} + void MavlinkReceiver::handle_message_gimbal_manager_set_manual_control(mavlink_message_t *msg) { diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index b76a2c1a7e..2f9da22783 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -196,6 +196,7 @@ private: void handle_message_debug_vect(mavlink_message_t *msg); void handle_message_named_value_float(mavlink_message_t *msg); #endif // !CONSTRAINED_FLASH + void handle_message_request_event(mavlink_message_t *msg); void CheckHeartbeats(const hrt_abstime &t, bool force = false); diff --git a/src/systemcmds/topic_listener/CMakeLists.txt b/src/systemcmds/topic_listener/CMakeLists.txt index c12972b95e..31c93a753b 100644 --- a/src/systemcmds/topic_listener/CMakeLists.txt +++ b/src/systemcmds/topic_listener/CMakeLists.txt @@ -53,7 +53,7 @@ px4_add_module( ${CMAKE_CURRENT_BINARY_DIR} SRCS listener_main.cpp - listener_generated.cpp + ${CMAKE_CURRENT_BINARY_DIR}/listener_generated.cpp DEPENDS generate_topic_listener )