fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity
PX4-Autopilot/src/modules/uavcan/uavcan_servers.cpp
José Roberto de Souza 925efe990d Initialize all outgoing vehicle_command_ack_s and vehicle_command_s 
This will initialize those structs with zero in all fields not set
and all fields set will only be change once to the final value not
wasting CPU time zeroing it.

This will guarantee that no non-unitialized structs will have
a trash value on from_external causing it to be sent to the
MAVLink channel without need it.
2017-08-08 21:46:30 +02:00

1164 lines
38 KiB
C++
Raw Blame History

/****************************************************************************
*
* Copyright (c) 2014-2017 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.
*
****************************************************************************/
#include <px4_tasks.h>
#include <nuttx/config.h>
#include <cstdlib>
#include <cstring>
#include <fcntl.h>
#include <dirent.h>
#include <pthread.h>
#include <mathlib/mathlib.h>
#include <systemlib/err.h>
#include <systemlib/systemlib.h>
#include <systemlib/param/param.h>
#include <systemlib/mixer/mixer.h>
#include <systemlib/board_serial.h>
#include <version/version.h>
#include <arch/board/board.h>
#include <arch/chip/chip.h>
#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>
#include <uORB/topics/uavcan_parameter_value.h>
#include <v2.0/common/mavlink.h>
/**
* @file uavcan_servers.cpp
*
* Implements basic functionality of UAVCAN node.
*
* @author Pavel Kirienko <pavel.kirienko@gmail.com>
* David Sidrane <david_s5@nscdg.com>
*/
/*
* UavcanNode
*/
UavcanServers *UavcanServers::_instance;
UavcanServers::UavcanServers(uavcan::INode &main_node) :
_subnode_thread(-1),
_vdriver(NumIfaces, uavcan_stm32::SystemClock::instance(), main_node.getAllocator(), VirtualIfaceBlockAllocationQuota),
_subnode(_vdriver, uavcan_stm32::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()
{
if (_mutex_inited) {
(void)Lock::deinit(_subnode_mutex);
}
_main_node.getDispatcher().removeRxFrameListener();
}
int UavcanServers::stop()
{
UavcanServers *server = instance();
if (server == nullptr) {
warnx("Already stopped");
return -1;
}
if (server->_subnode_thread) {
warnx("stopping fw srv thread...");
server->_subnode_thread_should_exit = 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) {
warnx("Already started");
return -1;
}
/*
* Node init
*/
_instance = new UavcanServers(main_node);
if (_instance == nullptr) {
warnx("Out of memory");
return -2;
}
int rv = _instance->init();
if (rv < 0) {
warnx("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, &param);
tattr.stacksize = PX4_STACK_ADJUSTED(StackSize);
param.sched_priority = Priority;
if (pthread_attr_setschedparam(&tattr, &param)) {
warnx("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;
warnx("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) {
warnx("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);
if (ret < 0) {
warnx("FirmwareVersionChecker init: %d, errno: %d", ret, errno);
return ret;
}
/* Start fw file server back */
ret = _fw_server.start();
if (ret < 0) {
warnx("BasicFileServer init: %d, errno: %d", ret, errno);
return ret;
}
/* 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) {
warnx("FileStorageBackend init: %d, errno: %d", ret, errno);
return ret;
}
/* Initialize trace in the UAVCAN_NODE_DB_PATH directory */
ret = _tracer.init(UAVCAN_LOG_FILE);
if (ret < 0) {
warnx("FileEventTracer init: %d, errno: %d", ret, errno);
return ret;
}
/* hardware version */
uavcan::protocol::HardwareVersion hwver;
UavcanNode::getHardwareVersion(hwver);
/* Initialize the dynamic node id server */
ret = _server_instance.init(hwver.unique_id);
if (ret < 0) {
warnx("CentralizedServer init: %d", ret);
return ret;
}
/* Start node info retriever to fetch node info from new nodes */
ret = _node_info_retriever.start();
if (ret < 0) {
warnx("NodeInfoRetriever init: %d", ret);
return ret;
}
/* Start the fw version checker */
ret = _fw_upgrade_trigger.start(_node_info_retriever, _fw_version_checker.getFirmwarePath());
if (ret < 0) {
warnx("FirmwareUpdateTrigger init: %d", ret);
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);
/*
Copy any firmware bundled in the ROMFS to the appropriate location on the
SD card, unless the user has copied other firmware for that device.
*/
unpackFwFromROMFS(UAVCAN_FIRMWARE_PATH, UAVCAN_ROMFS_FW_PATH);
/* the subscribe call needs to happen in the same thread,
* so not in the constructor */
int cmd_sub = orb_subscribe(ORB_ID(vehicle_command));
int param_request_sub = orb_subscribe(ORB_ID(uavcan_parameter_request));
int armed_sub = orb_subscribe(ORB_ID(actuator_armed));
/* 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) {
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) {
warnx("node spin error %i", spin_res);
}
// Check for parameter requests (get/set/list)
bool param_request_ready;
orb_check(param_request_sub, &param_request_ready);
if (param_request_ready && !_param_list_in_progress && !_param_in_progress && !_count_in_progress) {
struct uavcan_parameter_request_s request;
orb_copy(ORB_ID(uavcan_parameter_request), param_request_sub, &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 == MAVLINK_MSG_ID_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) {
warnx("UAVCAN command bridge: couldn't send GetSet: %d", call_res);
} else {
_param_in_progress = true;
_param_index = request.param_index;
}
} else if (request.message_type == MAVLINK_MSG_ID_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 == MAV_PARAM_TYPE_REAL32) {
req.value.to<uavcan::protocol::param::Value::Tag::real_value>() = request.real_value;
} else if (request.param_type == MAV_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) {
warnx("UAVCAN command bridge: couldn't send GetSet: %d", call_res);
} else {
_param_in_progress = true;
_param_index = request.param_index;
}
} else if (request.message_type == MAVLINK_MSG_ID_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;
warnx("UAVCAN command bridge: starting component-specific param list");
}
} else if (request.node_id == MAV_COMP_ID_ALL) {
if (request.message_type == MAVLINK_MSG_ID_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;
warnx("UAVCAN command bridge: 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]) {
warnx("UAVCAN command bridge: 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;
warnx("UAVCAN command bridge: 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;
warnx("UAVCAN command bridge: couldn't send param list GetSet: %d", call_res);
} else {
_param_in_progress = true;
}
}
// Check for ESC enumeration commands
bool cmd_ready;
orb_check(cmd_sub, &cmd_ready);
if (cmd_ready && !_cmd_in_progress) {
struct vehicle_command_s cmd;
orb_copy(ORB_ID(vehicle_command), cmd_sub, &cmd);
uint8_t cmd_ack_result = vehicle_command_ack_s::VEHICLE_RESULT_ACCEPTED;
if (cmd.command == vehicle_command_s::VEHICLE_CMD_PREFLIGHT_UAVCAN) {
int command_id = static_cast<int>(cmd.param1 + 0.5f);
int node_id = static_cast<int>(cmd.param2 + 0.5f);
int call_res;
warnx("UAVCAN command bridge: 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) {
warnx("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: {
warnx("UAVCAN command bridge: 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) {
int command_id = static_cast<int>(cmd.param1 + 0.5f);
warnx("UAVCAN command bridge: 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;
}
}
}
// Acknowledge the received command
struct vehicle_command_ack_s ack = {
.timestamp = 0,
.command = cmd.command,
.result = cmd_ack_result
};
if (_command_ack_pub == nullptr) {
_command_ack_pub = orb_advertise_queue(ORB_ID(vehicle_command_ack), &ack, vehicle_command_ack_s::ORB_QUEUE_LENGTH);
} else {
orb_publish(ORB_ID(vehicle_command_ack), _command_ack_pub, &ack);
}
}
// Shut down once armed
// TODO (elsewhere): start up again once disarmed?
bool updated;
orb_check(armed_sub, &updated);
if (updated) {
struct actuator_armed_s armed;
orb_copy(ORB_ID(actuator_armed), armed_sub, &armed);
if (armed.armed && !(armed.lockdown || armed.manual_lockdown)) {
warnx("UAVCAN command bridge: system armed, exiting now.");
break;
}
}
}
_subnode_thread_should_exit = false;
warnx("exiting");
return (pthread_addr_t) 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;
warnx("UAVCAN command bridge: couldn't send GetSet during param count: %d", call_res);
beep(BeepFrequencyError);
}
} else {
_count_in_progress = false;
_count_index = 0;
warnx("UAVCAN command bridge: 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;
warnx("UAVCAN command bridge: 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 = MAV_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 = MAV_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 = MAV_PARAM_TYPE_UINT8;
response.int_value = param.value.to<uavcan::protocol::param::Value::Tag::boolean_value>();
}
if (_param_response_pub == nullptr) {
_param_response_pub = orb_advertise(ORB_ID(uavcan_parameter_value), &response);
} else {
orb_publish(ORB_ID(uavcan_parameter_value), _param_response_pub, &response);
}
} else {
warnx("UAVCAN command bridge: 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) {
warnx("UAVCAN command bridge: couldn't start parameter count: %d", call_res);
} else {
_count_in_progress = true;
_count_index = 0;
warnx("UAVCAN command bridge: 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) {
warnx("UAVCAN command bridge: couldn't send ExecuteOpcode: %d", call_res);
} else {
_cmd_in_progress = true;
warnx("UAVCAN command bridge: 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()) {
warnx("UAVCAN command bridge: save request for node %hhu timed out.", node_id);
} else if (!result.getResponse().ok) {
warnx("UAVCAN command bridge: save request for node %hhu rejected.", node_id);
} else {
warnx("UAVCAN command bridge: 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) {
warnx("UAVCAN command bridge: couldn't send RestartNode: %d", call_res);
} else {
warnx("UAVCAN command bridge: 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) {
warnx("UAVCAN command bridge: restart request for node %hhu completed OK.", node_id);
// Clear the dirty flag
clear_node_params_dirty(node_id);
} else {
warnx("UAVCAN command bridge: 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()) {
warnx("UAVCAN ESC enumeration: begin request for node %hhu timed out.", result.getCallID().server_node_id.get());
} else if (result.getResponse().error) {
warnx("UAVCAN ESC enumeration: begin request for node %hhu rejected: %hhu", result.getCallID().server_node_id.get(), result.getResponse().error);
} else {
_esc_count++;
warnx("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) {
warnx("UAVCAN ESC enumeration: couldn't send Begin request: %d", call_res);
} else {
warnx("UAVCAN ESC enumeration: sent Begin request");
}
} else {
warnx("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.
warnx("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()) {
warnx("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) {
warnx("UAVCAN ESC enumeration: couldn't send GetSet: %d", call_res);
} else {
warnx("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()) {
warnx("UAVCAN ESC enumeration: save request for node %hhu timed out.", result.getCallID().server_node_id.get());
} else {
warnx("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) {
warnx("UAVCAN ESC enumeration: couldn't send ExecuteOpcode: %d", call_res);
} else {
warnx("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()) {
warnx("UAVCAN ESC enumeration: save request for node %hhu timed out.", result.getCallID().server_node_id.get());
beep(BeepFrequencyError);
} else if (!result.getResponse().ok) {
warnx("UAVCAN ESC enumeration: save request for node %hhu rejected", result.getCallID().server_node_id.get());
beep(BeepFrequencyError);
} else {
warnx("UAVCAN ESC enumeration: save request for node %hhu completed OK.", result.getCallID().server_node_id.get());
beep(this_is_the_last_one ? BeepFrequencySuccess : BeepFrequencyGenericIndication);
}
warnx("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) {
warnx("UAVCAN ESC enumeration: couldn't send Begin request to stop enumeration: %d", call_res);
} else {
warnx("UAVCAN ESC enumeration: sent Begin request to stop enumeration");
}
}
}
void UavcanServers::unpackFwFromROMFS(const char* sd_path, const char* romfs_path) {
/*
Copy the ROMFS firmware directory to the appropriate location on SD, without
overriding any firmware the user has already loaded there.
The SD firmware directory structure is along the lines of:
/fs/microsd/fw
- /c
- /nodename-v1-1.0.25d0137d.bin cache file (copy of /fs/microsd/fw/org.pixhawk.nodename-v1/1.1/nodename-v1-1.0.25d0137d.bin)
- /othernode-v3-1.6.25d0137d.bin cache file (copy of /fs/microsd/fw/com.example.othernode-v3/1.6/othernode-v3-1.6.25d0137d.bin)
- /org.pixhawk.nodename-v1 device directory for org.pixhawk.nodename-v1
- - /1.0 version directory for hardware 1.0
- - /1.1 version directory for hardware 1.1
- - - /nodename-v1-1.0.25d0137d.bin firmware file for org.pixhawk.nodename-v1 nodes, hardware version 1.1
- /com.example.othernode-v3 device directory for com.example.othernode-v3
- - /1.0 version directory for hardawre 1.0
- - - /othernode-v3-1.6.25d0137d.bin firmware file for com.example.othernode-v3, hardware version 1.6
The ROMFS directory structure is the same, but located at /etc/uavcan/fw
Files located there are prefixed with _ to identify them a comming from the rom
file system.
We iterate over all device directories in the ROMFS base directory, and create
corresponding device directories on the SD card if they don't already exist.
In each device directory, we iterate over each version directory and create a
corresponding version directory on the SD card if it doesn't already exist.
In each version directory, we remove any files with a name starting with "_"
in the corresponding directory on the SD card that don't match the bundled firmware
filename; if the directory is empty after that process, we copy the bundled firmware.
todo:This code would benefit from the use of strcat.
*/
const size_t maxlen = UAVCAN_MAX_PATH_LENGTH;
const size_t sd_path_len = strlen(sd_path);
const size_t romfs_path_len = strlen(romfs_path);
struct stat sb;
int rv;
char dstpath[maxlen + 1];
char srcpath[maxlen + 1];
DIR* const romfs_dir = opendir(romfs_path);
if (!romfs_dir) {
return;
}
memcpy(dstpath, sd_path, sd_path_len + 1);
memcpy(srcpath, romfs_path, romfs_path_len + 1);
// Iterate over all device directories in ROMFS
struct dirent* dev_dirent = NULL;
while ((dev_dirent = readdir(romfs_dir)) != NULL) {
// Skip if not a directory
if (!DIRENT_ISDIRECTORY(dev_dirent->d_type)) {
continue;
}
// Make sure the path fits
size_t dev_dirname_len = strlen(dev_dirent->d_name);
size_t srcpath_dev_len = romfs_path_len + 1 + dev_dirname_len;
if (srcpath_dev_len > maxlen) {
warnx("dev: srcpath '%s/%s' too long", romfs_path, dev_dirent->d_name);
continue;
}
size_t dstpath_dev_len = sd_path_len + 1 + dev_dirname_len;
if (dstpath_dev_len > maxlen) {
warnx("dev: dstpath '%s/%s' too long", sd_path, dev_dirent->d_name);
continue;
}
// Create the device name directory on the SD card if it doesn't already exist
dstpath[sd_path_len] = '/';
memcpy(&dstpath[sd_path_len + 1], dev_dirent->d_name, dev_dirname_len + 1);
if (stat(dstpath, &sb) != 0 || !S_ISDIR(sb.st_mode)) {
rv = mkdir(dstpath, S_IRWXU | S_IRWXG | S_IRWXO);
if (rv != 0) {
warnx("dev: couldn't create '%s'", dstpath);
continue;
}
}
// Set up the source path
srcpath[romfs_path_len] = '/';
memcpy(&srcpath[romfs_path_len + 1], dev_dirent->d_name, dev_dirname_len + 1);
DIR* const dev_dir = opendir(srcpath);
if (!dev_dir) {
warnx("dev: couldn't open '%s'", srcpath);
continue;
}
// Iterate over all version directories in the current ROMFS device directory
struct dirent* ver_dirent = NULL;
while ((ver_dirent = readdir(dev_dir)) != NULL) {
// Skip if not a directory
if (!DIRENT_ISDIRECTORY(ver_dirent->d_type)) {
continue;
}
// Make sure the path fits
size_t ver_dirname_len = strlen(ver_dirent->d_name);
size_t srcpath_ver_len = srcpath_dev_len + 1 + ver_dirname_len;
if (srcpath_ver_len > maxlen) {
warnx("ver: srcpath '%s/%s' too long", srcpath, ver_dirent->d_name);
continue;
}
size_t dstpath_ver_len = dstpath_dev_len + 1 + ver_dirname_len;
if (dstpath_ver_len > maxlen) {
warnx("ver: dstpath '%s/%s' too long", dstpath, ver_dirent->d_name);
continue;
}
// Create the device version directory on the SD card if it doesn't already exist
dstpath[dstpath_dev_len] = '/';
memcpy(&dstpath[dstpath_dev_len + 1], ver_dirent->d_name, ver_dirname_len + 1);
if (stat(dstpath, &sb) != 0 || !S_ISDIR(sb.st_mode)) {
rv = mkdir(dstpath, S_IRWXU | S_IRWXG | S_IRWXO);
if (rv != 0) {
warnx("ver: couldn't create '%s'", dstpath);
continue;
}
}
// Set up the source path
srcpath[srcpath_dev_len] = '/';
memcpy(&srcpath[srcpath_dev_len + 1], ver_dirent->d_name, ver_dirname_len + 1);
// Find the name of the bundled firmware file, or move on to the
// next directory if there's no file here.
DIR* const src_ver_dir = opendir(srcpath);
if (!src_ver_dir) {
warnx("ver: couldn't open '%s'", srcpath);
continue;
}
struct dirent* src_fw_dirent = NULL;
while ((src_fw_dirent = readdir(src_ver_dir)) != NULL &&
!DIRENT_ISFILE(src_fw_dirent->d_type));
if (!src_fw_dirent) {
(void)closedir(src_ver_dir);
continue;
}
size_t fw_len = strlen(src_fw_dirent->d_name);
bool copy_fw = true;
// Clear out any romfs_ files in the version directory on the SD card
DIR* const dst_ver_dir = opendir(dstpath);
if (!dst_ver_dir) {
warnx("unlink: couldn't open '%s'", dstpath);
} else {
struct dirent* fw_dirent = NULL;
while ((fw_dirent = readdir(dst_ver_dir)) != NULL) {
// Skip if not a file
if (!DIRENT_ISFILE(fw_dirent->d_type)) {
continue;
}
if (!memcmp(&fw_dirent->d_name, src_fw_dirent->d_name, fw_len)) {
/*
* Exact match between SD card filename and ROMFS filename; must be the same version
* so don't bother deleting and rewriting it.
*/
copy_fw = false;
} else if (!memcmp(fw_dirent->d_name, UAVCAN_ROMFS_FW_PREFIX, sizeof(UAVCAN_ROMFS_FW_PREFIX) - 1)) {
size_t dst_fw_len = strlen(fw_dirent->d_name);
size_t dstpath_fw_len = dstpath_ver_len + dst_fw_len;
if (dstpath_fw_len > maxlen) {
// sizeof(prefix) includes trailing NUL, cancelling out the +1 for the path separator
warnx("unlink: path '%s/%s' too long", dstpath, fw_dirent->d_name);
} else {
// File name starts with "_", delete it.
dstpath[dstpath_ver_len] = '/';
memcpy(&dstpath[dstpath_ver_len + 1], fw_dirent->d_name, dst_fw_len + 1);
unlink(dstpath);
warnx("unlink: removed '%s'", dstpath);
}
} else {
// User file, don't copy firmware
copy_fw = false;
}
}
(void)closedir(dst_ver_dir);
}
// If we need to, copy the file from ROMFS to the SD card
if (copy_fw) {
size_t srcpath_fw_len = srcpath_ver_len + 1 + fw_len;
size_t dstpath_fw_len = dstpath_ver_len + fw_len;
if (srcpath_fw_len > maxlen) {
warnx("copy: srcpath '%s/%s' too long", srcpath, src_fw_dirent->d_name);
} else if (dstpath_fw_len > maxlen) {
warnx("copy: dstpath '%s/%s' too long", dstpath, src_fw_dirent->d_name);
} else {
// All OK, make the paths and copy the file
srcpath[srcpath_ver_len] = '/';
memcpy(&srcpath[srcpath_ver_len + 1], src_fw_dirent->d_name, fw_len + 1);
dstpath[dstpath_ver_len] = '/';
memcpy(&dstpath[dstpath_ver_len +1], src_fw_dirent->d_name, fw_len + 1);
rv = copyFw(dstpath, srcpath);
if (rv != 0) {
warnx("copy: '%s' -> '%s' failed: %d", srcpath, dstpath, rv);
} else {
warnx("copy: '%s' -> '%s' succeeded", srcpath, dstpath);
}
}
}
(void)closedir(src_ver_dir);
}
(void)closedir(dev_dir);
}
(void)closedir(romfs_dir);
}
int UavcanServers::copyFw(const char* dst, const char* src) {
int rv = 0;
int dfd, sfd;
uint8_t buffer[512];
dfd = open(dst, O_WRONLY | O_CREAT, 0666);
if (dfd < 0) {
warnx("copyFw: couldn't open dst");
return -errno;
}
sfd = open(src, O_RDONLY, 0);
if (sfd < 0) {
(void)close(dfd);
warnx("copyFw: couldn't open src");
return -errno;
}
ssize_t size = 0;
do {
size = read(sfd, buffer, sizeof(buffer));
if (size < 0) {
warnx("copyFw: couldn't read");
rv = -errno;
} else if (size > 0) {
rv = 0;
ssize_t remaining = size;
ssize_t total_written = 0;
ssize_t written = 0;
do {
written = write(dfd, &buffer[total_written], remaining);
if (written < 0) {
warnx("copyFw: couldn't write");
rv = -errno;
} else {
total_written += written;
remaining -= written;
}
} while (written > 0 && remaining > 0);
}
} while (rv == 0 && size != 0);
(void)close(dfd);
(void)close(sfd);
return rv;
}
Reference in New Issue View Git Blame Copy Permalink