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

drivers/uavcan: make firmware server available on both CAN1/CAN2 always (if enabled)

Browse Source 
- 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 2022-01-10 11:13:02 -05:00 committed by GitHub
parent d9e7315420
commit 2e2ac36cab
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
7 changed files with 565 additions and 1822 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
ROMFS/px4fmu_common/init.d/rcS
View File

@ -291,15 +291,9 @@ else
# Start core UAVCAN module.
if uavcan start
then
if param greater UAVCAN_ENABLE 1
if param greater UAVCAN_ENABLE 2
then
# Start UAVCAN firmware update server and dynamic node ID allocation server.
uavcan start fw
if param greater UAVCAN_ENABLE 2
then
set OUTPUT_MODE uavcan_esc
fi
set OUTPUT_MODE uavcan_esc
fi
else
tune_control play error

727
src/drivers/uavcan/uavcan_main.cpp
View File

@ -84,15 +84,16 @@ UavcanNode::UavcanNode(uavcan::ICanDriver &can_driver, uavcan::ISystemClock &sys
_esc_controller(_node),
_servo_controller(_node),
_hardpoint_controller(_node),
_beep_controller(_node),
_safety_state_controller(_node),
_rgbled_controller(_node),
_time_sync_master(_node),
_time_sync_slave(_node),
_node_status_monitor(_node),
_cycle_perf(perf_alloc(PC_ELAPSED, "uavcan: cycle time")),
_interval_perf(perf_alloc(PC_INTERVAL, "uavcan: cycle interval")),
_master_timer(_node)
_node_info_retriever(_node),
_master_timer(_node),
_param_getset_client(_node),
_param_opcode_client(_node),
_param_restartnode_client(_node)
{
int res = pthread_mutex_init(&_node_mutex, nullptr);
@ -100,23 +101,16 @@ UavcanNode::UavcanNode(uavcan::ICanDriver &can_driver, uavcan::ISystemClock &sys
std::abort();
}
res = px4_sem_init(&_server_command_sem, 0, 0);
if (res < 0) {
std::abort();
}
/* _server_command_sem use case is a signal */
px4_sem_setprotocol(&_server_command_sem, SEM_PRIO_NONE);
_mixing_interface_esc.mixingOutput().setMaxTopicUpdateRate(1000000 / UavcanEscController::MAX_RATE_HZ);
_mixing_interface_servo.mixingOutput().setMaxTopicUpdateRate(1000000 / UavcanServoController::MAX_RATE_HZ);
}
UavcanNode::~UavcanNode()
{
fw_server(Stop);
if (_servers != nullptr) {
delete _servers;
_servers = nullptr;
}
if (_instance) {
@ -141,7 +135,6 @@ UavcanNode::~UavcanNode()
_sensor_bridges.clear();
pthread_mutex_destroy(&_node_mutex);
px4_sem_destroy(&_server_command_sem);
perf_free(_cycle_perf);
perf_free(_interval_perf);
@ -192,15 +185,6 @@ UavcanNode::print_params(uavcan::protocol::param::GetSet::Response &resp)
return -1;
}
void
UavcanNode::cb_opcode(const uavcan::ServiceCallResult<uavcan::protocol::param::ExecuteOpcode> &result)
{
uavcan::protocol::param::ExecuteOpcode::Response resp;
_callback_success = result.isSuccessful();
resp = result.getResponse();
_callback_success &= resp.ok;
}
int
UavcanNode::save_params(int remote_node_id)
{
@ -225,15 +209,6 @@ UavcanNode::save_params(int remote_node_id)
return 0;
}
void
UavcanNode::cb_restart(const uavcan::ServiceCallResult<uavcan::protocol::RestartNode> &result)
{
uavcan::protocol::RestartNode::Response resp;
_callback_success = result.isSuccessful();
resp = result.getResponse();
_callback_success &= resp.ok;
}
int
UavcanNode::reset_node(int remote_node_id)
{
@ -422,99 +397,6 @@ UavcanNode::update_params()
_mixing_interface_servo.updateParams();
}
int
UavcanNode::start_fw_server()
{
int rv = -1;
_fw_server_action.store((int)Busy);
UavcanServers *_servers = UavcanServers::instance();
if (_servers == nullptr) {
rv = UavcanServers::start(_node);
if (rv >= 0) {
/*
* Set our pointer to to the injector
* This is a work around as
* main_node.getDispatcher().installRxFrameListener(driver.get());
* would require a dynamic cast and rtti is not enabled.
*/
UavcanServers::instance()->attachITxQueueInjector(&_tx_injector);
}
}
_fw_server_action.store((int)None);
px4_sem_post(&_server_command_sem);
return rv;
}
int
UavcanNode::request_fw_check()
{
int rv = -1;
_fw_server_action.store((int)Busy);
UavcanServers *_servers = UavcanServers::instance();
if (_servers != nullptr) {
_servers->requestCheckAllNodesFirmwareAndUpdate();
rv = 0;
}
_fw_server_action.store((int)None);
px4_sem_post(&_server_command_sem);
return rv;
}
int
UavcanNode::stop_fw_server()
{
int rv = -1;
_fw_server_action.store((int)Busy);
UavcanServers *_servers = UavcanServers::instance();
if (_servers != nullptr) {
/*
* Set our pointer to to the injector
* This is a work around as
* main_node.getDispatcher().remeveRxFrameListener();
* would require a dynamic cast and rtti is not enabled.
*/
_tx_injector = nullptr;
rv = _servers->stop();
}
_fw_server_action.store((int)None);
px4_sem_post(&_server_command_sem);
return rv;
}
int
UavcanNode::fw_server(eServerAction action)
{
int rv = -EAGAIN;
switch (action) {
case Start:
case Stop:
case CheckFW:
if (_fw_server_action.load() == (int)None) {
_fw_server_action.store((int)action);
px4_sem_wait(&_server_command_sem);
rv = _fw_server_status;
}
break;
default:
rv = -EINVAL;
break;
}
return rv;
}
int
UavcanNode::start(uavcan::NodeID node_id, uint32_t bitrate)
{
@ -637,12 +519,6 @@ UavcanNode::init(uavcan::NodeID node_id, UAVCAN_DRIVER::BusEvent &bus_events)
return ret;
}
ret = _beep_controller.init();
if (ret < 0) {
return ret;
}
ret = _safety_state_controller.init();
if (ret < 0) {
@ -655,6 +531,14 @@ UavcanNode::init(uavcan::NodeID node_id, UAVCAN_DRIVER::BusEvent &bus_events)
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;
}
// Sensor bridges
IUavcanSensorBridge::make_all(_node, _sensor_bridges);
@ -687,25 +571,31 @@ UavcanNode::init(uavcan::NodeID node_id, UAVCAN_DRIVER::BusEvent &bus_events)
enable_idle_throttle_when_armed(true);
}
/* Start the Node */
/* Set up shared service clients */
_param_getset_client.setCallback(GetSetCallback(this, &UavcanNode::cb_getset));
_param_opcode_client.setCallback(ExecuteOpcodeCallback(this, &UavcanNode::cb_opcode));
_param_restartnode_client.setCallback(RestartNodeCallback(this, &UavcanNode::cb_restart));
int32_t uavcan_enable = 1;
(void)param_get(param_find("UAVCAN_ENABLE"), &uavcan_enable);
if (uavcan_enable > 1) {
_servers = new UavcanServers(_node, _node_info_retriever);
if (_servers) {
int rv = _servers->init();
if (rv < 0) {
PX4_ERR("UavcanServers init: %d", ret);
}
}
}
// Start the Node
return _node.start();
}
void
UavcanNode::node_spin_once()
{
const int spin_res = _node.spinOnce();
if (spin_res < 0) {
PX4_ERR("node spin error %i", spin_res);
}
if (_tx_injector != nullptr) {
_tx_injector->injectTxFramesInto(_node);
}
}
void
UavcanNode::handle_time_sync(const uavcan::TimerEvent &)
{
@ -746,8 +636,6 @@ UavcanNode::handle_time_sync(const uavcan::TimerEvent &)
_time_sync_master.publish();
}
void
UavcanNode::Run()
{
@ -789,7 +677,12 @@ UavcanNode::Run()
br->update();
}
node_spin_once(); // expected to be non-blocking
if (_check_fw) {
_check_fw = false;
_node_info_retriever.invalidateAll();
}
_node.spinOnce(); // expected to be non-blocking
// Check arming state
const actuator_armed_s &armed = _mixing_interface_esc.mixingOutput().armed();
@ -805,22 +698,206 @@ UavcanNode::Run()
update_params();
}
switch ((eServerAction)_fw_server_action.load()) {
case Start:
_fw_server_status = start_fw_server();
break;
// 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);
case Stop:
_fw_server_status = stop_fw_server();
break;
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;
case CheckFW:
_fw_server_status = request_fw_check();
break;
if (request.param_index >= 0) {
req.index = request.param_index;
case None:
default:
break;
} 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;
}
}
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_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);
}
}
perf_end(_cycle_perf);
@ -833,7 +910,6 @@ UavcanNode::Run()
_mixing_interface_servo.mixingOutput().unregister();
_mixing_interface_servo.ScheduleClear();
ScheduleClear();
teardown();
_instance = nullptr;
}
}
@ -851,13 +927,6 @@ UavcanNode::enable_idle_throttle_when_armed(bool value)
}
}
int
UavcanNode::teardown()
{
px4_sem_post(&_server_command_sem);
return 0;
}
int
UavcanNode::ioctl(file *filp, int cmd, unsigned long arg)
{
@ -884,33 +953,6 @@ UavcanNode::ioctl(file *filp, int cmd, unsigned long arg)
}
break;
case UAVCAN_IOCS_HARDPOINT_SET: {
const auto &hp_cmd = *reinterpret_cast<uavcan::equipment::hardpoint::Command *>(arg);
_hardpoint_controller.set_command(hp_cmd.hardpoint_id, hp_cmd.command);
}
break;
case UAVCAN_IOCG_NODEID_INPROGRESS: {
UavcanServers *_servers = UavcanServers::instance();
if (_servers == nullptr) {
// status unavailable
ret = -EINVAL;
break;
} else if (_servers->guessIfAllDynamicNodesAreAllocated()) {
// node discovery complete
ret = -ETIME;
break;
} else {
// node discovery in progress
ret = OK;
break;
}
}
default:
ret = -ENOTTY;
break;
@ -925,7 +967,6 @@ UavcanNode::ioctl(file *filp, int cmd, unsigned long arg)
return ret;
}
bool UavcanMixingInterfaceESC::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs,
unsigned num_control_groups_updated)
{
@ -1054,11 +1095,212 @@ UavcanNode::shrink()
}
void
UavcanNode::hardpoint_controller_set(uint8_t hardpoint_id, uint16_t command)
UavcanNode::cb_getset(const uavcan::ServiceCallResult<uavcan::protocol::param::GetSet> &result)
{
(void)pthread_mutex_lock(&_node_mutex);
_hardpoint_controller.set_command(hardpoint_id, command);
(void)pthread_mutex_unlock(&_node_mutex);
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);
}
} else {
_count_in_progress = false;
_count_index = 0;
PX4_DEBUG("completed param count for node %hhu: %hhu", node_id, _param_counts[node_id]);
}
} 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();
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
UavcanNode::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
UavcanNode::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
UavcanNode::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
UavcanNode::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
UavcanNode::get_next_active_node_id(uint8_t base)
{
base++;
for (; base < 128 && (!_node_info_retriever.isNodeKnown(base) || _node.getNodeID().get() == base); base++);
return base;
}
uint8_t
UavcanNode::get_next_dirty_node_id(uint8_t base)
{
base++;
for (; base < 128 && !are_node_params_dirty(base); base++);
return base;
}
/*
@ -1067,35 +1309,19 @@ UavcanNode::hardpoint_controller_set(uint8_t hardpoint_id, uint16_t command)
static void print_usage()
{
PX4_INFO("usage: \n"
"\tuavcan {start [fw]|status|stop [all|fw]|shrink|arm|disarm|update fw|\n"
"\t param [set|get|list|save] <node-id> <name> <value>|reset <node-id>|\n"
"\t hardpoint set <id> <command>}");
"\tuavcan {start|status|stop|shrink|update}\n"
"\t param [set|get|list|save] <node-id> <name> <value>|reset <node-id>");
}
extern "C" __EXPORT int uavcan_main(int argc, char *argv[]);
int uavcan_main(int argc, char *argv[])
extern "C" __EXPORT int uavcan_main(int argc, char *argv[])
{
if (argc < 2) {
print_usage();
::exit(1);
}
bool fw = argc > 2 && !std::strcmp(argv[2], "fw");
if (!std::strcmp(argv[1], "start")) {
if (UavcanNode::instance()) {
if (fw && UavcanServers::instance() == nullptr) {
int rv = UavcanNode::instance()->fw_server(UavcanNode::Start);
if (rv < 0) {
PX4_ERR("Firmware Server Failed to Start %d", rv);
::exit(rv);
}
::exit(0);
}
// Already running, no error
PX4_INFO("already started");
::exit(0);
@ -1126,17 +1352,12 @@ int uavcan_main(int argc, char *argv[])
errx(1, "application not running");
}
if (fw && !std::strcmp(argv[1], "update")) {
if (UavcanServers::instance() == nullptr) {
if (!std::strcmp(argv[1], "update")) {
if (UavcanNode::instance() == nullptr) {
errx(1, "firmware server is not running");
}
int rv = UavcanNode::instance()->fw_server(UavcanNode::CheckFW);
::exit(rv);
}
if (fw && (!std::strcmp(argv[1], "status") || !std::strcmp(argv[1], "info"))) {
printf("Firmware Server is %s\n", UavcanServers::instance() ? "Running" : "Stopped");
UavcanNode::instance()->requestCheckAllNodesFirmwareAndUpdate();
::exit(0);
}
@ -1204,47 +1425,9 @@ int uavcan_main(int argc, char *argv[])
}
}
if (!std::strcmp(argv[1], "hardpoint")) {
if (argc > 4 && !std::strcmp(argv[2], "set")) {
const int hardpoint_id = atoi(argv[3]);
const int command = atoi(argv[4]);
// Sanity check - weed out negative values, check against maximums
if (hardpoint_id >= 0 && hardpoint_id < 256 &&
command >= 0 && command < 65536) {
inst->hardpoint_controller_set((uint8_t) hardpoint_id, (uint16_t) command);
} else {
errx(1, "Invalid argument");
}
} else {
errx(1, "Invalid hardpoint command");
}
::exit(0);
}
if (!std::strcmp(argv[1], "stop")) {
if (fw) {
int rv = inst->fw_server(UavcanNode::Stop);
/* Let's recover any memory we can */
inst->shrink();
if (rv < 0) {
PX4_ERR("Firmware Server Failed to Stop %d", rv);
::exit(rv);
}
::exit(0);
} else {
delete inst;
::exit(0);
}
delete inst;
::exit(0);
}
print_usage();

116
src/drivers/uavcan/uavcan_main.hpp
View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2014 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
@ -46,32 +46,37 @@
#include <px4_platform_common/atomic.h>
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
#include "beep.hpp"
#include "rgbled.hpp"
#include "safety_state.hpp"
#include "uavcan_driver.hpp"
#include "uavcan_servers.hpp"
#include "allocator.hpp"
#include "actuators/esc.hpp"
#include "actuators/hardpoint.hpp"
#include "actuators/servo.hpp"
#include "allocator.hpp"
#include "beep.hpp"
#include "rgbled.hpp"
#include "safety_state.hpp"
#include "sensors/sensor_bridge.hpp"
#include "uavcan_driver.hpp"
#include "uavcan_servers.hpp"
#include <lib/drivers/device/Device.hpp>
#include <lib/mixer_module/mixer_module.hpp>
#include <lib/perf/perf_counter.h>
#include <uavcan/helpers/heap_based_pool_allocator.hpp>
#include <uavcan/protocol/global_time_sync_master.hpp>
#include <uavcan/protocol/global_time_sync_slave.hpp>
#include <uavcan/protocol/node_info_retriever.hpp>
#include <uavcan/protocol/node_status_monitor.hpp>
#include <uavcan/protocol/param/GetSet.hpp>
#include <uavcan/protocol/param/ExecuteOpcode.hpp>
#include <uavcan/protocol/param/GetSet.hpp>
#include <uavcan/protocol/RestartNode.hpp>
#include <lib/drivers/device/device.h>
#include <lib/mixer_module/mixer_module.hpp>
#include <lib/perf/perf_counter.h>
#include <uORB/Publication.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/SubscriptionInterval.hpp>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/uavcan_parameter_request.h>
#include <uORB/topics/uavcan_parameter_value.h>
#include <uORB/topics/vehicle_command_ack.h>
using namespace time_literals;
@ -178,18 +183,15 @@ public:
uavcan::Node<> &get_node() { return _node; }
int teardown();
void print_info();
void shrink();
void hardpoint_controller_set(uint8_t hardpoint_id, uint16_t command);
static UavcanNode *instance() { return _instance; }
static int getHardwareVersion(uavcan::protocol::HardwareVersion &hwver);
int fw_server(eServerAction action);
void attachITxQueueInjector(ITxQueueInjector *injector) {_tx_injector = injector;}
void requestCheckAllNodesFirmwareAndUpdate() { _check_fw = true; }
int list_params(int remote_node_id);
int save_params(int remote_node_id);
int set_param(int remote_node_id, const char *name, char *value);
@ -198,16 +200,11 @@ public:
static void busevent_signal_trampoline();
protected:
void Run() override;
private:
void Run() override;
void fill_node_info();
int init(uavcan::NodeID node_id, UAVCAN_DRIVER::BusEvent &bus_events);
void node_spin_once();
int start_fw_server();
int stop_fw_server();
int request_fw_check();
int print_params(uavcan::protocol::param::GetSet::Response &resp);
int get_set_param(int nodeid, const char *name, uavcan::protocol::param::GetSet::Request &req);
@ -219,10 +216,6 @@ private:
void enable_idle_throttle_when_armed(bool value);
px4::atomic_bool _task_should_exit{false}; ///< flag to indicate to tear down the CAN driver
px4::atomic<int> _fw_server_action{None};
int _fw_server_status{-1};
bool _is_armed{false}; ///< the arming status of the actuators on the bus
unsigned _output_count{0}; ///< number of actuators currently available
@ -232,30 +225,28 @@ private:
uavcan::Node<> _node; ///< library instance
pthread_mutex_t _node_mutex;
px4_sem_t _server_command_sem;
UavcanEscController _esc_controller;
UavcanServoController _servo_controller;
UavcanMixingInterfaceESC _mixing_interface_esc{_node_mutex, _esc_controller};
UavcanMixingInterfaceServo _mixing_interface_servo{_node_mutex, _servo_controller};
UavcanHardpointController _hardpoint_controller;
UavcanBeep _beep_controller;
UavcanSafetyState _safety_state_controller;
UavcanRGBController _rgbled_controller;
uavcan::GlobalTimeSyncMaster _time_sync_master;
uavcan::GlobalTimeSyncSlave _time_sync_slave;
uavcan::NodeStatusMonitor _node_status_monitor;
List<IUavcanSensorBridge *> _sensor_bridges; ///< List of active sensor bridges
uavcan::NodeInfoRetriever _node_info_retriever;
ITxQueueInjector *_tx_injector{nullptr};
List<IUavcanSensorBridge *> _sensor_bridges; ///< List of active sensor bridges
bool _idle_throttle_when_armed{false};
int32_t _idle_throttle_when_armed_param{0};
uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
perf_counter_t _cycle_perf;
perf_counter_t _interval_perf;
perf_counter_t _cycle_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle time")};
perf_counter_t _interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": cycle interval")};
void handle_time_sync(const uavcan::TimerEvent &);
@ -274,7 +265,58 @@ private:
void (UavcanNode::*)(const uavcan::ServiceCallResult<uavcan::protocol::RestartNode> &)> RestartNodeCallback;
void cb_setget(const uavcan::ServiceCallResult<uavcan::protocol::param::GetSet> &result);
uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
uORB::Subscription _vcmd_sub{ORB_ID(vehicle_command)};
uORB::Subscription _param_request_sub{ORB_ID(uavcan_parameter_request)};
uORB::Publication<uavcan_parameter_value_s> _param_response_pub{ORB_ID(uavcan_parameter_value)};
uORB::Publication<vehicle_command_ack_s> _command_ack_pub{ORB_ID(vehicle_command_ack)};
/*
* 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.
*/
uint8_t _param_counts[128] {};
bool _count_in_progress{false};
uint8_t _count_index{0};
bool _param_in_progress{false};
uint8_t _param_index{0};
bool _param_list_in_progress{false};
bool _param_list_all_nodes{false};
uint8_t _param_list_node_id{1};
uint32_t _param_dirty_bitmap[4] {};
uint8_t _param_save_opcode{0};
bool _cmd_in_progress{false};
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;
uavcan::ServiceClient<uavcan::protocol::param::ExecuteOpcode, ExecuteOpcodeCallback> _param_opcode_client;
uavcan::ServiceClient<uavcan::protocol::RestartNode, RestartNodeCallback> _param_restartnode_client;
void param_count(uavcan::NodeID node_id);
void param_opcode(uavcan::NodeID node_id);
uint8_t get_next_active_node_id(uint8_t base);
uint8_t get_next_dirty_node_id(uint8_t base);
void set_node_params_dirty(uint8_t node_id) { _param_dirty_bitmap[node_id >> 5] |= 1 << (node_id & 31); }
void clear_node_params_dirty(uint8_t node_id) { _param_dirty_bitmap[node_id >> 5] &= ~(1 << (node_id & 31)); }
bool are_node_params_dirty(uint8_t node_id) const { return bool((_param_dirty_bitmap[node_id >> 5] >> (node_id & 31)) & 1); }
bool _check_fw{false};
UavcanServers *_servers{nullptr};
};

12
src/drivers/uavcan/uavcan_module.hpp
View File

@ -66,18 +66,6 @@
// ioctl interface
#define _UAVCAN_IOC(_n) (_IOC(_UAVCAN_IOCBASE, _n))
#define _UAVCAN_IOCBASE (0x4000) // IOCTL base for module UAVCAN
/*
* Query if node identification is in progress. Returns:
* EINVAL - not applicable in the current operating mode
* ETIME - network discovery complete
* OK (0) - network discovery in progress
*/
#define UAVCAN_IOCG_NODEID_INPROGRESS _UAVCAN_IOC(1)
/*
* Set hardpoint command. Accepts a pointer to uavcan::equipment::hardpoint::Command; returns nothing.
* The pointer may be invalidated once the call returns.
*/
#define UAVCAN_IOCS_HARDPOINT_SET _UAVCAN_IOC(10)
// public prototypes
extern "C" __EXPORT int uavcan_main(int argc, char *argv[]);

840
src/drivers/uavcan/uavcan_servers.cpp
View File

@ -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, &param);
tattr.stacksize = PX4_STACK_ADJUSTED(StackSize);
param.sched_priority = Priority;
if (pthread_attr_setschedparam(&tattr, &param)) {
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] {};

170
src/drivers/uavcan/uavcan_servers.hpp
View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2014 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
@ -34,33 +34,18 @@
#pragma once
#include <px4_platform_common/px4_config.h>
#include "uavcan_driver.hpp"
#include <drivers/device/Device.hpp>
#include <lib/perf/perf_counter.h>
#include <uORB/Publication.hpp>
#include <uORB/topics/uavcan_parameter_value.h>
#include <uORB/topics/vehicle_command_ack.h>
#include <uavcan/node/sub_node.hpp>
#include <uavcan/protocol/node_status_monitor.hpp>
#include <uavcan/protocol/param/GetSet.hpp>
#include <uavcan/protocol/param/ExecuteOpcode.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/protocol/firmware_update_trigger.hpp>
#include <uavcan/protocol/node_info_retriever.hpp>
#include <uavcan/protocol/node_status_monitor.hpp>
#include <uavcan_posix/basic_file_server_backend.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_module.hpp"
#include "uavcan_virtual_can_driver.hpp"
/**
* @file uavcan_servers.hpp
@ -74,152 +59,29 @@
/**
* A UAVCAN Server Sub node.
*/
class __EXPORT UavcanServers
class UavcanServers
{
static constexpr unsigned NumIfaces = 1; // UAVCAN_STM32_NUM_IFACES
static constexpr unsigned StackSize = 6000;
static constexpr unsigned Priority = 120;
static constexpr unsigned VirtualIfaceBlockAllocationQuota = 80;
static constexpr float BeepFrequencyGenericIndication = 1000.0F;
static constexpr float BeepFrequencySuccess = 2000.0F;
static constexpr float BeepFrequencyError = 100.0F;
public:
UavcanServers(uavcan::INode &main_node);
UavcanServers(uavcan::INode &node, uavcan::NodeInfoRetriever &node_info_retriever);
~UavcanServers() = default;
virtual ~UavcanServers();
static int start(uavcan::INode &main_node);
static int stop();
uavcan::SubNode<> &get_node() { return _subnode; }
static UavcanServers *instance() { return _instance; }
/*
* Set the main node's pointer to to the injector
* This is a work around as main_node.getDispatcher().remeveRxFrameListener();
* would require a dynamic cast and rtti is not enabled.
*/
void attachITxQueueInjector(ITxQueueInjector **injector) {*injector = &_vdriver;}
void requestCheckAllNodesFirmwareAndUpdate() { _check_fw = true; }
int init();
bool guessIfAllDynamicNodesAreAllocated() { return _server_instance.guessIfAllDynamicNodesAreAllocated(); }
private:
pthread_t _subnode_thread{-1};
pthread_mutex_t _subnode_mutex{};
px4::atomic_bool _subnode_thread_should_exit{false};
int init();
pthread_addr_t run(pthread_addr_t);
static UavcanServers *_instance; ///< singleton pointer
VirtualCanDriver _vdriver;
uavcan::SubNode<> _subnode;
uavcan::INode &_main_node;
void unpackFwFromROMFS(const char *sd_path, const char *romfs_path);
void migrateFWFromRoot(const char *sd_path, const char *sd_root_path);
int copyFw(const char *dst, const char *src);
uavcan_posix::dynamic_node_id_server::FileEventTracer _tracer;
uavcan_posix::dynamic_node_id_server::FileStorageBackend _storage_backend;
uavcan_posix::FirmwareVersionChecker _fw_version_checker;
uavcan::dynamic_node_id_server::CentralizedServer _server_instance; ///< server singleton pointer
uavcan_posix::BasicFileServerBackend _fileserver_backend;
uavcan::NodeInfoRetriever _node_info_retriever;
uavcan::FirmwareUpdateTrigger _fw_upgrade_trigger;
uavcan::BasicFileServer _fw_server;
uavcan_posix::BasicFileServerBackend _fileserver_backend;
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.
*/
uint8_t _param_counts[128] = {};
bool _count_in_progress = false;
uint8_t _count_index = 0;
bool _param_in_progress = false;
uint8_t _param_index = 0;
bool _param_list_in_progress = false;
bool _param_list_all_nodes = false;
uint8_t _param_list_node_id = 1;
uint32_t _param_dirty_bitmap[4] = {};
uint8_t _param_save_opcode = 0;
bool _cmd_in_progress = false;
// uORB topic handle for MAVLink parameter responses
uORB::Publication<uavcan_parameter_value_s> _param_response_pub{ORB_ID(uavcan_parameter_value)};
uORB::Publication<vehicle_command_ack_s> _command_ack_pub{ORB_ID(vehicle_command_ack)};
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;
uavcan::ServiceClient<uavcan::protocol::param::ExecuteOpcode, ExecuteOpcodeCallback> _param_opcode_client;
uavcan::ServiceClient<uavcan::protocol::RestartNode, RestartNodeCallback> _param_restartnode_client;
void param_count(uavcan::NodeID node_id);
void param_opcode(uavcan::NodeID node_id);
uint8_t get_next_active_node_id(uint8_t base);
uint8_t get_next_dirty_node_id(uint8_t base);
void set_node_params_dirty(uint8_t node_id) { _param_dirty_bitmap[node_id >> 5] |= 1 << (node_id & 31); }
void clear_node_params_dirty(uint8_t node_id) { _param_dirty_bitmap[node_id >> 5] &= ~(1 << (node_id & 31)); }
bool are_node_params_dirty(uint8_t node_id) const { return bool((_param_dirty_bitmap[node_id >> 5] >> (node_id & 31)) & 1); }
void beep(float frequency);
bool _mutex_inited = false;
volatile bool _check_fw = false;
// ESC enumeration
bool _esc_enumeration_active = false;
uint8_t _esc_enumeration_ids[uavcan::equipment::esc::RawCommand::FieldTypes::cmd::MaxSize];
uint8_t _esc_enumeration_index = 0;
uint8_t _esc_count = 0;
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;
void unpackFwFromROMFS(const char *sd_path, const char *romfs_path);
void migrateFWFromRoot(const char *sd_path, const char *sd_root_path);
int copyFw(const char *dst, const char *src);
uavcan::NodeInfoRetriever &_node_info_retriever;
};

512
src/drivers/uavcan/uavcan_virtual_can_driver.hpp
View File

@ -1,512 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2015 PX4 Development Team. All rights reserved.
* Pavel Kirienko <pavel.kirienko@zubax.com>
* David Sidrane <david_s5@nscdg.com>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#pragma once
#include <nuttx/config.h>
#include <cstdlib>
#include <cstdint>
#include <cstring>
#include <cstdio>
#include <fcntl.h>
#include <pthread.h>
#include <semaphore.h>
#include <debug.h>
#include <uavcan/node/sub_node.hpp>
#include <uavcan/protocol/node_status_monitor.hpp>
/*
* General purpose wrapper around os's mutual exclusion
* mechanism.
*
* It supports the
*
* Note the naming of thier_mutex_ implies that the underlying
* mutex is owned by class using the Lock.
* and this wrapper provides service to initialize and de initialize
* the mutex.
*/
class Lock
{
pthread_mutex_t &thier_mutex_;
public:
Lock(pthread_mutex_t &m) :
thier_mutex_(m)
{
(void)pthread_mutex_lock(&m);
}
~Lock()
{
(void)pthread_mutex_unlock(&thier_mutex_);
}
static int init(pthread_mutex_t &thier_mutex_)
{
return pthread_mutex_init(&thier_mutex_, NULL);
}
static int deinit(pthread_mutex_t &thier_mutex_)
{
return pthread_mutex_destroy(&thier_mutex_);
}
};
/**
* Generic queue based on the linked list class defined in libuavcan.
* This class does not use heap memory.
*/
template <typename T>
class Queue
{
struct Item : public uavcan::LinkedListNode<Item> {
T payload;
template <typename... Args>
Item(Args... args) : payload(args...) { }
};
uavcan::LimitedPoolAllocator allocator_;
uavcan::LinkedListRoot<Item> list_;
public:
Queue(uavcan::IPoolAllocator &arg_allocator, std::size_t block_allocation_quota) :
allocator_(arg_allocator, block_allocation_quota)
{
uavcan::IsDynamicallyAllocatable<Item>::check();
}
~Queue()
{
while (!isEmpty()) {
pop();
}
}
bool isEmpty() const { return list_.isEmpty(); }
/**
* Creates one item in-place at the end of the list.
* Returns true if the item was appended successfully, false if there's not enough memory.
* Complexity is O(N) where N is queue length.
*/
template <typename... Args>
bool tryEmplace(Args... args)
{
// Allocating memory
void *const ptr = allocator_.allocate(sizeof(Item));
if (ptr == nullptr) {
return false;
}
// Constructing the new item
Item *const item = new (ptr) Item(args...);
assert(item != nullptr);
// Inserting the new item at the end of the list
Item *p = list_.get();
if (p == nullptr) {
list_.insert(item);
} else {
while (p->getNextListNode() != nullptr) {
p = p->getNextListNode();
}
assert(p->getNextListNode() == nullptr);
p->setNextListNode(item);
assert(p->getNextListNode()->getNextListNode() == nullptr);
}
return true;
}
/**
* Accesses the first element.
* Nullptr will be returned if the queue is empty.
* Complexity is O(1).
*/
T *peek() { return isEmpty() ? nullptr : &list_.get()->payload; }
const T *peek() const { return isEmpty() ? nullptr : &list_.get()->payload; }
/**
* Removes the first element.
* If the queue is empty, nothing will be done and assertion failure will be triggered.
* Complexity is O(1).
*/
void pop()
{
Item *const item = list_.get();
assert(item != nullptr);
if (item != nullptr) {
list_.remove(item);
item->~Item();
allocator_.deallocate(item);
}
}
};
/**
* Objects of this class are owned by the sub-node thread.
* This class does not use heap memory.
*/
class VirtualCanIface : public uavcan::ICanIface,
uavcan::Noncopyable
{
/**
* This class re-defines uavcan::RxCanFrame with flags.
* Simple inheritance or composition won't work here, because the 40 byte limit will be exceeded,
* rendering this class unusable with Queue<>.
*/
struct RxItem: public uavcan::CanFrame {
const uavcan::MonotonicTime ts_mono;
const uavcan::UtcTime ts_utc;
const uavcan::CanIOFlags flags;
const uint8_t iface_index;
RxItem(const uavcan::CanRxFrame &arg_frame, uavcan::CanIOFlags arg_flags) :
uavcan::CanFrame(arg_frame),
ts_mono(arg_frame.ts_mono),
ts_utc(arg_frame.ts_utc),
flags(arg_flags),
iface_index(arg_frame.iface_index)
{
// Making sure it will fit into a pool block with a pointer prefix
static_assert(sizeof(RxItem) <= (uavcan::MemPoolBlockSize - 8), "Bad coder, no coffee");
}
};
pthread_mutex_t &common_driver_mutex_;
uavcan::CanTxQueue prioritized_tx_queue_;
Queue<RxItem> rx_queue_;
int16_t send(const uavcan::CanFrame &frame, uavcan::MonotonicTime tx_deadline, uavcan::CanIOFlags flags) override
{
Lock lock(common_driver_mutex_);
prioritized_tx_queue_.push(frame, tx_deadline, uavcan::CanTxQueue::Volatile, flags);
return 1;
}
int16_t receive(uavcan::CanFrame &out_frame, uavcan::MonotonicTime &out_ts_monotonic,
uavcan::UtcTime &out_ts_utc, uavcan::CanIOFlags &out_flags) override
{
Lock lock(common_driver_mutex_);
if (rx_queue_.isEmpty()) {
return 0;
}
const auto item = *rx_queue_.peek();
rx_queue_.pop();
out_frame = item;
out_ts_monotonic = item.ts_mono;
out_ts_utc = item.ts_utc;
out_flags = item.flags;
return 1;
}
int16_t configureFilters(const uavcan::CanFilterConfig *, std::uint16_t) override { return -uavcan::ErrDriver; }
uint16_t getNumFilters() const override { return 0; }
uint64_t getErrorCount() const override { return 0; }
public:
VirtualCanIface(uavcan::IPoolAllocator &allocator, uavcan::ISystemClock &clock,
pthread_mutex_t &arg_mutex, unsigned quota_per_queue) :
common_driver_mutex_(arg_mutex),
prioritized_tx_queue_(allocator, clock, quota_per_queue),
rx_queue_(allocator, quota_per_queue)
{
}
~VirtualCanIface()
{
}
/**
* Note that RX queue overwrites oldest items when overflowed.
* Call this from the main thread only.
* No additional locking is required.
*/
void addRxFrame(const uavcan::CanRxFrame &frame, uavcan::CanIOFlags flags)
{
Lock lock(common_driver_mutex_);
if (!rx_queue_.tryEmplace(frame, flags) && !rx_queue_.isEmpty()) {
rx_queue_.pop();
(void)rx_queue_.tryEmplace(frame, flags);
}
}
/**
* Call this from the main thread only.
* No additional locking is required.
*/
void flushTxQueueTo(uavcan::INode &main_node, std::uint8_t iface_index)
{
Lock lock(common_driver_mutex_);
const std::uint8_t iface_mask = static_cast<std::uint8_t>(1U << iface_index);
while (auto e = prioritized_tx_queue_.peek()) {
UAVCAN_TRACE("VirtualCanIface", "TX injection [iface=0x%02x]: %s",
unsigned(iface_mask), e->toString().c_str());
const int res = main_node.injectTxFrame(e->frame, e->deadline, iface_mask,
uavcan::CanTxQueue::Qos(e->qos), e->flags);
prioritized_tx_queue_.remove(e);
if (res <= 0) {
break;
}
}
}
/**
* Call this from the sub-node thread only.
* No additional locking is required.
*/
bool hasDataInRxQueue()
{
Lock lock(common_driver_mutex_);
return !rx_queue_.isEmpty();
}
};
/**
* This interface defines one method that will be called by the main node thread periodically in order to
* transfer contents of TX queue of the sub-node into the TX queue of the main node.
*/
class ITxQueueInjector
{
public:
virtual ~ITxQueueInjector() { }
/**
* Flush contents of TX queues into the main node.
* @param main_node Reference to the main node.
*/
virtual void injectTxFramesInto(uavcan::INode &main_node) = 0;
};
/**
* Objects of this class are owned by the sub-node thread.
* This class does not use heap memory.
*/
class VirtualCanDriver : public uavcan::ICanDriver,
public uavcan::IRxFrameListener,
public ITxQueueInjector,
uavcan::Noncopyable
{
class Event
{
FAR px4_sem_t sem;
public:
int init()
{
int rv = px4_sem_init(&sem, 0, 0);
if (rv == 0) {
px4_sem_setprotocol(&sem, SEM_PRIO_NONE);
}
return rv;
}
int deinit()
{
return px4_sem_destroy(&sem);
}
Event()
{
}
~Event()
{
}
/**
*/
void waitFor(uavcan::MonotonicDuration duration)
{
static const int NsPerSec = 1000000000;
if (duration.isPositive()) {
auto abstime = ::timespec();
if (clock_gettime(CLOCK_REALTIME, &abstime) >= 0) {
abstime.tv_nsec += duration.toUSec() * 1000;
if (abstime.tv_nsec >= NsPerSec) {
abstime.tv_sec++;
abstime.tv_nsec -= NsPerSec;
}
(void)px4_sem_timedwait(&sem, &abstime);
}
}
}
void signal()
{
int count;
int rv = px4_sem_getvalue(&sem, &count);
if (rv == 0 && count <= 0) {
px4_sem_post(&sem);
}
}
};
Event event_; ///< Used to unblock the select() call when IO happens.
pthread_mutex_t driver_mutex_; ///< Shared across all ifaces
uavcan::LazyConstructor<VirtualCanIface> ifaces_[uavcan::MaxCanIfaces];
const unsigned num_ifaces_;
uavcan::ISystemClock &clock_;
uavcan::ICanIface *getIface(uint8_t iface_index) override
{
return (iface_index < num_ifaces_) ? ifaces_[iface_index].operator VirtualCanIface * () : nullptr;
}
uint8_t getNumIfaces() const override { return num_ifaces_; }
/**
* This and other methods of ICanDriver will be invoked by the sub-node thread.
*/
int16_t select(uavcan::CanSelectMasks &inout_masks,
const uavcan::CanFrame * (&)[uavcan::MaxCanIfaces],
uavcan::MonotonicTime blocking_deadline) override
{
bool need_block = (inout_masks.write == 0); // Write queue is infinite
for (unsigned i = 0; need_block && (i < num_ifaces_); i++) {
const bool need_read = inout_masks.read & (1U << i);
if (need_read && ifaces_[i]->hasDataInRxQueue()) {
need_block = false;
}
}
if (need_block) {
event_.waitFor(blocking_deadline - clock_.getMonotonic());
}
inout_masks = uavcan::CanSelectMasks();
for (unsigned i = 0; i < num_ifaces_; i++) {
const std::uint8_t iface_mask = 1U << i;
inout_masks.write |= iface_mask; // Always ready to write
if (ifaces_[i]->hasDataInRxQueue()) {
inout_masks.read |= iface_mask;
}
}
return num_ifaces_; // We're always ready to write, hence > 0.
}
/**
* This handler will be invoked by the main node thread.
*/
void handleRxFrame(const uavcan::CanRxFrame &frame, uavcan::CanIOFlags flags) override
{
UAVCAN_TRACE("VirtualCanDriver", "RX [flags=%u]: %s", unsigned(flags), frame.toString().c_str());
if (frame.iface_index < num_ifaces_) {
ifaces_[frame.iface_index]->addRxFrame(frame, flags);
event_.signal();
}
}
/**
* This method will be invoked by the main node thread.
*/
void injectTxFramesInto(uavcan::INode &main_node) override
{
for (unsigned i = 0; i < num_ifaces_; i++) {
ifaces_[i]->flushTxQueueTo(main_node, i);
}
event_.signal();
}
public:
VirtualCanDriver(unsigned arg_num_ifaces,
uavcan::ISystemClock &system_clock,
uavcan::IPoolAllocator &allocator,
unsigned virtual_iface_block_allocation_quota) :
num_ifaces_(arg_num_ifaces),
clock_(system_clock)
{
Lock::init(driver_mutex_);
event_.init();
assert(num_ifaces_ > 0 && num_ifaces_ <= uavcan::MaxCanIfaces);
const unsigned quota_per_queue = virtual_iface_block_allocation_quota; // 2x overcommit
for (unsigned i = 0; i < num_ifaces_; i++) {
ifaces_[i].construct<uavcan::IPoolAllocator &, uavcan::ISystemClock &, pthread_mutex_t &,
unsigned>(allocator, clock_, driver_mutex_, quota_per_queue);
}
}
~VirtualCanDriver()
{
Lock::deinit(driver_mutex_);
event_.deinit();
}
};