mirror of
https://github.com/PX4/PX4-Autopilot.git
synced 2026-05-22 14:24:21 +08:00
Added MAVLink/UAVCAN parameter bridge; implemented UAVCAN ESC enumeration
This commit is contained in:
Ben Dyer
@@ -0,0 +1,8 @@
|
||||
# UAVCAN-MAVLink parameter bridge request type
|
||||
uint8 message_type # MAVLink message type: PARAM_REQUEST_READ, PARAM_REQUEST_LIST, PARAM_SET
|
||||
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 # 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
|
||||
@@ -0,0 +1,8 @@
|
||||
# UAVCAN-MAVLink parameter bridge response type
|
||||
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
|
||||
@@ -41,25 +41,25 @@
|
||||
|
||||
#include <stdio.h>
|
||||
|
||||
#include <uORB/topics/uavcan_parameter_request.h>
|
||||
#include <uORB/topics/uavcan_parameter_value.h>
|
||||
|
||||
#include "mavlink_parameters.h"
|
||||
#include "mavlink_main.h"
|
||||
|
||||
MavlinkParametersManager::MavlinkParametersManager(Mavlink *mavlink) : MavlinkStream(mavlink),
|
||||
_send_all_index(-1),
|
||||
_rc_param_map_pub(nullptr),
|
||||
_rc_param_map()
|
||||
_rc_param_map(),
|
||||
_uavcan_parameter_request_pub(nullptr),
|
||||
_uavcan_parameter_value_sub(-1)
|
||||
{
|
||||
}
|
||||
|
||||
unsigned
|
||||
MavlinkParametersManager::get_size()
|
||||
{
|
||||
if (_send_all_index >= 0) {
|
||||
return MAVLINK_MSG_ID_PARAM_VALUE_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
|
||||
|
||||
} else {
|
||||
return 0;
|
||||
}
|
||||
return MAVLINK_MSG_ID_PARAM_VALUE_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
|
||||
}
|
||||
|
||||
void
|
||||
@@ -76,36 +76,75 @@ MavlinkParametersManager::handle_message(const mavlink_message_t *msg)
|
||||
|
||||
_send_all_index = 0;
|
||||
}
|
||||
|
||||
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;
|
||||
req.message_type = msg->msgid;
|
||||
req.node_id = req_list.target_component;
|
||||
req.param_index = 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);
|
||||
}
|
||||
}
|
||||
break;
|
||||
}
|
||||
|
||||
case MAVLINK_MSG_ID_PARAM_SET: {
|
||||
/* set parameter */
|
||||
if (msg->msgid == MAVLINK_MSG_ID_PARAM_SET) {
|
||||
mavlink_param_set_t set;
|
||||
mavlink_msg_param_set_decode(msg, &set);
|
||||
mavlink_param_set_t set;
|
||||
mavlink_msg_param_set_decode(msg, &set);
|
||||
|
||||
if (set.target_system == mavlink_system.sysid &&
|
||||
(set.target_component == mavlink_system.compid || set.target_component == MAV_COMP_ID_ALL)) {
|
||||
if (set.target_system == mavlink_system.sysid &&
|
||||
(set.target_component == mavlink_system.compid || set.target_component == MAV_COMP_ID_ALL)) {
|
||||
|
||||
/* local name buffer to enforce null-terminated string */
|
||||
char name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN + 1];
|
||||
strncpy(name, set.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN);
|
||||
/* enforce null termination */
|
||||
name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN] = '\0';
|
||||
/* attempt to find parameter, set and send it */
|
||||
param_t param = param_find_no_notification(name);
|
||||
/* local name buffer to enforce null-terminated string */
|
||||
char name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN + 1];
|
||||
strncpy(name, set.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN);
|
||||
/* enforce null termination */
|
||||
name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN] = '\0';
|
||||
/* attempt to find parameter, set and send it */
|
||||
param_t param = param_find_no_notification(name);
|
||||
|
||||
if (param == PARAM_INVALID) {
|
||||
char buf[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN];
|
||||
sprintf(buf, "[pm] unknown param: %s", name);
|
||||
_mavlink->send_statustext_info(buf);
|
||||
if (param == PARAM_INVALID) {
|
||||
char buf[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN];
|
||||
sprintf(buf, "[pm] unknown param: %s", name);
|
||||
_mavlink->send_statustext_info(buf);
|
||||
|
||||
} else {
|
||||
/* set and send parameter */
|
||||
param_set(param, &(set.param_value));
|
||||
send_param(param);
|
||||
}
|
||||
} else {
|
||||
/* set and send parameter */
|
||||
param_set(param, &(set.param_value));
|
||||
send_param(param);
|
||||
}
|
||||
}
|
||||
|
||||
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;
|
||||
req.message_type = msg->msgid;
|
||||
req.node_id = set.target_component;
|
||||
req.param_index = -1;
|
||||
strncpy(req.param_id, set.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN + 1);
|
||||
req.param_id[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN] = '\0';
|
||||
if (set.param_type == MAV_PARAM_TYPE_REAL32) {
|
||||
req.param_type = MAV_PARAM_TYPE_REAL32;
|
||||
req.real_value = set.param_value;
|
||||
} else {
|
||||
int32_t val;
|
||||
memcpy(&val, &set.param_value, sizeof(int32_t));
|
||||
req.param_type = MAV_PARAM_TYPE_INT64;
|
||||
req.int_value = val;
|
||||
}
|
||||
|
||||
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);
|
||||
}
|
||||
}
|
||||
break;
|
||||
@@ -144,6 +183,23 @@ 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;
|
||||
req.message_type = msg->msgid;
|
||||
req.node_id = req_read.target_component;
|
||||
req.param_index = req_read.param_index;
|
||||
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);
|
||||
}
|
||||
}
|
||||
break;
|
||||
}
|
||||
|
||||
@@ -192,11 +248,38 @@ MavlinkParametersManager::handle_message(const mavlink_message_t *msg)
|
||||
void
|
||||
MavlinkParametersManager::send(const hrt_abstime t)
|
||||
{
|
||||
/* send all parameters if requested, but only after the system has booted */
|
||||
if (_send_all_index >= 0 && _mavlink->boot_complete()) {
|
||||
bool space_available = _mavlink->get_free_tx_buf() >= get_size();
|
||||
|
||||
/* Send parameter values received from the UAVCAN topic */
|
||||
if (_uavcan_parameter_value_sub < 0) {
|
||||
_uavcan_parameter_value_sub = orb_subscribe(ORB_ID(uavcan_parameter_value));
|
||||
}
|
||||
|
||||
bool param_value_ready;
|
||||
orb_check(_uavcan_parameter_value_sub, ¶m_value_ready);
|
||||
if (space_available && param_value_ready) {
|
||||
struct uavcan_parameter_value_s value;
|
||||
orb_copy(ORB_ID(uavcan_parameter_value), _uavcan_parameter_value_sub, &value);
|
||||
|
||||
mavlink_param_value_t msg;
|
||||
msg.param_count = value.param_count;
|
||||
msg.param_index = value.param_index;
|
||||
strncpy(msg.param_id, value.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN);
|
||||
if (value.param_type == MAV_PARAM_TYPE_REAL32) {
|
||||
msg.param_type = MAVLINK_TYPE_FLOAT;
|
||||
msg.param_value = value.real_value;
|
||||
} else {
|
||||
int32_t val;
|
||||
val = (int32_t)value.int_value;
|
||||
memcpy(&msg.param_value, &val, sizeof(int32_t));
|
||||
msg.param_type = MAVLINK_TYPE_INT32_T;
|
||||
}
|
||||
_mavlink->send_message(MAVLINK_MSG_ID_PARAM_VALUE, &msg, value.node_id);
|
||||
} else if (_send_all_index >= 0 && _mavlink->boot_complete()) {
|
||||
/* send all parameters if requested, but only after the system has booted */
|
||||
|
||||
/* skip if no space is available */
|
||||
if (_mavlink->get_free_tx_buf() < get_size()) {
|
||||
if (!space_available) {
|
||||
return;
|
||||
}
|
||||
|
||||
|
||||
@@ -118,4 +118,7 @@ protected:
|
||||
|
||||
orb_advert_t _rc_param_map_pub;
|
||||
struct rc_parameter_map_s _rc_param_map;
|
||||
|
||||
orb_advert_t _uavcan_parameter_request_pub;
|
||||
int _uavcan_parameter_value_sub;
|
||||
};
|
||||
|
||||
@@ -1,53 +0,0 @@
|
||||
############################################################################
|
||||
#
|
||||
# Copyright (c) 2014 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.
|
||||
#
|
||||
############################################################################
|
||||
|
||||
#
|
||||
# System state machine tests.
|
||||
#
|
||||
|
||||
MODULE_COMMAND = mavlink_tests
|
||||
SRCS = mavlink_tests.cpp \
|
||||
mavlink_ftp_test.cpp \
|
||||
../mavlink_stream.cpp \
|
||||
../mavlink_ftp.cpp \
|
||||
../mavlink.c
|
||||
|
||||
INCLUDE_DIRS += $(MAVLINK_SRC)/include/mavlink
|
||||
|
||||
MODULE_STACKSIZE = 5000
|
||||
|
||||
MAXOPTIMIZATION = -Os
|
||||
|
||||
EXTRACXXFLAGS = -Weffc++ -DMAVLINK_FTP_UNIT_TEST -Wno-attributes -Wno-packed
|
||||
|
||||
EXTRACFLAGS = -Wno-packed
|
||||
File diff suppressed because it is too large
Load Diff
@@ -42,16 +42,20 @@
|
||||
#include <uavcan/node/sub_node.hpp>
|
||||
#include <uavcan/protocol/node_status_monitor.hpp>
|
||||
|
||||
# include <uavcan/protocol/dynamic_node_id_server/centralized.hpp>
|
||||
# include <uavcan/protocol/node_info_retriever.hpp>
|
||||
# include <uavcan_posix/basic_file_server_backend.hpp>
|
||||
# include <uavcan/protocol/firmware_update_trigger.hpp>
|
||||
# include <uavcan/protocol/file_server.hpp>
|
||||
# include <uavcan_posix/dynamic_node_id_server/file_event_tracer.hpp>
|
||||
# include <uavcan_posix/dynamic_node_id_server/file_storage_backend.hpp>
|
||||
# include <uavcan_posix/firmware_version_checker.hpp>
|
||||
#include <uavcan/protocol/dynamic_node_id_server/centralized.hpp>
|
||||
#include <uavcan/protocol/node_info_retriever.hpp>
|
||||
#include <uavcan_posix/basic_file_server_backend.hpp>
|
||||
#include <uavcan/protocol/firmware_update_trigger.hpp>
|
||||
#include <uavcan/protocol/file_server.hpp>
|
||||
#include <uavcan_posix/dynamic_node_id_server/file_event_tracer.hpp>
|
||||
#include <uavcan_posix/dynamic_node_id_server/file_storage_backend.hpp>
|
||||
#include <uavcan_posix/firmware_version_checker.hpp>
|
||||
#include <uavcan/equipment/esc/RawCommand.hpp>
|
||||
#include <uavcan/equipment/indication/BeepCommand.hpp>
|
||||
#include <uavcan/protocol/enumeration/Begin.hpp>
|
||||
#include <uavcan/protocol/enumeration/Indication.hpp>
|
||||
|
||||
# include "uavcan_virtual_can_driver.hpp"
|
||||
#include "uavcan_virtual_can_driver.hpp"
|
||||
|
||||
/**
|
||||
* @file uavcan_servers.hpp
|
||||
@@ -140,7 +144,71 @@ private:
|
||||
uavcan::FirmwareUpdateTrigger _fw_upgrade_trigger;
|
||||
uavcan::BasicFileServer _fw_server;
|
||||
|
||||
/*
|
||||
* The MAVLink parameter bridge needs to know the maximum parameter index
|
||||
* of each node so that clients can determine when parameter listings have
|
||||
* finished. We do that by querying a node's entire parameter set whenever
|
||||
* a parameter message is received for a node with a zero _param_count,
|
||||
* and storing the count here. If a node doesn't respond, or doesn't have
|
||||
* any parameters, its count will stay at zero and we'll try to query the
|
||||
* set again next time.
|
||||
*
|
||||
* The node's UAVCAN ID is used as the index into the _param_counts array.
|
||||
*/
|
||||
uint16_t _param_counts[128];
|
||||
bool _count_in_progress;
|
||||
uint16_t _count_index;
|
||||
|
||||
bool _param_in_progress;
|
||||
uint16_t _param_index;
|
||||
bool _param_list_in_progress;
|
||||
bool _param_list_all_nodes;
|
||||
uavcan::NodeID _param_list_node_id;
|
||||
|
||||
bool _cmd_in_progress;
|
||||
|
||||
// uORB topic handle for MAVLink parameter responses
|
||||
orb_advert_t _param_response_pub;
|
||||
|
||||
typedef uavcan::MethodBinder<UavcanServers *,
|
||||
void (UavcanServers::*)(const uavcan::ServiceCallResult<uavcan::protocol::param::GetSet> &)> GetSetCallback;
|
||||
typedef uavcan::MethodBinder<UavcanServers *,
|
||||
void (UavcanServers::*)(const uavcan::ServiceCallResult<uavcan::protocol::param::ExecuteOpcode> &)> ExecuteOpcodeCallback;
|
||||
typedef uavcan::MethodBinder<UavcanServers *,
|
||||
void (UavcanServers::*)(const uavcan::ServiceCallResult<uavcan::protocol::RestartNode> &)> RestartNodeCallback;
|
||||
void cb_getset(const uavcan::ServiceCallResult<uavcan::protocol::param::GetSet> &result);
|
||||
void cb_count(const uavcan::ServiceCallResult<uavcan::protocol::param::GetSet> &result);
|
||||
void cb_opcode(const uavcan::ServiceCallResult<uavcan::protocol::param::ExecuteOpcode> &result);
|
||||
void cb_restart(const uavcan::ServiceCallResult<uavcan::protocol::RestartNode> &result);
|
||||
|
||||
uavcan::ServiceClient<uavcan::protocol::param::GetSet, GetSetCallback> _param_getset_client;
|
||||
void param_count(uavcan::NodeID node_id);
|
||||
|
||||
uavcan::NodeID get_next_active_node_id(const uavcan::NodeID &base);
|
||||
|
||||
bool _mutex_inited;
|
||||
volatile bool _check_fw;
|
||||
|
||||
// ESC enumeration
|
||||
bool _esc_enumeration_active;
|
||||
uint8_t _esc_enumeration_ids[uavcan::equipment::esc::RawCommand::FieldTypes::cmd::MaxSize];
|
||||
uint8_t _esc_enumeration_index;
|
||||
uint8_t _esc_set_index;
|
||||
uint8_t _esc_count;
|
||||
|
||||
typedef uavcan::MethodBinder<UavcanServers *,
|
||||
void (UavcanServers::*)(const uavcan::ServiceCallResult<uavcan::protocol::enumeration::Begin> &)> EnumerationBeginCallback;
|
||||
typedef uavcan::MethodBinder<UavcanServers *,
|
||||
void (UavcanServers::*)(const uavcan::ReceivedDataStructure<uavcan::protocol::enumeration::Indication>&)>
|
||||
EnumerationIndicationCallback;
|
||||
void cb_enumeration_begin(const uavcan::ServiceCallResult<uavcan::protocol::enumeration::Begin> &result);
|
||||
void cb_enumeration_indication(const uavcan::ReceivedDataStructure<uavcan::protocol::enumeration::Indication> &msg);
|
||||
void cb_enumeration_getset(const uavcan::ServiceCallResult<uavcan::protocol::param::GetSet> &result);
|
||||
void cb_enumeration_save(const uavcan::ServiceCallResult<uavcan::protocol::param::ExecuteOpcode> &result);
|
||||
|
||||
uavcan::Publisher<uavcan::equipment::indication::BeepCommand> _beep_pub;
|
||||
uavcan::Subscriber<uavcan::protocol::enumeration::Indication, EnumerationIndicationCallback> _enumeration_indication_sub;
|
||||
uavcan::ServiceClient<uavcan::protocol::enumeration::Begin, EnumerationBeginCallback> _enumeration_client;
|
||||
uavcan::ServiceClient<uavcan::protocol::param::GetSet, GetSetCallback> _enumeration_getset_client;
|
||||
uavcan::ServiceClient<uavcan::protocol::param::ExecuteOpcode, ExecuteOpcodeCallback> _enumeration_save_client;
|
||||
};
|
||||
|
||||
Reference in New Issue
Block a user