From 2e2ac36cab867fae38e2180edbc3159672e9020d Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Mon, 10 Jan 2022 11:13:02 -0500 Subject: [PATCH] drivers/uavcan: make firmware server available on both CAN1/CAN2 always (if enabled) - uavcan firmware server no longer shuts down when arming (nodes might restart in flight) - always handle UAVCAN parameters with or without the FW server active - remove legacy ESC enumeration in FW server --- ROMFS/px4fmu_common/init.d/rcS | 10 +- src/drivers/uavcan/uavcan_main.cpp | 727 +++++++++------ src/drivers/uavcan/uavcan_main.hpp | 116 ++- src/drivers/uavcan/uavcan_module.hpp | 12 - src/drivers/uavcan/uavcan_servers.cpp | 840 +----------------- src/drivers/uavcan/uavcan_servers.hpp | 170 +--- .../uavcan/uavcan_virtual_can_driver.hpp | 512 ----------- 7 files changed, 565 insertions(+), 1822 deletions(-) delete mode 100644 src/drivers/uavcan/uavcan_virtual_can_driver.hpp diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index f71d1d655b..f1bcd9fc40 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -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 diff --git a/src/drivers/uavcan/uavcan_main.cpp b/src/drivers/uavcan/uavcan_main.cpp index fbb7f904bc..3a2575c6a2 100644 --- a/src/drivers/uavcan/uavcan_main.cpp +++ b/src/drivers/uavcan/uavcan_main.cpp @@ -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 &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 &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() = request.real_value; + + } else if (request.param_type == uavcan_parameter_request_s::PARAM_TYPE_UINT8) { + req.value.to() = request.int_value; + + } else { + req.value.to() = 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(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(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 &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(); + + } 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(); + + } 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(); + } + + _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 &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 &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] |reset |\n" - "\t hardpoint set }"); + "\tuavcan {start|status|stop|shrink|update}\n" + "\t param [set|get|list|save] |reset "); } -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(); diff --git a/src/drivers/uavcan/uavcan_main.hpp b/src/drivers/uavcan/uavcan_main.hpp index 7b40213667..4e9d04aa72 100644 --- a/src/drivers/uavcan/uavcan_main.hpp +++ b/src/drivers/uavcan/uavcan_main.hpp @@ -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 #include -#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 +#include +#include #include #include #include +#include #include -#include #include +#include #include -#include -#include -#include - +#include #include #include #include +#include +#include +#include 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 _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 _sensor_bridges; ///< List of active sensor bridges + uavcan::NodeInfoRetriever _node_info_retriever; - ITxQueueInjector *_tx_injector{nullptr}; + List _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 &)> RestartNodeCallback; void cb_setget(const uavcan::ServiceCallResult &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 _param_response_pub{ORB_ID(uavcan_parameter_value)}; + uORB::Publication _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 &result); + void cb_count(const uavcan::ServiceCallResult &result); void cb_opcode(const uavcan::ServiceCallResult &result); void cb_restart(const uavcan::ServiceCallResult &result); + uavcan::ServiceClient _param_getset_client; + uavcan::ServiceClient _param_opcode_client; + uavcan::ServiceClient _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}; }; diff --git a/src/drivers/uavcan/uavcan_module.hpp b/src/drivers/uavcan/uavcan_module.hpp index ea7420482a..9941248dbf 100644 --- a/src/drivers/uavcan/uavcan_module.hpp +++ b/src/drivers/uavcan/uavcan_module.hpp @@ -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[]); diff --git a/src/drivers/uavcan/uavcan_servers.cpp b/src/drivers/uavcan/uavcan_servers.cpp index 210887be43..d6a05dba67 100644 --- a/src/drivers/uavcan/uavcan_servers.cpp +++ b/src/drivers/uavcan/uavcan_servers.cpp @@ -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 #include #include -#include -#include -#include - /** * @file uavcan_servers.cpp * @@ -70,147 +65,22 @@ * David Sidrane */ -/* - * UavcanNode - */ - -UavcanServers *UavcanServers::_instance; - -UavcanServers::UavcanServers(uavcan::INode &main_node) : - _vdriver(NumIfaces, UAVCAN_DRIVER::SystemClock::instance(), main_node.getAllocator(), VirtualIfaceBlockAllocationQuota), - _subnode(_vdriver, UAVCAN_DRIVER::SystemClock::instance(), main_node.getAllocator()), - _main_node(main_node), - _server_instance(_subnode, _storage_backend, _tracer), - _fileserver_backend(_subnode), - _node_info_retriever(_subnode), - _fw_upgrade_trigger(_subnode, _fw_version_checker), - _fw_server(_subnode, _fileserver_backend), - _param_getset_client(_subnode), - _param_opcode_client(_subnode), - _param_restartnode_client(_subnode), - _beep_pub(_subnode), - _enumeration_indication_sub(_subnode), - _enumeration_client(_subnode), - _enumeration_getset_client(_subnode), - _enumeration_save_client(_subnode) +UavcanServers::UavcanServers(uavcan::INode &node, uavcan::NodeInfoRetriever &node_info_retriever) : + _server_instance(node, _storage_backend, _tracer), + _fileserver_backend(node), + _fw_upgrade_trigger(node, _fw_version_checker), + _fw_server(node, _fileserver_backend), + _node_info_retriever(node_info_retriever) { } -UavcanServers::~UavcanServers() +int UavcanServers::init() { - if (_mutex_inited) { - (void)Lock::deinit(_subnode_mutex); - } - - _main_node.getDispatcher().removeRxFrameListener(); -} - -int -UavcanServers::stop() -{ - UavcanServers *server = instance(); - - if (server == nullptr) { - PX4_INFO("Already stopped"); - return -1; - } - - if (server->_subnode_thread) { - PX4_INFO("stopping fw srv thread..."); - server->_subnode_thread_should_exit.store(true); - (void)pthread_join(server->_subnode_thread, NULL); - } - - _instance = nullptr; - - server->_main_node.getDispatcher().removeRxFrameListener(); - - delete server; - return 0; -} - -int -UavcanServers::start(uavcan::INode &main_node) -{ - if (_instance != nullptr) { - PX4_INFO("Already started"); - return -1; - } - - /* - * Node init - */ - _instance = new UavcanServers(main_node); - - if (_instance == nullptr) { - PX4_ERR("Out of memory"); - return -2; - } - - int rv = _instance->init(); - - if (rv < 0) { - PX4_ERR("Node init failed: %d", rv); - delete _instance; - _instance = nullptr; - return rv; - } - - /* - * Start the thread. Normally it should never exit. - */ - pthread_attr_t tattr; - struct sched_param param; - - pthread_attr_init(&tattr); - (void)pthread_attr_getschedparam(&tattr, ¶m); - tattr.stacksize = PX4_STACK_ADJUSTED(StackSize); - param.sched_priority = Priority; - - if (pthread_attr_setschedparam(&tattr, ¶m)) { - PX4_ERR("setting sched params failed"); - } - - static auto run_trampoline = [](void *) {return UavcanServers::_instance->run(_instance);}; - - rv = pthread_create(&_instance->_subnode_thread, &tattr, static_cast(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() = request.real_value; - - } else if (request.param_type == uavcan_parameter_request_s::PARAM_TYPE_UINT8) { - req.value.to() = request.int_value; - - } else { - req.value.to() = 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(cmd.param1 + 0.5f); - int node_id = static_cast(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(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 &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(); - - } 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(); - - } 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(); - } - - _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 &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 &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 &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 - &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() = 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 &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(); - 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 &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] {}; diff --git a/src/drivers/uavcan/uavcan_servers.hpp b/src/drivers/uavcan/uavcan_servers.hpp index 8c6716edcb..ea2649ad67 100644 --- a/src/drivers/uavcan/uavcan_servers.hpp +++ b/src/drivers/uavcan/uavcan_servers.hpp @@ -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 -#include "uavcan_driver.hpp" -#include -#include -#include -#include -#include - -#include -#include -#include -#include #include -#include -#include -#include #include +#include +#include +#include +#include #include #include #include -#include -#include -#include -#include #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 _param_response_pub{ORB_ID(uavcan_parameter_value)}; - uORB::Publication _command_ack_pub{ORB_ID(vehicle_command_ack)}; - - typedef uavcan::MethodBinder &)> GetSetCallback; - typedef uavcan::MethodBinder &)> - ExecuteOpcodeCallback; - typedef uavcan::MethodBinder &)> RestartNodeCallback; - - void cb_getset(const uavcan::ServiceCallResult &result); - void cb_count(const uavcan::ServiceCallResult &result); - void cb_opcode(const uavcan::ServiceCallResult &result); - void cb_restart(const uavcan::ServiceCallResult &result); - - uavcan::ServiceClient _param_getset_client; - uavcan::ServiceClient _param_opcode_client; - uavcan::ServiceClient _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 &)> - EnumerationBeginCallback; - typedef uavcan::MethodBinder&)> - EnumerationIndicationCallback; - void cb_enumeration_begin(const uavcan::ServiceCallResult &result); - void cb_enumeration_indication(const uavcan::ReceivedDataStructure &msg); - void cb_enumeration_getset(const uavcan::ServiceCallResult &result); - void cb_enumeration_save(const uavcan::ServiceCallResult &result); - - uavcan::Publisher _beep_pub; - uavcan::Subscriber - _enumeration_indication_sub; - uavcan::ServiceClient _enumeration_client; - uavcan::ServiceClient _enumeration_getset_client; - uavcan::ServiceClient _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; }; diff --git a/src/drivers/uavcan/uavcan_virtual_can_driver.hpp b/src/drivers/uavcan/uavcan_virtual_can_driver.hpp deleted file mode 100644 index 171c164c1b..0000000000 --- a/src/drivers/uavcan/uavcan_virtual_can_driver.hpp +++ /dev/null @@ -1,512 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2015 PX4 Development Team. All rights reserved. - * Pavel Kirienko - * David Sidrane - * - * 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 - -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include - -/* - * 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 -class Queue -{ - struct Item : public uavcan::LinkedListNode { - T payload; - - template - Item(Args... args) : payload(args...) { } - }; - - uavcan::LimitedPoolAllocator allocator_; - uavcan::LinkedListRoot list_; - -public: - Queue(uavcan::IPoolAllocator &arg_allocator, std::size_t block_allocation_quota) : - allocator_(arg_allocator, block_allocation_quota) - { - uavcan::IsDynamicallyAllocatable::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 - 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 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(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 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(allocator, clock_, driver_mutex_, quota_per_queue); - } - } - - ~VirtualCanDriver() - { - Lock::deinit(driver_mutex_); - event_.deinit(); - } - -};