fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-04 08:00:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

mavlink: add kconfig option to disable UAVCAN parameter bridge

Browse Source 
- depends on DRIVERS_UAVCAN
This commit is contained in:
Daniel Agar
2023-03-08 19:30:06 -05:00
committed by GitHub
parent eb86cb85b7
commit 60b85c2e1a
3 changed files with 101 additions and 73 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mavlink/Kconfig
+9 -2
View File
@@ -8,8 +8,15 @@ menuconfig MAVLINK_DIALECT
depends on MODULES_MAVLINK
string "Mavlink dialect"
default "common"
help
Select the Mavlink dialect to generate and use.
---help---
Select the Mavlink dialect to generate and use.
menuconfig MAVLINK_UAVCAN_PARAMETERS
depends on MODULES_MAVLINK && DRIVERS_UAVCAN
bool "Mavlink UAVCAN parameter support"
default y
---help---
Expose UAVCAN parameters over Mavlink.
menuconfig USER_MAVLINK
bool "mavlink running as userspace module"
src/modules/mavlink/mavlink_parameters.cpp
+77 -61
View File
@@ -77,6 +77,8 @@ MavlinkParametersManager::handle_message(const mavlink_message_t *msg)
}
}
#if defined(CONFIG_MAVLINK_UAVCAN_PARAMETERS)
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.
@@ -88,6 +90,7 @@ MavlinkParametersManager::handle_message(const mavlink_message_t *msg)
_uavcan_parameter_request_pub.publish(req);
}
#endif // CONFIG_MAVLINK_UAVCAN_PARAMETERS
break;
}
@@ -133,6 +136,8 @@ MavlinkParametersManager::handle_message(const mavlink_message_t *msg)
}
}
#if defined(CONFIG_MAVLINK_UAVCAN_PARAMETERS)
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.
@@ -158,6 +163,7 @@ MavlinkParametersManager::handle_message(const mavlink_message_t *msg)
_uavcan_parameter_request_pub.publish(req);
}
#endif // CONFIG_MAVLINK_UAVCAN_PARAMETERS
break;
}
@@ -208,6 +214,8 @@ MavlinkParametersManager::handle_message(const mavlink_message_t *msg)
}
}
#if defined(CONFIG_MAVLINK_UAVCAN_PARAMETERS)
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.
@@ -224,6 +232,7 @@ MavlinkParametersManager::handle_message(const mavlink_message_t *msg)
request_next_uavcan_parameter();
}
#endif // CONFIG_MAVLINK_UAVCAN_PARAMETERS
break;
}
@@ -328,18 +337,22 @@ MavlinkParametersManager::send()
bool
MavlinkParametersManager::send_params()
{
#if defined(CONFIG_MAVLINK_UAVCAN_PARAMETERS)
if (send_uavcan()) {
return true;
}
} else if (send_one()) {
#endif // CONFIG_MAVLINK_UAVCAN_PARAMETERS
if (send_one()) {
return true;
} else if (send_untransmitted()) {
return true;
} else {
return false;
}
return false;
}
bool
@@ -393,63 +406,6 @@ MavlinkParametersManager::send_untransmitted()
return sent_one;
}
bool
MavlinkParametersManager::send_uavcan()
{
/* Send parameter values received from the UAVCAN topic */
uavcan_parameter_value_s value{};
if (_uavcan_parameter_value_sub.update(&value)) {
// Check if we received a matching parameter, drop it from the list and request the next
if ((_uavcan_open_request_list != nullptr)
&& (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;
#if defined(__GNUC__) && __GNUC__ >= 8
#pragma GCC diagnostic ignored "-Wstringop-truncation"
#endif
/*
* coverity[buffer_size_warning : FALSE]
*
* The MAVLink spec does not require the string to be NUL-terminated if it
* has length 16. In this case the receiving end needs to terminate it
* when copying it.
*/
strncpy(msg.param_id, value.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN);
#if defined(__GNUC__) && __GNUC__ >= 8
#pragma GCC diagnostic pop
#endif
if (value.param_type == MAV_PARAM_TYPE_REAL32) {
msg.param_type = MAVLINK_TYPE_FLOAT;
msg.param_value = value.real_value;
} else {
int32_t val = (int32_t)value.int_value;
memcpy(&msg.param_value, &val, sizeof(int32_t));
msg.param_type = MAVLINK_TYPE_INT32_T;
}
// Re-pack the message with the UAVCAN node ID
mavlink_message_t mavlink_packet{};
mavlink_msg_param_value_encode_chan(mavlink_system.sysid, value.node_id, _mavlink->get_channel(), &mavlink_packet,
&msg);
_mavlink_resend_uart(_mavlink->get_channel(), &mavlink_packet);
return true;
}
return false;
}
bool
MavlinkParametersManager::send_one()
{
@@ -591,6 +547,64 @@ MavlinkParametersManager::send_param(param_t param, int component_id)
return 0;
}
#if defined(CONFIG_MAVLINK_UAVCAN_PARAMETERS)
bool MavlinkParametersManager::send_uavcan()
{
/* Send parameter values received from the UAVCAN topic */
uavcan_parameter_value_s value{};
if (_uavcan_parameter_value_sub.update(&value)) {
// Check if we received a matching parameter, drop it from the list and request the next
if ((_uavcan_open_request_list != nullptr)
&& (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;
#if defined(__GNUC__) && __GNUC__ >= 8
#pragma GCC diagnostic ignored "-Wstringop-truncation"
#endif
/*
* coverity[buffer_size_warning : FALSE]
*
* The MAVLink spec does not require the string to be NUL-terminated if it
* has length 16. In this case the receiving end needs to terminate it
* when copying it.
*/
strncpy(msg.param_id, value.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN);
#if defined(__GNUC__) && __GNUC__ >= 8
#pragma GCC diagnostic pop
#endif
if (value.param_type == MAV_PARAM_TYPE_REAL32) {
msg.param_type = MAVLINK_TYPE_FLOAT;
msg.param_value = value.real_value;
} else {
int32_t val = (int32_t)value.int_value;
memcpy(&msg.param_value, &val, sizeof(int32_t));
msg.param_type = MAVLINK_TYPE_INT32_T;
}
// Re-pack the message with the UAVCAN node ID
mavlink_message_t mavlink_packet{};
mavlink_msg_param_value_encode_chan(mavlink_system.sysid, value.node_id, _mavlink->get_channel(), &mavlink_packet,
&msg);
_mavlink_resend_uart(_mavlink->get_channel(), &mavlink_packet);
return true;
}
return false;
}
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
@@ -643,3 +657,5 @@ void MavlinkParametersManager::dequeue_uavcan_request()
_uavcan_waiting_for_request_response = false;
}
}
#endif // CONFIG_MAVLINK_UAVCAN_PARAMETERS
src/modules/mavlink/mavlink_parameters.h
+15 -10
View File
@@ -49,11 +49,14 @@
#include <uORB/Subscription.hpp>
#include <uORB/SubscriptionInterval.hpp>
#include <uORB/topics/rc_parameter_map.h>
#include <uORB/topics/uavcan_parameter_request.h>
#include <uORB/topics/uavcan_parameter_value.h>
#include <uORB/topics/parameter_update.h>
#include <drivers/drv_hrt.h>
#if defined(CONFIG_MAVLINK_UAVCAN_PARAMETERS)
# include <uORB/topics/uavcan_parameter_request.h>
# include <uORB/topics/uavcan_parameter_value.h>
#endif // CONFIG_MAVLINK_UAVCAN_PARAMETERS
using namespace time_literals;
class Mavlink;
@@ -91,11 +94,6 @@ protected:
*/
bool send_params();
/**
* Send UAVCAN params
*/
bool send_uavcan();
/**
* Send untransmitted params
*/
@@ -103,6 +101,12 @@ protected:
int send_param(param_t param, int component_id = -1);
#if defined(CONFIG_MAVLINK_UAVCAN_PARAMETERS)
/**
* Send UAVCAN params
*/
bool send_uavcan();
// Item of a single-linked list to store requested uavcan parameters
struct _uavcan_open_request_list_item {
uavcan_parameter_request_s req;
@@ -128,9 +132,6 @@ protected:
bool _uavcan_waiting_for_request_response{false}; ///< We have reqested a parameter and wait for the response
uint16_t _uavcan_queued_request_items{0}; ///< Number of stored parameter requests currently in the list
uORB::Publication<rc_parameter_map_s> _rc_param_map_pub{ORB_ID(rc_parameter_map)};
rc_parameter_map_s _rc_param_map{};
uORB::Publication<uavcan_parameter_request_s> _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,
@@ -149,6 +150,10 @@ protected:
"uavcan_parameter_request_s MAV_PARAM_TYPE_INT64 constant mismatch");
uORB::Subscription _uavcan_parameter_value_sub{ORB_ID(uavcan_parameter_value)};
#endif // CONFIG_MAVLINK_UAVCAN_PARAMETERS
uORB::Publication<rc_parameter_map_s> _rc_param_map_pub{ORB_ID(rc_parameter_map)};
rc_parameter_map_s _rc_param_map{};
uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
hrt_abstime _param_update_time{0};