From 0d61e22d9b710ad8fe995eab46d7f19628aa1fb6 Mon Sep 17 00:00:00 2001 From: Michael Schaeuble Date: Wed, 15 Feb 2017 18:56:16 +0100 Subject: [PATCH] mavlink: Buffer UAVCAN parameter re-requests We buffer the mavlink messages and don't forward them directly via uORB. This solves an issue where many requests are dropped and QGC does not start properly in combination with UAVCAN devices. --- src/modules/mavlink/mavlink_parameters.cpp | 77 ++++++++++++++++++++-- src/modules/mavlink/mavlink_parameters.h | 18 +++++ 2 files changed, 89 insertions(+), 6 deletions(-) diff --git a/src/modules/mavlink/mavlink_parameters.cpp b/src/modules/mavlink/mavlink_parameters.cpp index 3bda4489c9..671d2a36d6 100644 --- a/src/modules/mavlink/mavlink_parameters.cpp +++ b/src/modules/mavlink/mavlink_parameters.cpp @@ -51,6 +51,9 @@ MavlinkParametersManager::MavlinkParametersManager(Mavlink *mavlink) : MavlinkStream(mavlink), _send_all_index(-1), + _uavcan_open_request_list(nullptr), + _uavcan_waiting_for_request_response(false), + _uavcan_queued_request_items(0), _rc_param_map_pub(nullptr), _rc_param_map(), _uavcan_parameter_request_pub(nullptr), @@ -258,12 +261,9 @@ MavlinkParametersManager::handle_message(const mavlink_message_t *msg) strncpy(req.param_id, req_read.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN + 1); req.param_id[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN] = '\0'; - if (_uavcan_parameter_request_pub == nullptr) { - _uavcan_parameter_request_pub = orb_advertise(ORB_ID(uavcan_parameter_request), &req); - - } else { - orb_publish(ORB_ID(uavcan_parameter_request), _uavcan_parameter_request_pub, &req); - } + // Enque the request and forward the first to the uavcan node + enque_uavcan_request(&req); + request_next_uavcan_parameter(); } break; @@ -333,6 +333,13 @@ MavlinkParametersManager::send(const hrt_abstime t) struct uavcan_parameter_value_s value; orb_copy(ORB_ID(uavcan_parameter_value), _uavcan_parameter_value_sub, &value); + // Check if we received a matching parameter, drop it from the list and request the next + if (value.param_index == _uavcan_open_request_list->req.param_index + && value.node_id == _uavcan_open_request_list->req.node_id) { + dequeue_uavcan_request(); + request_next_uavcan_parameter(); + } + mavlink_param_value_t msg; msg.param_count = value.param_count; msg.param_index = value.param_index; @@ -477,3 +484,61 @@ MavlinkParametersManager::send_param(param_t param, int component_id) return 0; } + +void MavlinkParametersManager::request_next_uavcan_parameter() +{ + // Request a parameter if we are not already waiting on a response and if the list is not empty + if (!_uavcan_waiting_for_request_response && _uavcan_open_request_list != nullptr) { + uavcan_parameter_request_s req = _uavcan_open_request_list->req; + + if (_uavcan_parameter_request_pub == nullptr) { + _uavcan_parameter_request_pub = orb_advertise_queue(ORB_ID(uavcan_parameter_request), &req, 5); + + } else { + orb_publish(ORB_ID(uavcan_parameter_request), _uavcan_parameter_request_pub, &req); + } + + _uavcan_waiting_for_request_response = true; + } +} + +void MavlinkParametersManager::enque_uavcan_request(uavcan_parameter_request_s *req) +{ + // We store at max 10 requests to keep memory consumption low. + // Dropped requests will be repeated by the ground station + if (_uavcan_queued_request_items >= 10) { + return; + } + + _uavcan_open_request_list_item *new_reqest = new _uavcan_open_request_list_item; + new_reqest->req = *req; + new_reqest->next = nullptr; + + _uavcan_open_request_list_item *item = _uavcan_open_request_list; + ++_uavcan_queued_request_items; + + if (item == nullptr) { + // Add the first item to the list + _uavcan_open_request_list = new_reqest; + + } else { + // Find the last item and add the new request at the end + while (item->next != nullptr) { + item = item->next; + } + + item->next = new_reqest; + } +} + +void MavlinkParametersManager::dequeue_uavcan_request() +{ + if (_uavcan_open_request_list != nullptr) { + // Drop the first item in the list and free the used memory + _uavcan_open_request_list_item *first = _uavcan_open_request_list; + _uavcan_open_request_list = first->next; + --_uavcan_queued_request_items; + delete first; + _uavcan_waiting_for_request_response = false; + } +} diff --git a/src/modules/mavlink/mavlink_parameters.h b/src/modules/mavlink/mavlink_parameters.h index b0de654563..9e0e875787 100644 --- a/src/modules/mavlink/mavlink_parameters.h +++ b/src/modules/mavlink/mavlink_parameters.h @@ -47,6 +47,7 @@ #include "mavlink_stream.h" #include #include +#include class MavlinkParametersManager : public MavlinkStream { @@ -92,6 +93,23 @@ protected: int send_param(param_t param, int component_id = -1); + // Item of a single-linked list to store requested uavcan parameters + struct _uavcan_open_request_list_item { + uavcan_parameter_request_s req; + struct _uavcan_open_request_list_item *next; + }; + + // Request the next uavcan parameter + void request_next_uavcan_parameter(); + // Enque one uavcan parameter reqest. We store 10 at max. + void enque_uavcan_request(uavcan_parameter_request_s *req); + // Drop the first reqest from the list + void dequeue_uavcan_request(); + + _uavcan_open_request_list_item *_uavcan_open_request_list; // Pointer to the first item in the linked list + bool _uavcan_waiting_for_request_response; // We have reqested a parameter and wait for the response + uint16_t _uavcan_queued_request_items; // Number of stored parameter requests currently in the list + orb_advert_t _rc_param_map_pub; struct rc_parameter_map_s _rc_param_map;