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;