From 4e6f366ead87cd0c48430770af611f0762a4ef67 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Wed, 7 Apr 2021 20:37:06 -0400 Subject: [PATCH] WIP: parameter uORB access --- msg/CMakeLists.txt | 4 +- msg/parameter_request.msg | 26 +++++ ...arameter_value.msg => parameter_value.msg} | 14 ++- msg/uavcan_parameter_request.msg | 23 ---- src/drivers/uavcan/uavcan_servers.cpp | 28 ++--- src/drivers/uavcan/uavcan_servers.hpp | 4 +- src/lib/parameters/CMakeLists.txt | 2 + src/lib/parameters/ParametersServer.cpp | 109 ++++++++++++++++++ src/lib/parameters/ParametersServer.hpp | 62 ++++++++++ src/lib/parameters/parameters.cpp | 81 +------------ src/modules/mavlink/mavlink_parameters.cpp | 12 +- src/modules/mavlink/mavlink_parameters.h | 40 +++---- 12 files changed, 260 insertions(+), 145 deletions(-) create mode 100644 msg/parameter_request.msg rename msg/{uavcan_parameter_value.msg => parameter_value.msg} (56%) delete mode 100644 msg/uavcan_parameter_request.msg create mode 100644 src/lib/parameters/ParametersServer.cpp create mode 100644 src/lib/parameters/ParametersServer.hpp diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index b66c6e41b5..e27130a27d 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -148,8 +148,8 @@ set(msg_files trajectory_waypoint.msg transponder_report.msg tune_control.msg - uavcan_parameter_request.msg - uavcan_parameter_value.msg + parameter_request.msg + parameter_value.msg ulog_stream.msg ulog_stream_ack.msg vehicle_acceleration.msg diff --git a/msg/parameter_request.msg b/msg/parameter_request.msg new file mode 100644 index 0000000000..3d7852d304 --- /dev/null +++ b/msg/parameter_request.msg @@ -0,0 +1,26 @@ +# UAVCAN-MAVLink parameter bridge request type +uint64 timestamp # time since system start (microseconds) + +uint8 MESSAGE_TYPE_PARAM_REQUEST_READ = 20 # MAVLINK_MSG_ID_PARAM_REQUEST_READ +uint8 MESSAGE_TYPE_PARAM_REQUEST_LIST = 21 # MAVLINK_MSG_ID_PARAM_REQUEST_LIST +uint8 MESSAGE_TYPE_PARAM_SET = 23 # MAVLINK_MSG_ID_PARAM_SET +uint8 message_type # message type: PARAM_REQUEST_READ, PARAM_REQUEST_LIST, PARAM_SET + +uint8 NODE_ID_ALL = 0 # MAV_COMP_ID_ALL +uint8 node_id # UAVCAN node ID mapped from MAVLink component ID + +char[17] param_id # parameter name +int16 param_index # -1 if the param_id field should be used as identifier + +uint8 TYPE_UINT8 = 1 +uint8 TYPE_INT32 = 6 +uint8 TYPE_INT64 = 8 +uint8 TYPE_REAL32 = 9 +uint8 param_type # parameter type + +int64 int_value # current value if param_type is int-like +float32 real_value # current value if param_type is float-like + +uint8 ORB_QUEUE_LENGTH = 4 + +# TOPICS parameter_request uavcan_parameter_request diff --git a/msg/uavcan_parameter_value.msg b/msg/parameter_value.msg similarity index 56% rename from msg/uavcan_parameter_value.msg rename to msg/parameter_value.msg index 8eff663a57..fc3b595885 100644 --- a/msg/uavcan_parameter_value.msg +++ b/msg/parameter_value.msg @@ -1,9 +1,17 @@ # UAVCAN-MAVLink parameter bridge response type uint64 timestamp # time since system start (microseconds) + uint8 node_id # UAVCAN node ID mapped from MAVLink component ID + char[17] param_id # MAVLink/UAVCAN parameter name + int16 param_index # parameter index, if known + uint16 param_count # number of parameters exposed by the node -uint8 param_type # MAVLink parameter type -int64 int_value # current value if param_type is int-like -float32 real_value # current value if param_type is float-like + +uint8 param_type # parameter type + +int64 int_value # current value if param_type is int-like +float32 real_value # current value if param_type is float-like + +# TOPICS parameter_value uavcan_parameter_value diff --git a/msg/uavcan_parameter_request.msg b/msg/uavcan_parameter_request.msg deleted file mode 100644 index 3cfec15f81..0000000000 --- a/msg/uavcan_parameter_request.msg +++ /dev/null @@ -1,23 +0,0 @@ -# UAVCAN-MAVLink parameter bridge request type -uint64 timestamp # time since system start (microseconds) - -uint8 MESSAGE_TYPE_PARAM_REQUEST_READ = 20 # MAVLINK_MSG_ID_PARAM_REQUEST_READ -uint8 MESSAGE_TYPE_PARAM_REQUEST_LIST = 21 # MAVLINK_MSG_ID_PARAM_REQUEST_LIST -uint8 MESSAGE_TYPE_PARAM_SET = 23 # MAVLINK_MSG_ID_PARAM_SET -uint8 message_type # MAVLink message type: PARAM_REQUEST_READ, PARAM_REQUEST_LIST, PARAM_SET - -uint8 NODE_ID_ALL = 0 # MAV_COMP_ID_ALL -uint8 node_id # UAVCAN node ID mapped from MAVLink component ID - -char[17] param_id # MAVLink/UAVCAN parameter name -int16 param_index # -1 if the param_id field should be used as identifier - -uint8 PARAM_TYPE_UINT8 = 1 # MAV_PARAM_TYPE_UINT8 -uint8 PARAM_TYPE_INT64 = 8 # MAV_PARAM_TYPE_INT64 -uint8 PARAM_TYPE_REAL32 = 9 # MAV_PARAM_TYPE_REAL32 -uint8 param_type # MAVLink parameter type - -int64 int_value # current value if param_type is int-like -float32 real_value # current value if param_type is float-like - -uint8 ORB_QUEUE_LENGTH = 4 diff --git a/src/drivers/uavcan/uavcan_servers.cpp b/src/drivers/uavcan/uavcan_servers.cpp index 210887be43..4765c339d5 100644 --- a/src/drivers/uavcan/uavcan_servers.cpp +++ b/src/drivers/uavcan/uavcan_servers.cpp @@ -59,7 +59,7 @@ #include #include -#include +#include /** * @file uavcan_servers.cpp @@ -326,7 +326,7 @@ UavcanServers::run(pthread_addr_t) // Check for parameter requests (get/set/list) if (param_request_sub.updated() && !_param_list_in_progress && !_param_in_progress && !_count_in_progress) { - uavcan_parameter_request_s request{}; + parameter_request_s request{}; param_request_sub.copy(&request); if (_param_counts[request.node_id]) { @@ -334,7 +334,7 @@ UavcanServers::run(pthread_addr_t) * We know how many parameters are exposed by this node, so * process the request. */ - if (request.message_type == uavcan_parameter_request_s::MESSAGE_TYPE_PARAM_REQUEST_READ) { + if (request.message_type == parameter_request_s::MESSAGE_TYPE_PARAM_REQUEST_READ) { uavcan::protocol::param::GetSet::Request req; if (request.param_index >= 0) { @@ -354,7 +354,7 @@ UavcanServers::run(pthread_addr_t) _param_index = request.param_index; } - } else if (request.message_type == uavcan_parameter_request_s::MESSAGE_TYPE_PARAM_SET) { + } else if (request.message_type == parameter_request_s::MESSAGE_TYPE_PARAM_SET) { uavcan::protocol::param::GetSet::Request req; if (request.param_index >= 0) { @@ -364,10 +364,10 @@ UavcanServers::run(pthread_addr_t) req.name = (char *)request.param_id; } - if (request.param_type == uavcan_parameter_request_s::PARAM_TYPE_REAL32) { + if (request.param_type == parameter_request_s::TYPE_REAL32) { req.value.to() = request.real_value; - } else if (request.param_type == uavcan_parameter_request_s::PARAM_TYPE_UINT8) { + } else if (request.param_type == parameter_request_s::TYPE_UINT8) { req.value.to() = request.int_value; } else { @@ -387,7 +387,7 @@ UavcanServers::run(pthread_addr_t) _param_index = request.param_index; } - } else if (request.message_type == uavcan_parameter_request_s::MESSAGE_TYPE_PARAM_REQUEST_LIST) { + } else if (request.message_type == parameter_request_s::MESSAGE_TYPE_PARAM_REQUEST_LIST) { // This triggers the _param_list_in_progress case below. _param_index = 0; _param_list_in_progress = true; @@ -397,8 +397,8 @@ UavcanServers::run(pthread_addr_t) PX4_DEBUG("starting component-specific param list"); } - } else if (request.node_id == uavcan_parameter_request_s::NODE_ID_ALL) { - if (request.message_type == uavcan_parameter_request_s::MESSAGE_TYPE_PARAM_REQUEST_LIST) { + } else if (request.node_id == parameter_request_s::NODE_ID_ALL) { + if (request.message_type == parameter_request_s::MESSAGE_TYPE_PARAM_REQUEST_LIST) { /* * This triggers the _param_list_in_progress case below, * but additionally iterates over all active nodes. @@ -631,13 +631,13 @@ UavcanServers::cb_getset(const uavcan::ServiceCallResult(); } else if (param.value.is(uavcan::protocol::param::Value::Tag::real_value)) { - response.param_type = uavcan_parameter_request_s::PARAM_TYPE_REAL32; + response.param_type = parameter_request_s::TYPE_REAL32; response.real_value = param.value.to(); } else if (param.value.is(uavcan::protocol::param::Value::Tag::boolean_value)) { - response.param_type = uavcan_parameter_request_s::PARAM_TYPE_UINT8; + response.param_type = parameter_request_s::TYPE_UINT8; response.int_value = param.value.to(); } diff --git a/src/drivers/uavcan/uavcan_servers.hpp b/src/drivers/uavcan/uavcan_servers.hpp index 8c6716edcb..614f5e3312 100644 --- a/src/drivers/uavcan/uavcan_servers.hpp +++ b/src/drivers/uavcan/uavcan_servers.hpp @@ -38,7 +38,7 @@ #include #include #include -#include +#include #include #include @@ -162,7 +162,7 @@ private: bool _cmd_in_progress = false; // uORB topic handle for MAVLink parameter responses - uORB::Publication _param_response_pub{ORB_ID(uavcan_parameter_value)}; + uORB::Publication _param_response_pub{ORB_ID(uavcan_parameter_value)}; uORB::Publication _command_ack_pub{ORB_ID(vehicle_command_ack)}; typedef uavcan::MethodBinder + +#include "param.h" + +static ParametersServer *_param_autosave{nullptr}; + +float ParametersServer::last_autosave_elapsed() const +{ + return hrt_elapsed_time_atomic(&_last_autosave_timestamp) * 1e-6f; +} + +void ParametersServer::AutoSave() +{ + if (_param_autosave == nullptr) { + return; + } + + // wait at least 300ms before saving, because: + // - tasks often call param_set() for multiple params, so this avoids unnecessary save calls + // - the logger stores changed params. He gets notified on a param change via uORB and then + // looks at all unsaved params. + float delay = 0.3f; + + static constexpr float rate_limit = 2.0f; // rate-limit saving to 2 seconds + const float last_save_elapsed = _param_autosave->last_autosave_elapsed(); + + if (last_save_elapsed < rate_limit && rate_limit > last_save_elapsed + delay) { + delay = rate_limit - last_save_elapsed; + } + + uint64_t delay_us = delay * 1e6f; + _param_autosave->ScheduleDelayed(delay_us); +} + +bool ParametersServer::EnableAutoSave(bool enable) +{ + if (enable) { + if (_param_autosave == nullptr) { + _param_autosave = new ParametersServer(); + + if (_param_autosave == nullptr) { + PX4_ERR("ParametersServer alloc failed"); + return false; + } + } + + } else { + // TODO: how to prevent delete if currently running? + delete _param_autosave; + _param_autosave = nullptr; + } + + return true; +} + +void ParametersServer::print_status() +{ + if (_param_autosave) { + PX4_INFO("last auto save: %.3f seconds ago", (double)_param_autosave->last_autosave_elapsed()); + + } else { + PX4_INFO("auto save: off"); + } +} + +void ParametersServer::Run() +{ + _last_autosave_timestamp = hrt_absolute_time(); + int ret = param_save_default(); + + if (ret != 0) { + PX4_ERR("param auto save failed (%i)", ret); + } +} diff --git a/src/lib/parameters/ParametersServer.hpp b/src/lib/parameters/ParametersServer.hpp new file mode 100644 index 0000000000..1e23ac13fb --- /dev/null +++ b/src/lib/parameters/ParametersServer.hpp @@ -0,0 +1,62 @@ +/**************************************************************************** + * + * Copyright (c) 2021 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 + +/** + * Automatically save the parameters after a timeout and limited rate. + * + * This needs to be called with the writer lock held (it's not necessary that it's the writer lock, but it + * needs to be the same lock as autosave_worker() and param_control_autosave() use). + */ + +class ParametersServer : public px4::ScheduledWorkItem +{ +public: + ParametersServer() : px4::ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::lp_default) {} + ~ParametersServer() override = default; + + float last_autosave_elapsed() const; + + static void AutoSave(); + static bool EnableAutoSave(bool enable = true); + + static void print_status(); + +private: + hrt_abstime _last_autosave_timestamp{0}; + + void Run() override; +}; diff --git a/src/lib/parameters/parameters.cpp b/src/lib/parameters/parameters.cpp index a7745e45ea..4590b27d56 100644 --- a/src/lib/parameters/parameters.cpp +++ b/src/lib/parameters/parameters.cpp @@ -89,12 +89,7 @@ static char *param_user_file = nullptr; #define PARAM_CLOSE close #endif -#include -/* autosaving variables */ -static hrt_abstime last_autosave_timestamp = 0; -static struct work_s autosave_work {}; -static px4::atomic autosave_scheduled{false}; -static bool autosave_disabled = false; +#include "ParametersServer.hpp" static constexpr uint16_t param_info_count = sizeof(px4::parameters) / sizeof(param_info_s); static px4::AtomicBitset params_active; // params found @@ -199,6 +194,8 @@ param_init() param_find_perf = perf_alloc(PC_COUNT, "param: find"); param_get_perf = perf_alloc(PC_COUNT, "param: get"); param_set_perf = perf_alloc(PC_ELAPSED, "param: set"); + + param_control_autosave(true); } /** @@ -645,44 +642,6 @@ param_get_system_default_value(param_t param, void *default_val) return ret; } -/** - * worker callback method to save the parameters - * @param arg unused - */ -static void -autosave_worker(void *arg) -{ - bool disabled = false; - - if (!param_get_default_file()) { - // In case we save to FLASH, defer param writes until disarmed, - // as writing to FLASH can stall the entire CPU (in rare cases around 300ms on STM32F7) - uORB::SubscriptionData armed_sub{ORB_ID(actuator_armed)}; - - if (armed_sub.get().armed) { - work_queue(LPWORK, &autosave_work, (worker_t)&autosave_worker, nullptr, USEC2TICK(1_s)); - return; - } - } - - param_lock_writer(); - last_autosave_timestamp = hrt_absolute_time(); - autosave_scheduled.store(false); - disabled = autosave_disabled; - param_unlock_writer(); - - if (disabled) { - return; - } - - PX4_DEBUG("Autosaving params"); - int ret = param_save_default(); - - if (ret != 0) { - PX4_ERR("param auto save failed (%i)", ret); - } -} - /** * Automatically save the parameters after a timeout and limited rate. * @@ -692,38 +651,14 @@ autosave_worker(void *arg) static void param_autosave() { - if (autosave_scheduled.load() || autosave_disabled) { - return; - } - - // wait at least 300ms before saving, because: - // - tasks often call param_set() for multiple params, so this avoids unnecessary save calls - // - the logger stores changed params. He gets notified on a param change via uORB and then - // looks at all unsaved params. - hrt_abstime delay = 300_ms; - - static constexpr const hrt_abstime rate_limit = 2_s; // rate-limit saving to 2 seconds - const hrt_abstime last_save_elapsed = hrt_elapsed_time(&last_autosave_timestamp); - - if (last_save_elapsed < rate_limit && rate_limit > last_save_elapsed + delay) { - delay = rate_limit - last_save_elapsed; - } - - autosave_scheduled.store(true); - work_queue(LPWORK, &autosave_work, (worker_t)&autosave_worker, nullptr, USEC2TICK(delay)); + ParametersServer::AutoSave(); } void param_control_autosave(bool enable) { param_lock_writer(); - - if (!enable && autosave_scheduled.load()) { - work_cancel(LPWORK, &autosave_work); - autosave_scheduled.store(false); - } - - autosave_disabled = !enable; + ParametersServer::EnableAutoSave(enable); param_unlock_writer(); } @@ -1527,11 +1462,7 @@ void param_print_status() param_custom_default_values->n * sizeof(UT_icd)); } - PX4_INFO("auto save: %s", autosave_disabled ? "off" : "on"); - - if (!autosave_disabled && (last_autosave_timestamp > 0)) { - PX4_INFO("last auto save: %.3f seconds ago", hrt_elapsed_time(&last_autosave_timestamp) * 1e-6); - } + ParametersServer::print_status(); perf_print_counter(param_export_perf); perf_print_counter(param_find_perf); diff --git a/src/modules/mavlink/mavlink_parameters.cpp b/src/modules/mavlink/mavlink_parameters.cpp index eb0a31e41e..0ff1d70ad1 100644 --- a/src/modules/mavlink/mavlink_parameters.cpp +++ b/src/modules/mavlink/mavlink_parameters.cpp @@ -80,7 +80,7 @@ MavlinkParametersManager::handle_message(const mavlink_message_t *msg) if (req_list.target_system == mavlink_system.sysid && req_list.target_component < 127 && (req_list.target_component != mavlink_system.compid || req_list.target_component == MAV_COMP_ID_ALL)) { // publish list request to UAVCAN driver via uORB. - uavcan_parameter_request_s req{}; + parameter_request_s req{}; req.message_type = msg->msgid; req.node_id = req_list.target_component; req.param_index = 0; @@ -140,7 +140,7 @@ MavlinkParametersManager::handle_message(const mavlink_message_t *msg) if (set.target_system == mavlink_system.sysid && set.target_component < 127 && (set.target_component != mavlink_system.compid || set.target_component == MAV_COMP_ID_ALL)) { // publish set request to UAVCAN driver via uORB. - uavcan_parameter_request_s req{}; + parameter_request_s req{}; req.message_type = msg->msgid; req.node_id = set.target_component; req.param_index = -1; @@ -219,7 +219,7 @@ MavlinkParametersManager::handle_message(const mavlink_message_t *msg) if (req_read.target_system == mavlink_system.sysid && req_read.target_component < 127 && (req_read.target_component != mavlink_system.compid || req_read.target_component == MAV_COMP_ID_ALL)) { // publish set request to UAVCAN driver via uORB. - uavcan_parameter_request_s req{}; + parameter_request_s req{}; req.timestamp = hrt_absolute_time(); req.message_type = msg->msgid; req.node_id = req_read.target_component; @@ -406,7 +406,7 @@ bool MavlinkParametersManager::send_uavcan() { /* Send parameter values received from the UAVCAN topic */ - uavcan_parameter_value_s value{}; + parameter_value_s value{}; if (_uavcan_parameter_value_sub.update(&value)) { @@ -593,7 +593,7 @@ 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; + parameter_request_s req = _uavcan_open_request_list->req; _uavcan_parameter_request_pub.publish(req); @@ -601,7 +601,7 @@ void MavlinkParametersManager::request_next_uavcan_parameter() } } -void MavlinkParametersManager::enque_uavcan_request(uavcan_parameter_request_s *req) +void MavlinkParametersManager::enque_uavcan_request(parameter_request_s *req) { // We store at max 10 requests to keep memory consumption low. // Dropped requests will be repeated by the ground station diff --git a/src/modules/mavlink/mavlink_parameters.h b/src/modules/mavlink/mavlink_parameters.h index d55ed8e029..5d30c9ffba 100644 --- a/src/modules/mavlink/mavlink_parameters.h +++ b/src/modules/mavlink/mavlink_parameters.h @@ -49,8 +49,8 @@ #include #include #include -#include -#include +#include +#include #include #include @@ -105,8 +105,8 @@ protected: // 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; + parameter_request_s req{}; + _uavcan_open_request_list_item *next{nullptr}; }; /** @@ -117,7 +117,7 @@ protected: /** * Enqueue one uavcan parameter reqest. We store 10 at max. */ - void enque_uavcan_request(uavcan_parameter_request_s *req); + void enque_uavcan_request(parameter_request_s *req); /** * Drop the first reqest from the list @@ -131,22 +131,22 @@ protected: uORB::Publication _rc_param_map_pub{ORB_ID(rc_parameter_map)}; rc_parameter_map_s _rc_param_map{}; - uORB::Publication _uavcan_parameter_request_pub{ORB_ID(uavcan_parameter_request)}; + uORB::Publication _uavcan_parameter_request_pub{ORB_ID(uavcan_parameter_request)}; // enforce ORB_ID(uavcan_parameter_request) constants that map to MAVLINK defines - static_assert(uavcan_parameter_request_s::MESSAGE_TYPE_PARAM_REQUEST_READ == MAVLINK_MSG_ID_PARAM_REQUEST_READ, - "uavcan_parameter_request_s MAVLINK_MSG_ID_PARAM_REQUEST_READ constant mismatch"); - static_assert(uavcan_parameter_request_s::MESSAGE_TYPE_PARAM_SET == MAVLINK_MSG_ID_PARAM_SET, - "uavcan_parameter_request_s MAVLINK_MSG_ID_PARAM_SET constant mismatch"); - static_assert(uavcan_parameter_request_s::MESSAGE_TYPE_PARAM_REQUEST_LIST == MAVLINK_MSG_ID_PARAM_REQUEST_LIST, - "uavcan_parameter_request_s MAVLINK_MSG_ID_PARAM_REQUEST_LIST constant mismatch"); - static_assert(uavcan_parameter_request_s::NODE_ID_ALL == MAV_COMP_ID_ALL, - "uavcan_parameter_request_s MAV_COMP_ID_ALL constant mismatch"); - static_assert(uavcan_parameter_request_s::PARAM_TYPE_UINT8 == MAV_PARAM_TYPE_UINT8, - "uavcan_parameter_request_s MAV_PARAM_TYPE_UINT8 constant mismatch"); - static_assert(uavcan_parameter_request_s::PARAM_TYPE_REAL32 == MAV_PARAM_TYPE_REAL32, - "uavcan_parameter_request_s MAV_PARAM_TYPE_REAL32 constant mismatch"); - static_assert(uavcan_parameter_request_s::PARAM_TYPE_INT64 == MAV_PARAM_TYPE_INT64, - "uavcan_parameter_request_s MAV_PARAM_TYPE_INT64 constant mismatch"); + static_assert(parameter_request_s::MESSAGE_TYPE_PARAM_REQUEST_READ == MAVLINK_MSG_ID_PARAM_REQUEST_READ, + "parameter_request_s MAVLINK_MSG_ID_PARAM_REQUEST_READ constant mismatch"); + static_assert(parameter_request_s::MESSAGE_TYPE_PARAM_SET == MAVLINK_MSG_ID_PARAM_SET, + "parameter_request_s MAVLINK_MSG_ID_PARAM_SET constant mismatch"); + static_assert(parameter_request_s::MESSAGE_TYPE_PARAM_REQUEST_LIST == MAVLINK_MSG_ID_PARAM_REQUEST_LIST, + "parameter_request_s MAVLINK_MSG_ID_PARAM_REQUEST_LIST constant mismatch"); + static_assert(parameter_request_s::NODE_ID_ALL == MAV_COMP_ID_ALL, + "parameter_request_s MAV_COMP_ID_ALL constant mismatch"); + static_assert(parameter_request_s::TYPE_UINT8 == MAV_PARAM_TYPE_UINT8, + "parameter_request_s MAV_PARAM_TYPE_UINT8 constant mismatch"); + static_assert(parameter_request_s::TYPE_REAL32 == MAV_PARAM_TYPE_REAL32, + "parameter_request_s MAV_PARAM_TYPE_REAL32 constant mismatch"); + static_assert(parameter_request_s::TYPE_INT64 == MAV_PARAM_TYPE_INT64, + "parameter_request_s MAV_PARAM_TYPE_INT64 constant mismatch"); uORB::Subscription _uavcan_parameter_value_sub{ORB_ID(uavcan_parameter_value)};