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

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

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 <px4_log.h>
+#include <errno.h>
+
+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(), &current_event_seq);
+}
+
+} /* namespace events */

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 <stdint.h>
+
+#include "mavlink_bridge_header.h"
+#include <drivers/drv_hrt.h>
+#include <px4_platform_common/atomic.h>
+#include <px4_platform_common/events.h>
+#include <pthread.h>
+
+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<uint16_t> _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 */

src/modules/mavlink/mavlink_main.cpp

@@ -54,6 +54,7 @@
 #include <lib/systemlib/mavlink_log.h>
 #include <lib/version/version.h>
 
+#include <uORB/topics/event.h>
 #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();

src/modules/mavlink/mavlink_main.h

@@ -79,6 +79,7 @@
 #include <uORB/topics/vehicle_status.h>
 
 #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};
 

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)
 {

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);