mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-22 17:53:34 +08:00
drivers/uavcan: make firmware server available on both CAN1/CAN2 always (if enabled)
- uavcan firmware server no longer shuts down when arming (nodes might restart in flight) - always handle UAVCAN parameters with or without the FW server active - remove legacy ESC enumeration in FW server
This commit is contained in:
Daniel Agar
@@ -1,6 +1,6 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2014-2017 PX4 Development Team. All rights reserved.
|
||||
* Copyright (c) 2014-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
|
||||
@@ -51,16 +51,11 @@
|
||||
|
||||
#include "uavcan_main.hpp"
|
||||
#include "uavcan_servers.hpp"
|
||||
#include "uavcan_virtual_can_driver.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 <uORB/topics/vehicle_command.h>
|
||||
#include <uORB/topics/vehicle_command_ack.h>
|
||||
#include <uORB/topics/uavcan_parameter_request.h>
|
||||
|
||||
/**
|
||||
* @file uavcan_servers.cpp
|
||||
*
|
||||
@@ -70,147 +65,22 @@
|
||||
* David Sidrane <david_s5@nscdg.com>
|
||||
*/
|
||||
|
||||
/*
|
||||
* UavcanNode
|
||||
*/
|
||||
|
||||
UavcanServers *UavcanServers::_instance;
|
||||
|
||||
UavcanServers::UavcanServers(uavcan::INode &main_node) :
|
||||
_vdriver(NumIfaces, UAVCAN_DRIVER::SystemClock::instance(), main_node.getAllocator(), VirtualIfaceBlockAllocationQuota),
|
||||
_subnode(_vdriver, UAVCAN_DRIVER::SystemClock::instance(), main_node.getAllocator()),
|
||||
_main_node(main_node),
|
||||
_server_instance(_subnode, _storage_backend, _tracer),
|
||||
_fileserver_backend(_subnode),
|
||||
_node_info_retriever(_subnode),
|
||||
_fw_upgrade_trigger(_subnode, _fw_version_checker),
|
||||
_fw_server(_subnode, _fileserver_backend),
|
||||
_param_getset_client(_subnode),
|
||||
_param_opcode_client(_subnode),
|
||||
_param_restartnode_client(_subnode),
|
||||
_beep_pub(_subnode),
|
||||
_enumeration_indication_sub(_subnode),
|
||||
_enumeration_client(_subnode),
|
||||
_enumeration_getset_client(_subnode),
|
||||
_enumeration_save_client(_subnode)
|
||||
UavcanServers::UavcanServers(uavcan::INode &node, uavcan::NodeInfoRetriever &node_info_retriever) :
|
||||
_server_instance(node, _storage_backend, _tracer),
|
||||
_fileserver_backend(node),
|
||||
_fw_upgrade_trigger(node, _fw_version_checker),
|
||||
_fw_server(node, _fileserver_backend),
|
||||
_node_info_retriever(node_info_retriever)
|
||||
{
|
||||
}
|
||||
|
||||
UavcanServers::~UavcanServers()
|
||||
int UavcanServers::init()
|
||||
{
|
||||
if (_mutex_inited) {
|
||||
(void)Lock::deinit(_subnode_mutex);
|
||||
}
|
||||
|
||||
_main_node.getDispatcher().removeRxFrameListener();
|
||||
}
|
||||
|
||||
int
|
||||
UavcanServers::stop()
|
||||
{
|
||||
UavcanServers *server = instance();
|
||||
|
||||
if (server == nullptr) {
|
||||
PX4_INFO("Already stopped");
|
||||
return -1;
|
||||
}
|
||||
|
||||
if (server->_subnode_thread) {
|
||||
PX4_INFO("stopping fw srv thread...");
|
||||
server->_subnode_thread_should_exit.store(true);
|
||||
(void)pthread_join(server->_subnode_thread, NULL);
|
||||
}
|
||||
|
||||
_instance = nullptr;
|
||||
|
||||
server->_main_node.getDispatcher().removeRxFrameListener();
|
||||
|
||||
delete server;
|
||||
return 0;
|
||||
}
|
||||
|
||||
int
|
||||
UavcanServers::start(uavcan::INode &main_node)
|
||||
{
|
||||
if (_instance != nullptr) {
|
||||
PX4_INFO("Already started");
|
||||
return -1;
|
||||
}
|
||||
|
||||
/*
|
||||
* Node init
|
||||
*/
|
||||
_instance = new UavcanServers(main_node);
|
||||
|
||||
if (_instance == nullptr) {
|
||||
PX4_ERR("Out of memory");
|
||||
return -2;
|
||||
}
|
||||
|
||||
int rv = _instance->init();
|
||||
|
||||
if (rv < 0) {
|
||||
PX4_ERR("Node init failed: %d", rv);
|
||||
delete _instance;
|
||||
_instance = nullptr;
|
||||
return rv;
|
||||
}
|
||||
|
||||
/*
|
||||
* Start the thread. Normally it should never exit.
|
||||
*/
|
||||
pthread_attr_t tattr;
|
||||
struct sched_param param;
|
||||
|
||||
pthread_attr_init(&tattr);
|
||||
(void)pthread_attr_getschedparam(&tattr, ¶m);
|
||||
tattr.stacksize = PX4_STACK_ADJUSTED(StackSize);
|
||||
param.sched_priority = Priority;
|
||||
|
||||
if (pthread_attr_setschedparam(&tattr, ¶m)) {
|
||||
PX4_ERR("setting sched params failed");
|
||||
}
|
||||
|
||||
static auto run_trampoline = [](void *) {return UavcanServers::_instance->run(_instance);};
|
||||
|
||||
rv = pthread_create(&_instance->_subnode_thread, &tattr, static_cast<pthread_startroutine_t>(run_trampoline), NULL);
|
||||
|
||||
if (rv != 0) {
|
||||
rv = -rv;
|
||||
PX4_ERR("pthread_create() failed: %d", rv);
|
||||
delete _instance;
|
||||
_instance = nullptr;
|
||||
}
|
||||
|
||||
return rv;
|
||||
}
|
||||
|
||||
int
|
||||
UavcanServers::init()
|
||||
{
|
||||
errno = 0;
|
||||
|
||||
/*
|
||||
* Initialize the mutex.
|
||||
* giving it its path
|
||||
*/
|
||||
int ret = Lock::init(_subnode_mutex);
|
||||
|
||||
if (ret < 0) {
|
||||
PX4_ERR("Lock init: %d", errno);
|
||||
return ret;
|
||||
}
|
||||
|
||||
_mutex_inited = true;
|
||||
|
||||
_subnode.setNodeID(_main_node.getNodeID());
|
||||
_main_node.getDispatcher().installRxFrameListener(&_vdriver);
|
||||
|
||||
/*
|
||||
* Initialize the fw version checker.
|
||||
* giving it its path
|
||||
*/
|
||||
ret = _fw_version_checker.createFwPaths(UAVCAN_FIRMWARE_PATH, UAVCAN_ROMFS_FW_PATH);
|
||||
int ret = _fw_version_checker.createFwPaths(UAVCAN_FIRMWARE_PATH, UAVCAN_ROMFS_FW_PATH);
|
||||
|
||||
if (ret < 0) {
|
||||
PX4_ERR("FirmwareVersionChecker init: %d, errno: %d", ret, errno);
|
||||
@@ -218,7 +88,6 @@ UavcanServers::init()
|
||||
}
|
||||
|
||||
/* Start fw file server back */
|
||||
|
||||
ret = _fw_server.start(UAVCAN_FIRMWARE_PATH, UAVCAN_ROMFS_FW_PATH);
|
||||
|
||||
if (ret < 0) {
|
||||
@@ -227,7 +96,6 @@ UavcanServers::init()
|
||||
}
|
||||
|
||||
/* Initialize storage back end for the node allocator using UAVCAN_NODE_DB_PATH directory */
|
||||
|
||||
ret = _storage_backend.init(UAVCAN_NODE_DB_PATH);
|
||||
|
||||
if (ret < 0) {
|
||||
@@ -236,7 +104,6 @@ UavcanServers::init()
|
||||
}
|
||||
|
||||
/* Initialize trace in the UAVCAN_NODE_DB_PATH directory */
|
||||
|
||||
ret = _tracer.init(UAVCAN_LOG_FILE);
|
||||
|
||||
if (ret < 0) {
|
||||
@@ -256,14 +123,6 @@ UavcanServers::init()
|
||||
return ret;
|
||||
}
|
||||
|
||||
/* Start node info retriever to fetch node info from new nodes */
|
||||
ret = _node_info_retriever.start();
|
||||
|
||||
if (ret < 0) {
|
||||
PX4_ERR("NodeInfoRetriever init: %d", ret);
|
||||
return ret;
|
||||
}
|
||||
|
||||
/* Start the fw version checker */
|
||||
ret = _fw_upgrade_trigger.start(_node_info_retriever);
|
||||
|
||||
@@ -272,689 +131,17 @@ UavcanServers::init()
|
||||
return ret;
|
||||
}
|
||||
|
||||
/* Start the Node */
|
||||
return 0;
|
||||
}
|
||||
|
||||
pthread_addr_t
|
||||
UavcanServers::run(pthread_addr_t)
|
||||
{
|
||||
prctl(PR_SET_NAME, "uavcan_fw_srv", 0);
|
||||
|
||||
Lock lock(_subnode_mutex);
|
||||
|
||||
/*
|
||||
Check for firmware in the root directory, move it to appropriate location on
|
||||
the SD card, as defined by the APDesc.
|
||||
*/
|
||||
migrateFWFromRoot(UAVCAN_FIRMWARE_PATH, UAVCAN_SD_ROOT_PATH);
|
||||
|
||||
/* the subscribe call needs to happen in the same thread,
|
||||
* so not in the constructor */
|
||||
uORB::Subscription armed_sub{ORB_ID(actuator_armed)};
|
||||
uORB::Subscription vcmd_sub{ORB_ID(vehicle_command)};
|
||||
uORB::Subscription param_request_sub{ORB_ID(uavcan_parameter_request)};
|
||||
|
||||
/* Set up shared service clients */
|
||||
_param_getset_client.setCallback(GetSetCallback(this, &UavcanServers::cb_getset));
|
||||
_param_opcode_client.setCallback(ExecuteOpcodeCallback(this, &UavcanServers::cb_opcode));
|
||||
_param_restartnode_client.setCallback(RestartNodeCallback(this, &UavcanServers::cb_restart));
|
||||
_enumeration_client.setCallback(EnumerationBeginCallback(this, &UavcanServers::cb_enumeration_begin));
|
||||
_enumeration_indication_sub.start(EnumerationIndicationCallback(this, &UavcanServers::cb_enumeration_indication));
|
||||
_enumeration_getset_client.setCallback(GetSetCallback(this, &UavcanServers::cb_enumeration_getset));
|
||||
_enumeration_save_client.setCallback(ExecuteOpcodeCallback(this, &UavcanServers::cb_enumeration_save));
|
||||
|
||||
_count_in_progress = _param_in_progress = _param_list_in_progress = _cmd_in_progress = _param_list_all_nodes = false;
|
||||
memset(_param_counts, 0, sizeof(_param_counts));
|
||||
|
||||
_esc_enumeration_active = false;
|
||||
memset(_esc_enumeration_ids, 0, sizeof(_esc_enumeration_ids));
|
||||
_esc_enumeration_index = 0;
|
||||
|
||||
while (!_subnode_thread_should_exit.load()) {
|
||||
|
||||
if (_check_fw == true) {
|
||||
_check_fw = false;
|
||||
_node_info_retriever.invalidateAll();
|
||||
}
|
||||
|
||||
const int spin_res = _subnode.spin(uavcan::MonotonicDuration::fromMSec(10));
|
||||
|
||||
if (spin_res < 0) {
|
||||
PX4_ERR("node spin error %i", spin_res);
|
||||
}
|
||||
|
||||
// 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{};
|
||||
param_request_sub.copy(&request);
|
||||
|
||||
if (_param_counts[request.node_id]) {
|
||||
/*
|
||||
* 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) {
|
||||
uavcan::protocol::param::GetSet::Request req;
|
||||
|
||||
if (request.param_index >= 0) {
|
||||
req.index = request.param_index;
|
||||
|
||||
} else {
|
||||
req.name = (char *)request.param_id;
|
||||
}
|
||||
|
||||
int call_res = _param_getset_client.call(request.node_id, req);
|
||||
|
||||
if (call_res < 0) {
|
||||
PX4_ERR("couldn't send GetSet: %d", call_res);
|
||||
|
||||
} else {
|
||||
_param_in_progress = true;
|
||||
_param_index = request.param_index;
|
||||
}
|
||||
|
||||
} else if (request.message_type == uavcan_parameter_request_s::MESSAGE_TYPE_PARAM_SET) {
|
||||
uavcan::protocol::param::GetSet::Request req;
|
||||
|
||||
if (request.param_index >= 0) {
|
||||
req.index = request.param_index;
|
||||
|
||||
} else {
|
||||
req.name = (char *)request.param_id;
|
||||
}
|
||||
|
||||
if (request.param_type == uavcan_parameter_request_s::PARAM_TYPE_REAL32) {
|
||||
req.value.to<uavcan::protocol::param::Value::Tag::real_value>() = request.real_value;
|
||||
|
||||
} else if (request.param_type == uavcan_parameter_request_s::PARAM_TYPE_UINT8) {
|
||||
req.value.to<uavcan::protocol::param::Value::Tag::boolean_value>() = request.int_value;
|
||||
|
||||
} else {
|
||||
req.value.to<uavcan::protocol::param::Value::Tag::integer_value>() = request.int_value;
|
||||
}
|
||||
|
||||
// Set the dirty bit for this node
|
||||
set_node_params_dirty(request.node_id);
|
||||
|
||||
int call_res = _param_getset_client.call(request.node_id, req);
|
||||
|
||||
if (call_res < 0) {
|
||||
PX4_ERR("couldn't send GetSet: %d", call_res);
|
||||
|
||||
} else {
|
||||
_param_in_progress = true;
|
||||
_param_index = request.param_index;
|
||||
}
|
||||
|
||||
} else if (request.message_type == uavcan_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;
|
||||
_param_list_node_id = request.node_id;
|
||||
_param_list_all_nodes = false;
|
||||
|
||||
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) {
|
||||
/*
|
||||
* This triggers the _param_list_in_progress case below,
|
||||
* but additionally iterates over all active nodes.
|
||||
*/
|
||||
_param_index = 0;
|
||||
_param_list_in_progress = true;
|
||||
_param_list_node_id = get_next_active_node_id(0);
|
||||
_param_list_all_nodes = true;
|
||||
|
||||
PX4_DEBUG("starting global param list with node %hhu", _param_list_node_id);
|
||||
|
||||
if (_param_counts[_param_list_node_id] == 0) {
|
||||
param_count(_param_list_node_id);
|
||||
}
|
||||
}
|
||||
|
||||
} else {
|
||||
/*
|
||||
* Need to know how many parameters this node has before we can
|
||||
* continue; count them now and then process the request.
|
||||
*/
|
||||
param_count(request.node_id);
|
||||
}
|
||||
}
|
||||
|
||||
// Handle parameter listing index/node ID advancement
|
||||
if (_param_list_in_progress && !_param_in_progress && !_count_in_progress) {
|
||||
if (_param_index >= _param_counts[_param_list_node_id]) {
|
||||
PX4_DEBUG("completed param list for node %hhu", _param_list_node_id);
|
||||
// Reached the end of the current node's parameter set.
|
||||
_param_list_in_progress = false;
|
||||
|
||||
if (_param_list_all_nodes) {
|
||||
// We're listing all parameters for all nodes -- get the next node ID
|
||||
uint8_t next_id = get_next_active_node_id(_param_list_node_id);
|
||||
|
||||
if (next_id < 128) {
|
||||
_param_list_node_id = next_id;
|
||||
|
||||
/*
|
||||
* If there is a next node ID, check if that node's parameters
|
||||
* have been counted before. If not, do it now.
|
||||
*/
|
||||
if (_param_counts[_param_list_node_id] == 0) {
|
||||
param_count(_param_list_node_id);
|
||||
}
|
||||
|
||||
// Keep on listing.
|
||||
_param_index = 0;
|
||||
_param_list_in_progress = true;
|
||||
PX4_DEBUG("started param list for node %hhu", _param_list_node_id);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Check if we're still listing, and need to get the next parameter
|
||||
if (_param_list_in_progress && !_param_in_progress && !_count_in_progress) {
|
||||
// Ready to request the next value -- _param_index is incremented
|
||||
// after each successful fetch by cb_getset
|
||||
uavcan::protocol::param::GetSet::Request req;
|
||||
req.index = _param_index;
|
||||
|
||||
int call_res = _param_getset_client.call(_param_list_node_id, req);
|
||||
|
||||
if (call_res < 0) {
|
||||
_param_list_in_progress = false;
|
||||
PX4_ERR("couldn't send param list GetSet: %d", call_res);
|
||||
|
||||
} else {
|
||||
_param_in_progress = true;
|
||||
}
|
||||
}
|
||||
|
||||
// Check for ESC enumeration commands
|
||||
if (vcmd_sub.updated() && !_cmd_in_progress) {
|
||||
bool acknowledge = false;
|
||||
vehicle_command_s cmd{};
|
||||
vcmd_sub.copy(&cmd);
|
||||
|
||||
uint8_t cmd_ack_result = vehicle_command_ack_s::VEHICLE_RESULT_ACCEPTED;
|
||||
|
||||
if (cmd.command == vehicle_command_s::VEHICLE_CMD_PREFLIGHT_UAVCAN) {
|
||||
acknowledge = true;
|
||||
int command_id = static_cast<int>(cmd.param1 + 0.5f);
|
||||
int node_id = static_cast<int>(cmd.param2 + 0.5f);
|
||||
int call_res;
|
||||
|
||||
PX4_DEBUG("received UAVCAN command ID %d, node ID %d", command_id, node_id);
|
||||
|
||||
switch (command_id) {
|
||||
case 0:
|
||||
case 1: {
|
||||
_esc_enumeration_active = command_id;
|
||||
_esc_enumeration_index = 0;
|
||||
_esc_count = 0;
|
||||
uavcan::protocol::enumeration::Begin::Request req;
|
||||
// TODO: Incorrect implementation; the parameter name field should be left empty.
|
||||
// Leaving it as-is to avoid breaking compatibility with non-compliant nodes.
|
||||
req.parameter_name = "esc_index";
|
||||
req.timeout_sec = _esc_enumeration_active ? 65535 : 0;
|
||||
call_res = _enumeration_client.call(get_next_active_node_id(0), req);
|
||||
|
||||
if (call_res < 0) {
|
||||
PX4_ERR("UAVCAN ESC enumeration: couldn't send initial Begin request: %d", call_res);
|
||||
beep(BeepFrequencyError);
|
||||
cmd_ack_result = vehicle_command_ack_s::VEHICLE_RESULT_FAILED;
|
||||
|
||||
} else {
|
||||
beep(BeepFrequencyGenericIndication);
|
||||
}
|
||||
|
||||
break;
|
||||
}
|
||||
|
||||
default: {
|
||||
PX4_ERR("unknown command ID %d", command_id);
|
||||
cmd_ack_result = vehicle_command_ack_s::VEHICLE_RESULT_UNSUPPORTED;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
} else if (cmd.command == vehicle_command_s::VEHICLE_CMD_PREFLIGHT_STORAGE) {
|
||||
acknowledge = true;
|
||||
int command_id = static_cast<int>(cmd.param1 + 0.5f);
|
||||
|
||||
PX4_DEBUG("received storage command ID %d", command_id);
|
||||
|
||||
switch (command_id) {
|
||||
case 1: {
|
||||
// Param save request
|
||||
int node_id;
|
||||
node_id = get_next_dirty_node_id(1);
|
||||
|
||||
if (node_id < 128) {
|
||||
_param_save_opcode = uavcan::protocol::param::ExecuteOpcode::Request::OPCODE_SAVE;
|
||||
param_opcode(node_id);
|
||||
}
|
||||
|
||||
break;
|
||||
}
|
||||
|
||||
case 2: {
|
||||
// Command is a param erase request -- apply it to all active nodes by setting the dirty bit
|
||||
_param_save_opcode = uavcan::protocol::param::ExecuteOpcode::Request::OPCODE_ERASE;
|
||||
|
||||
for (int i = 1; i < 128; i = get_next_active_node_id(i)) {
|
||||
set_node_params_dirty(i);
|
||||
}
|
||||
|
||||
param_opcode(get_next_dirty_node_id(1));
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if (acknowledge) {
|
||||
// Acknowledge the received command
|
||||
vehicle_command_ack_s ack{};
|
||||
ack.command = cmd.command;
|
||||
ack.result = cmd_ack_result;
|
||||
ack.target_system = cmd.source_system;
|
||||
ack.target_component = cmd.source_component;
|
||||
ack.timestamp = hrt_absolute_time();
|
||||
_command_ack_pub.publish(ack);
|
||||
}
|
||||
}
|
||||
|
||||
// Shut down once armed
|
||||
// TODO (elsewhere): start up again once disarmed?
|
||||
if (armed_sub.updated()) {
|
||||
actuator_armed_s armed;
|
||||
|
||||
if (armed_sub.copy(&armed)) {
|
||||
if (armed.armed) {
|
||||
_subnode_thread_should_exit.store(true);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return (pthread_addr_t) 0;
|
||||
/* Start the Node */
|
||||
return 0;
|
||||
}
|
||||
|
||||
void
|
||||
UavcanServers::cb_getset(const uavcan::ServiceCallResult<uavcan::protocol::param::GetSet> &result)
|
||||
{
|
||||
if (_count_in_progress) {
|
||||
/*
|
||||
* Currently in parameter count mode:
|
||||
* Iterate over all parameters for the node to which the request was
|
||||
* originally sent, in order to find the maximum parameter ID. If a
|
||||
* request fails, set the node's parameter count to zero.
|
||||
*/
|
||||
uint8_t node_id = result.getCallID().server_node_id.get();
|
||||
|
||||
if (result.isSuccessful()) {
|
||||
uavcan::protocol::param::GetSet::Response resp = result.getResponse();
|
||||
|
||||
if (resp.name.size()) {
|
||||
_count_index++;
|
||||
_param_counts[node_id] = _count_index;
|
||||
|
||||
uavcan::protocol::param::GetSet::Request req;
|
||||
req.index = _count_index;
|
||||
|
||||
int call_res = _param_getset_client.call(result.getCallID().server_node_id, req);
|
||||
|
||||
if (call_res < 0) {
|
||||
_count_in_progress = false;
|
||||
_count_index = 0;
|
||||
PX4_ERR("couldn't send GetSet during param count: %d", call_res);
|
||||
beep(BeepFrequencyError);
|
||||
}
|
||||
|
||||
} else {
|
||||
_count_in_progress = false;
|
||||
_count_index = 0;
|
||||
PX4_DEBUG("completed param count for node %hhu: %hhu", node_id, _param_counts[node_id]);
|
||||
beep(BeepFrequencyGenericIndication);
|
||||
}
|
||||
|
||||
} else {
|
||||
_param_counts[node_id] = 0;
|
||||
_count_in_progress = false;
|
||||
_count_index = 0;
|
||||
PX4_ERR("GetSet error during param count");
|
||||
}
|
||||
|
||||
} else {
|
||||
/*
|
||||
* Currently in parameter get/set mode:
|
||||
* Publish a uORB uavcan_parameter_value message containing the current value
|
||||
* of the parameter.
|
||||
*/
|
||||
if (result.isSuccessful()) {
|
||||
uavcan::protocol::param::GetSet::Response param = result.getResponse();
|
||||
|
||||
struct uavcan_parameter_value_s response;
|
||||
response.node_id = result.getCallID().server_node_id.get();
|
||||
strncpy(response.param_id, param.name.c_str(), sizeof(response.param_id) - 1);
|
||||
response.param_id[16] = '\0';
|
||||
response.param_index = _param_index;
|
||||
response.param_count = _param_counts[response.node_id];
|
||||
|
||||
if (param.value.is(uavcan::protocol::param::Value::Tag::integer_value)) {
|
||||
response.param_type = uavcan_parameter_request_s::PARAM_TYPE_INT64;
|
||||
response.int_value = param.value.to<uavcan::protocol::param::Value::Tag::integer_value>();
|
||||
|
||||
} else if (param.value.is(uavcan::protocol::param::Value::Tag::real_value)) {
|
||||
response.param_type = uavcan_parameter_request_s::PARAM_TYPE_REAL32;
|
||||
response.real_value = param.value.to<uavcan::protocol::param::Value::Tag::real_value>();
|
||||
|
||||
} else if (param.value.is(uavcan::protocol::param::Value::Tag::boolean_value)) {
|
||||
response.param_type = uavcan_parameter_request_s::PARAM_TYPE_UINT8;
|
||||
response.int_value = param.value.to<uavcan::protocol::param::Value::Tag::boolean_value>();
|
||||
}
|
||||
|
||||
_param_response_pub.publish(response);
|
||||
|
||||
} else {
|
||||
PX4_ERR("GetSet error");
|
||||
}
|
||||
|
||||
_param_in_progress = false;
|
||||
_param_index++;
|
||||
}
|
||||
}
|
||||
|
||||
void
|
||||
UavcanServers::param_count(uavcan::NodeID node_id)
|
||||
{
|
||||
uavcan::protocol::param::GetSet::Request req;
|
||||
req.index = 0;
|
||||
int call_res = _param_getset_client.call(node_id, req);
|
||||
|
||||
if (call_res < 0) {
|
||||
PX4_ERR("couldn't start parameter count: %d", call_res);
|
||||
|
||||
} else {
|
||||
_count_in_progress = true;
|
||||
_count_index = 0;
|
||||
PX4_DEBUG("starting param count");
|
||||
}
|
||||
}
|
||||
|
||||
void
|
||||
UavcanServers::param_opcode(uavcan::NodeID node_id)
|
||||
{
|
||||
uavcan::protocol::param::ExecuteOpcode::Request opcode_req;
|
||||
opcode_req.opcode = _param_save_opcode;
|
||||
int call_res = _param_opcode_client.call(node_id, opcode_req);
|
||||
|
||||
if (call_res < 0) {
|
||||
PX4_ERR("couldn't send ExecuteOpcode: %d", call_res);
|
||||
|
||||
} else {
|
||||
_cmd_in_progress = true;
|
||||
PX4_INFO("sent ExecuteOpcode");
|
||||
}
|
||||
}
|
||||
|
||||
void
|
||||
UavcanServers::cb_opcode(const uavcan::ServiceCallResult<uavcan::protocol::param::ExecuteOpcode> &result)
|
||||
{
|
||||
bool success = result.isSuccessful();
|
||||
uint8_t node_id = result.getCallID().server_node_id.get();
|
||||
uavcan::protocol::param::ExecuteOpcode::Response resp = result.getResponse();
|
||||
success &= resp.ok;
|
||||
_cmd_in_progress = false;
|
||||
|
||||
if (!result.isSuccessful()) {
|
||||
PX4_ERR("save request for node %hhu timed out.", node_id);
|
||||
|
||||
} else if (!result.getResponse().ok) {
|
||||
PX4_ERR("save request for node %hhu rejected.", node_id);
|
||||
|
||||
} else {
|
||||
PX4_INFO("save request for node %hhu completed OK, restarting.", node_id);
|
||||
|
||||
uavcan::protocol::RestartNode::Request restart_req;
|
||||
restart_req.magic_number = restart_req.MAGIC_NUMBER;
|
||||
int call_res = _param_restartnode_client.call(node_id, restart_req);
|
||||
|
||||
if (call_res < 0) {
|
||||
PX4_ERR("couldn't send RestartNode: %d", call_res);
|
||||
|
||||
} else {
|
||||
PX4_ERR("sent RestartNode");
|
||||
_cmd_in_progress = true;
|
||||
}
|
||||
}
|
||||
|
||||
if (!_cmd_in_progress) {
|
||||
/*
|
||||
* Something went wrong, so cb_restart is never going to be called as a result of this request.
|
||||
* To ensure we try to execute the opcode on all nodes that permit it, get the next dirty node
|
||||
* ID and keep processing here. The dirty bit on the current node is still set, so the
|
||||
* save/erase attempt will occur when the next save/erase command is received over MAVLink.
|
||||
*/
|
||||
node_id = get_next_dirty_node_id(node_id);
|
||||
|
||||
if (node_id < 128) {
|
||||
param_opcode(node_id);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void
|
||||
UavcanServers::cb_restart(const uavcan::ServiceCallResult<uavcan::protocol::RestartNode> &result)
|
||||
{
|
||||
bool success = result.isSuccessful();
|
||||
uint8_t node_id = result.getCallID().server_node_id.get();
|
||||
uavcan::protocol::RestartNode::Response resp = result.getResponse();
|
||||
success &= resp.ok;
|
||||
_cmd_in_progress = false;
|
||||
|
||||
if (success) {
|
||||
PX4_DEBUG("restart request for node %hhu completed OK.", node_id);
|
||||
|
||||
// Clear the dirty flag
|
||||
clear_node_params_dirty(node_id);
|
||||
|
||||
} else {
|
||||
PX4_ERR("restart request for node %hhu failed.", node_id);
|
||||
}
|
||||
|
||||
// Get the next dirty node ID and send the same command to it
|
||||
node_id = get_next_dirty_node_id(node_id);
|
||||
|
||||
if (node_id < 128) {
|
||||
param_opcode(node_id);
|
||||
}
|
||||
}
|
||||
|
||||
uint8_t
|
||||
UavcanServers::get_next_active_node_id(uint8_t base)
|
||||
{
|
||||
base++;
|
||||
|
||||
for (; base < 128 && (!_node_info_retriever.isNodeKnown(base) ||
|
||||
_subnode.getNodeID().get() == base); base++);
|
||||
|
||||
return base;
|
||||
}
|
||||
|
||||
uint8_t
|
||||
UavcanServers::get_next_dirty_node_id(uint8_t base)
|
||||
{
|
||||
base++;
|
||||
|
||||
for (; base < 128 && !are_node_params_dirty(base); base++);
|
||||
|
||||
return base;
|
||||
}
|
||||
|
||||
void
|
||||
UavcanServers::beep(float frequency)
|
||||
{
|
||||
uavcan::equipment::indication::BeepCommand cmd;
|
||||
cmd.frequency = frequency;
|
||||
cmd.duration = 0.1F; // We don't want to incapacitate ESC for longer time that this
|
||||
(void)_beep_pub.broadcast(cmd);
|
||||
}
|
||||
|
||||
void
|
||||
UavcanServers::cb_enumeration_begin(const uavcan::ServiceCallResult<uavcan::protocol::enumeration::Begin> &result)
|
||||
{
|
||||
uint8_t next_id = get_next_active_node_id(result.getCallID().server_node_id.get());
|
||||
|
||||
if (!result.isSuccessful()) {
|
||||
PX4_ERR("UAVCAN ESC enumeration: begin request for node %hhu timed out.", result.getCallID().server_node_id.get());
|
||||
|
||||
} else if (result.getResponse().error) {
|
||||
PX4_ERR("UAVCAN ESC enumeration: begin request for node %hhu rejected: %hhu", result.getCallID().server_node_id.get(),
|
||||
result.getResponse().error);
|
||||
|
||||
} else {
|
||||
_esc_count++;
|
||||
PX4_INFO("UAVCAN ESC enumeration: begin request for node %hhu completed OK.", result.getCallID().server_node_id.get());
|
||||
}
|
||||
|
||||
if (next_id < 128) {
|
||||
// Still other active nodes to send the request to
|
||||
uavcan::protocol::enumeration::Begin::Request req;
|
||||
// TODO: Incorrect implementation; the parameter name field should be left empty.
|
||||
// Leaving it as-is to avoid breaking compatibility with non-compliant nodes.
|
||||
req.parameter_name = "esc_index";
|
||||
req.timeout_sec = _esc_enumeration_active ? 65535 : 0;
|
||||
|
||||
int call_res = _enumeration_client.call(next_id, req);
|
||||
|
||||
if (call_res < 0) {
|
||||
PX4_ERR("UAVCAN ESC enumeration: couldn't send Begin request: %d", call_res);
|
||||
|
||||
} else {
|
||||
PX4_INFO("UAVCAN ESC enumeration: sent Begin request");
|
||||
}
|
||||
|
||||
} else {
|
||||
PX4_INFO("UAVCAN ESC enumeration: begun enumeration on all nodes.");
|
||||
}
|
||||
}
|
||||
|
||||
void
|
||||
UavcanServers::cb_enumeration_indication(const uavcan::ReceivedDataStructure<uavcan::protocol::enumeration::Indication>
|
||||
&msg)
|
||||
{
|
||||
// Called whenever an ESC thinks it has received user input.
|
||||
PX4_INFO("UAVCAN ESC enumeration: got indication");
|
||||
|
||||
if (!_esc_enumeration_active) {
|
||||
// Ignore any messages received when we're not expecting them
|
||||
return;
|
||||
}
|
||||
|
||||
// First, check if we've already seen an indication from this ESC. If so, just ignore this indication.
|
||||
int i = 0;
|
||||
|
||||
for (; i < _esc_enumeration_index; i++) {
|
||||
if (_esc_enumeration_ids[i] == msg.getSrcNodeID().get()) {
|
||||
PX4_INFO("UAVCAN ESC enumeration: already enumerated ESC ID %hhu as index %d, ignored", _esc_enumeration_ids[i], i);
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
uavcan::protocol::param::GetSet::Request req;
|
||||
req.name = msg.parameter_name; // 'esc_index' or something alike, the name is not standardized
|
||||
req.value.to<uavcan::protocol::param::Value::Tag::integer_value>() = i;
|
||||
|
||||
int call_res = _enumeration_getset_client.call(msg.getSrcNodeID(), req);
|
||||
|
||||
if (call_res < 0) {
|
||||
PX4_ERR("UAVCAN ESC enumeration: couldn't send GetSet: %d", call_res);
|
||||
|
||||
} else {
|
||||
PX4_INFO("UAVCAN ESC enumeration: sent GetSet to node %hhu (index %d)", _esc_enumeration_ids[i], i);
|
||||
}
|
||||
}
|
||||
|
||||
void
|
||||
UavcanServers::cb_enumeration_getset(const uavcan::ServiceCallResult<uavcan::protocol::param::GetSet> &result)
|
||||
{
|
||||
if (!result.isSuccessful()) {
|
||||
PX4_ERR("UAVCAN ESC enumeration: save request for node %hhu timed out.", result.getCallID().server_node_id.get());
|
||||
|
||||
} else {
|
||||
PX4_INFO("UAVCAN ESC enumeration: save request for node %hhu completed OK.", result.getCallID().server_node_id.get());
|
||||
|
||||
uavcan::protocol::param::GetSet::Response resp = result.getResponse();
|
||||
uint8_t esc_index = (uint8_t)resp.value.to<uavcan::protocol::param::Value::Tag::integer_value>();
|
||||
esc_index = math::min((uint8_t)(uavcan::equipment::esc::RawCommand::FieldTypes::cmd::MaxSize - 1), esc_index);
|
||||
_esc_enumeration_index = math::max(_esc_enumeration_index, (uint8_t)(esc_index + 1));
|
||||
|
||||
_esc_enumeration_ids[esc_index] = result.getCallID().server_node_id.get();
|
||||
|
||||
uavcan::protocol::param::ExecuteOpcode::Request opcode_req;
|
||||
opcode_req.opcode = opcode_req.OPCODE_SAVE;
|
||||
int call_res = _enumeration_save_client.call(result.getCallID().server_node_id, opcode_req);
|
||||
|
||||
if (call_res < 0) {
|
||||
PX4_ERR("UAVCAN ESC enumeration: couldn't send ExecuteOpcode: %d", call_res);
|
||||
|
||||
} else {
|
||||
PX4_INFO("UAVCAN ESC enumeration: sent ExecuteOpcode to node %hhu (index %hhu)", _esc_enumeration_ids[esc_index],
|
||||
esc_index);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void
|
||||
UavcanServers::cb_enumeration_save(const uavcan::ServiceCallResult<uavcan::protocol::param::ExecuteOpcode> &result)
|
||||
{
|
||||
const bool this_is_the_last_one =
|
||||
(_esc_enumeration_index >= uavcan::equipment::esc::RawCommand::FieldTypes::cmd::MaxSize - 1) ||
|
||||
(_esc_enumeration_index >= _esc_count);
|
||||
|
||||
if (!result.isSuccessful()) {
|
||||
PX4_ERR("UAVCAN ESC enumeration: save request for node %hhu timed out.", result.getCallID().server_node_id.get());
|
||||
beep(BeepFrequencyError);
|
||||
|
||||
} else if (!result.getResponse().ok) {
|
||||
PX4_ERR("UAVCAN ESC enumeration: save request for node %hhu rejected", result.getCallID().server_node_id.get());
|
||||
beep(BeepFrequencyError);
|
||||
|
||||
} else {
|
||||
PX4_INFO("UAVCAN ESC enumeration: save request for node %hhu completed OK.", result.getCallID().server_node_id.get());
|
||||
beep(this_is_the_last_one ? BeepFrequencySuccess : BeepFrequencyGenericIndication);
|
||||
}
|
||||
|
||||
PX4_INFO("UAVCAN ESC enumeration: completed %hhu of %hhu", _esc_enumeration_index, _esc_count);
|
||||
|
||||
if (this_is_the_last_one) {
|
||||
_esc_enumeration_active = false;
|
||||
|
||||
// Tell all ESCs to stop enumerating
|
||||
uavcan::protocol::enumeration::Begin::Request req;
|
||||
|
||||
// TODO: Incorrect implementation; the parameter name field should be left empty.
|
||||
// Leaving it as-is to avoid breaking compatibility with non-compliant nodes.
|
||||
req.parameter_name = "esc_index";
|
||||
req.timeout_sec = 0;
|
||||
int call_res = _enumeration_client.call(get_next_active_node_id(0), req);
|
||||
|
||||
if (call_res < 0) {
|
||||
PX4_ERR("UAVCAN ESC enumeration: couldn't send Begin request to stop enumeration: %d", call_res);
|
||||
|
||||
} else {
|
||||
PX4_INFO("UAVCAN ESC enumeration: sent Begin request to stop enumeration");
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
void
|
||||
UavcanServers::migrateFWFromRoot(const char *sd_path, const char *sd_root_path)
|
||||
void UavcanServers::migrateFWFromRoot(const char *sd_path, const char *sd_root_path)
|
||||
{
|
||||
/*
|
||||
Copy Any bin files with APDes into appropriate location on SD card
|
||||
@@ -1032,8 +219,7 @@ UavcanServers::migrateFWFromRoot(const char *sd_path, const char *sd_root_path)
|
||||
}
|
||||
}
|
||||
|
||||
int
|
||||
UavcanServers::copyFw(const char *dst, const char *src)
|
||||
int UavcanServers::copyFw(const char *dst, const char *src)
|
||||
{
|
||||
int rv = 0;
|
||||
uint8_t buffer[512] {};
|
||||
|
||||
Reference in New Issue
Block a user