Ejemplo n.º 1
0
int  UavcanNode::reset_node(int remote_node_id)
{
	uavcan::protocol::RestartNode::Request restart_req;
	restart_req.magic_number = restart_req.MAGIC_NUMBER;
	uavcan::ServiceClient<uavcan::protocol::RestartNode, RestartNodeCallback> client(_node);
	client.setCallback(RestartNodeCallback(this, &UavcanNode::cb_restart));
	_callback_success = false;
	int call_res = client.call(remote_node_id, restart_req);

	if (call_res >= 0) {
		while (client.hasPendingCalls()) {
			usleep(10000);
		}
	}

	if (!call_res) {
		std::printf("Failed to reset node: %d\n", remote_node_id);
		return -1;
	}

	return 0;
}
Ejemplo n.º 2
0
pthread_addr_t UavcanServers::run(pthread_addr_t)
{
	prctl(PR_SET_NAME, "uavcan fw srv", 0);

	Lock lock(_subnode_mutex);

	/*
	Copy any firmware bundled in the ROMFS to the appropriate location on the
	SD card, unless the user has copied other firmware for that device.
	*/
	unpackFwFromROMFS(UAVCAN_FIRMWARE_PATH, UAVCAN_ROMFS_FW_PATH);

	/* the subscribe call needs to happen in the same thread,
	 * so not in the constructor */
	int cmd_sub = orb_subscribe(ORB_ID(vehicle_command));
	int param_request_sub = orb_subscribe(ORB_ID(uavcan_parameter_request));
	int armed_sub = orb_subscribe(ORB_ID(actuator_armed));

	/* Set up shared service clients */
	_param_getset_client.setCallback(GetSetCallback(this, &UavcanServers::cb_getset));
	_param_opcode_client.setCallback(ExecuteOpcodeCallback(this, &UavcanServers::cb_opcode));
	_param_restartnode_client.setCallback(RestartNodeCallback(this, &UavcanServers::cb_restart));
	_enumeration_client.setCallback(EnumerationBeginCallback(this, &UavcanServers::cb_enumeration_begin));
	_enumeration_indication_sub.start(EnumerationIndicationCallback(this, &UavcanServers::cb_enumeration_indication));
	_enumeration_getset_client.setCallback(GetSetCallback(this, &UavcanServers::cb_enumeration_getset));
	_enumeration_save_client.setCallback(ExecuteOpcodeCallback(this, &UavcanServers::cb_enumeration_save));

	_count_in_progress = _param_in_progress = _param_list_in_progress = _cmd_in_progress = _param_list_all_nodes = false;
	memset(_param_counts, 0, sizeof(_param_counts));

	_esc_enumeration_active = false;
	memset(_esc_enumeration_ids, 0, sizeof(_esc_enumeration_ids));
	_esc_enumeration_index = 0;

	while (!_subnode_thread_should_exit) {

		if (_check_fw == true) {
			_check_fw = false;
			_node_info_retriever.invalidateAll();
		}

		const int spin_res = _subnode.spin(uavcan::MonotonicDuration::fromMSec(10));
		if (spin_res < 0) {
			warnx("node spin error %i", spin_res);
		}

		// Check for parameter requests (get/set/list)
		bool param_request_ready;
		orb_check(param_request_sub, &param_request_ready);

		if (param_request_ready && !_param_list_in_progress && !_param_in_progress && !_count_in_progress) {
			struct uavcan_parameter_request_s request;
			orb_copy(ORB_ID(uavcan_parameter_request), param_request_sub, &request);

			if (_param_counts[request.node_id]) {
				/*
				 * We know how many parameters are exposed by this node, so
				 * process the request.
				 */
				if (request.message_type == MAVLINK_MSG_ID_PARAM_REQUEST_READ) {
					uavcan::protocol::param::GetSet::Request req;
					if (request.param_index >= 0) {
						req.index = request.param_index;
					} else {
						req.name = (char*)request.param_id;
					}

					int call_res = _param_getset_client.call(request.node_id, req);
					if (call_res < 0) {
						warnx("UAVCAN command bridge: couldn't send GetSet: %d", call_res);
					} else {
						_param_in_progress = true;
						_param_index = request.param_index;
					}
				} else if (request.message_type == MAVLINK_MSG_ID_PARAM_SET) {
					uavcan::protocol::param::GetSet::Request req;
					if (request.param_index >= 0) {
						req.index = request.param_index;
					} else {
						req.name = (char*)request.param_id;
					}

					if (request.param_type == MAV_PARAM_TYPE_REAL32) {
						req.value.to<uavcan::protocol::param::Value::Tag::real_value>() = request.real_value;
					} else if (request.param_type == MAV_PARAM_TYPE_UINT8) {
						req.value.to<uavcan::protocol::param::Value::Tag::boolean_value>() = request.int_value;
					} else {
						req.value.to<uavcan::protocol::param::Value::Tag::integer_value>() = request.int_value;
					}

					// Set the dirty bit for this node
					set_node_params_dirty(request.node_id);

					int call_res = _param_getset_client.call(request.node_id, req);
					if (call_res < 0) {
						warnx("UAVCAN command bridge: couldn't send GetSet: %d", call_res);
					} else {
						_param_in_progress = true;
						_param_index = request.param_index;
					}
				} else if (request.message_type == MAVLINK_MSG_ID_PARAM_REQUEST_LIST) {
					// This triggers the _param_list_in_progress case below.
					_param_index = 0;
					_param_list_in_progress = true;
					_param_list_node_id = request.node_id;
					_param_list_all_nodes = false;

					warnx("UAVCAN command bridge: starting component-specific param list");
				}
			} else if (request.node_id == MAV_COMP_ID_ALL) {
				if (request.message_type == MAVLINK_MSG_ID_PARAM_REQUEST_LIST) {
					/*
					 * This triggers the _param_list_in_progress case below,
					 * but additionally iterates over all active nodes.
					 */
					_param_index = 0;
					_param_list_in_progress = true;
					_param_list_node_id = get_next_active_node_id(1);
					_param_list_all_nodes = true;

					warnx("UAVCAN command bridge: starting global param list with node %hhu", _param_list_node_id);

					if (_param_counts[_param_list_node_id] == 0) {
						param_count(_param_list_node_id);
					}
				}
			} else {
				/*
				 * Need to know how many parameters this node has before we can
				 * continue; count them now and then process the request.
				 */
				param_count(request.node_id);
			}
		}

		// Handle parameter listing index/node ID advancement
		if (_param_list_in_progress && !_param_in_progress && !_count_in_progress) {
			if (_param_index >= _param_counts[_param_list_node_id]) {
				warnx("UAVCAN command bridge: completed param list for node %hhu", _param_list_node_id);
				// Reached the end of the current node's parameter set.
				_param_list_in_progress = false;

				if (_param_list_all_nodes) {
					// We're listing all parameters for all nodes -- get the next node ID
					uint8_t next_id = get_next_active_node_id(_param_list_node_id);
					if (next_id < 128) {
						_param_list_node_id = next_id;
						/*
						 * If there is a next node ID, check if that node's parameters
						 * have been counted before. If not, do it now.
						 */
						if (_param_counts[_param_list_node_id] == 0) {
							param_count(_param_list_node_id);
						}
						// Keep on listing.
						_param_index = 0;
						_param_list_in_progress = true;
						warnx("UAVCAN command bridge: started param list for node %hhu", _param_list_node_id);
					}
				}
			}
		}

		// Check if we're still listing, and need to get the next parameter
		if (_param_list_in_progress && !_param_in_progress && !_count_in_progress) {
			// Ready to request the next value -- _param_index is incremented
			// after each successful fetch by cb_getset
			uavcan::protocol::param::GetSet::Request req;
			req.index = _param_index;

			int call_res = _param_getset_client.call(_param_list_node_id, req);
			if (call_res < 0) {
				_param_list_in_progress = false;
				warnx("UAVCAN command bridge: couldn't send param list GetSet: %d", call_res);
			} else {
				_param_in_progress = true;
			}
		}

		// Check for ESC enumeration commands
		bool cmd_ready;
		orb_check(cmd_sub, &cmd_ready);

		if (cmd_ready && !_cmd_in_progress) {
			struct vehicle_command_s cmd;
			orb_copy(ORB_ID(vehicle_command), cmd_sub, &cmd);

			if (cmd.command == vehicle_command_s::VEHICLE_CMD_PREFLIGHT_UAVCAN) {
				int command_id = static_cast<int>(cmd.param1 + 0.5f);
				int node_id = static_cast<int>(cmd.param2 + 0.5f);
				int call_res;

				warnx("UAVCAN command bridge: received UAVCAN command ID %d, node ID %d", command_id, node_id);

				switch (command_id) {
				case 0:
				case 1: {
						_esc_enumeration_active = command_id;
						_esc_enumeration_index = 0;
						_esc_count = 0;
						uavcan::protocol::enumeration::Begin::Request req;
						// TODO: Incorrect implementation; the parameter name field should be left empty.
						//       Leaving it as-is to avoid breaking compatibility with non-compliant nodes.
						req.parameter_name = "esc_index";
						req.timeout_sec = _esc_enumeration_active ? 65535 : 0;
						call_res = _enumeration_client.call(get_next_active_node_id(1), req);
						if (call_res < 0) {
							warnx("UAVCAN ESC enumeration: couldn't send initial Begin request: %d", call_res);
							beep(BeepFrequencyError);
						} else {
							beep(BeepFrequencyGenericIndication);
						}
						break;
					}
				default: {
						warnx("UAVCAN command bridge: unknown command ID %d", command_id);
						break;
					}
				}
			} else if (cmd.command == vehicle_command_s::VEHICLE_CMD_PREFLIGHT_STORAGE) {
				int command_id = static_cast<int>(cmd.param1 + 0.5f);

				warnx("UAVCAN command bridge: received storage command ID %d", command_id);

				switch (command_id) {
				case 1: {
						// Param save request
						int node_id;
						node_id = get_next_dirty_node_id(1);
						if (node_id < 128) {
							_param_save_opcode = uavcan::protocol::param::ExecuteOpcode::Request::OPCODE_SAVE;
							param_opcode(node_id);
						}
						break;
					}
				case 2: {
						// Command is a param erase request -- apply it to all active nodes by setting the dirty bit
						_param_save_opcode = uavcan::protocol::param::ExecuteOpcode::Request::OPCODE_ERASE;
						for (int i = 1; i < 128; i = get_next_active_node_id(i)) {
							set_node_params_dirty(i);
						}
						param_opcode(get_next_dirty_node_id(1));
						break;
					}
				}
			}
		}

		// Shut down once armed
		// TODO (elsewhere): start up again once disarmed?
		bool updated;
		orb_check(armed_sub, &updated);
		if (updated) {
			struct actuator_armed_s armed;
			orb_copy(ORB_ID(actuator_armed), armed_sub, &armed);

			if (armed.armed && !armed.lockdown) {
				warnx("UAVCAN command bridge: system armed, exiting now.");
				break;
			}
		}
	}

	_subnode_thread_should_exit = false;

	warnx("exiting");
	return (pthread_addr_t) 0;
}