PX4飞控--uORB消息机制--发布与订阅

神奇的月亮狗

已于 2024-02-11 11:08:27 修改

阅读量1.8k

点赞数 43

文章标签：学习

于 2024-02-11 11:05:52 首次发布

本文链接：https://blog.csdn.net/qq_42985705/article/details/135422862

版权

第一节 uORB消息唯一性标识

我首先看的是多旋翼姿态控制模块到底有哪些类型的uORB消息。一共两类：Subcription（订阅消息，代码里的SubcriptionInterval和SubcriptionCallbackWorkItem都是订阅消息的延申）/Publication（发布消息）。多旋翼姿态控制模块共订阅了共参数更新、自动姿态控制状态、手动控制设定值、载体状态（姿态设定值、控制模式、着陆状态、位置状态）等9个消息，发布了期望速度及期望姿态设定值2个消息，从这些订阅和发布的关系里能了解到和哪些功能模块进行了交互。

uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};

uORB::Subscription _autotune_attitude_control_status_sub{ORB_ID(autotune_attitude_control_status)};
uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)};
uORB::Subscription _vehicle_attitude_setpoint_sub{ORB_ID(vehicle_attitude_setpoint)};
uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)};
uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)};
uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)};
uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};

uORB::SubscriptionCallbackWorkItem _vehicle_attitude_sub{this, ORB_ID(vehicle_attitude)};

uORB::Publication<vehicle_rates_setpoint_s>     _vehicle_rates_setpoint_pub{ORB_ID(vehicle_rates_setpoint)};    /**< rate setpoint publication */
uORB::Publication<vehicle_attitude_setpoint_s>  _vehicle_attitude_setpoint_pub;

当我在看到这段代码时，我很疑问为什么这里就能够让程序知道订阅了哪些消息主题。首先，我们看关于在 uORB 中如何创建一个唯一的标识符，区分不同的主题。

以自动姿态控制状态消息为例，一个主题一定是有发布者及订阅者。前文订阅该消息，{}内的ORB_ID(autotune_attitude_control_status)，ORB_ID宏展开过后得到&__orb_autotune_attitude_control_status作为构造函数的输入。（ps：这里插一句，这里使用了统一初始化来初始化各个对象。）这里构造函数进行了重载，根据输入，找到源码，这里初始化了两个成员变量，其中_orb_id就用于储存标识符（默认值无效），初始化该变量即完成对这个主题的唯一性标识。

uORB::Subscription _autotune_attitude_control_status_sub{ORB_ID(autotune_attitude_control_status)};

#define ORB_ID(_name)		&__orb_##_name

Subscription(ORB_ID id, uint8_t instance = 0) :
	_orb_id(id),
	_instance(instance)
	{
		subscribe();
	}

ORB_ID _orb_id{ORB_ID::INVALID};

而在C:\Users\Desktop\learning\PX4\PX4-Autopilot\src\modules\mc_autotune_attitude_control\mc_autotune_attitude_control.hpp中发布了这个主题。

uORB::PublicationData<autotune_attitude_control_status_s> _autotune_attitude_control_status_pub{ORB_ID(autotune_attitude_control_status)};

uORB::PublicationData类模板，实例化时，一步步调用基类的构造函数，跳转值PublicationBase类的构造函数，将成员变量_orb_id设置为输入的id值，和订阅时一样，实现了唯一性标识的设置。但在此时，并未进行主题的发布，仅仅只是实例化了一个对象。

PublicationData(ORB_ID id) : Publication<T>(id) {}

Publication(ORB_ID id) : PublicationBase(id) {}

PublicationBase(ORB_ID id) : _orb_id(id) {}

第二节 uORB消息管理器

在这里，我们先打个断点，跳出来先将uORB消息管理器，作为后面的铺垫。说来这个管理器也不算陌生，在第一篇文章里，工作队列管理器启动时，刚好我们看到一个uorb_start函数，显然这里就是uORB消息的起点了，于是乎我们开始查看这个函数的源码来了解uORB消息的一些机制。

uorb_start函数的源码如下。首先同样是老生常谈的通过标志位查看是否已经在运行，在确保没有运行的前提下，执行uORB消息管理器的初始化，也就是initialize函数。

int uorb_start(void)
{
	if (g_dev != nullptr) {
		PX4_WARN("already loaded");
		return 0;
	}

	if (!uORB::Manager::initialize()) {
		PX4_ERR("uorb manager alloc failed");
		return -ENOMEM;
	}

	return OK;
}

在initialize函数中，判断如果 _instance是 nullptr，说明 uORB 管理器尚未初始化，此时会创建一个新的 uORB 管理器实例，同时将 _instance指向生成的唯一实例uORB::Manager。这个实例里就包含了uORB消息传递所用到的各个函数。

bool uORB::Manager::initialize()
{
	if (_Instance == nullptr) {
		_Instance = new uORB::Manager();
	}

}

第三节 uORB消息的发布

然后我们开始分析，在订阅主题的时候到底发生了什么。在这里还是以自动姿态控制状态这个主题为例，已知自动姿态控制模块发布了这个主题，多旋翼姿态控制模块订阅了这个主题。为了逻辑上更加通顺，我们这里先看发布的时候发生了什么。

那么消息主题在什么地方被发布呢？这里我们首先看自动姿态控制模块的源码，当模块启动后，在其task_spawn函数中实例化了这个模块。而模块的构造函数中，就调用了消息发布的advertis()函数，来完成消息的发布。

int McAutotuneAttitudeControl::task_spawn(int argc, char *argv[])
{
	McAutotuneAttitudeControl *instance = new McAutotuneAttitudeControl();
}

McAutotuneAttitudeControl::McAutotuneAttitudeControl() :
	ModuleParams(nullptr),
	WorkItem(MODULE_NAME, px4::wq_configurations::hp_default)
{
	_autotune_attitude_control_status_pub.advertise();
	reset();
}

在advertise函数中就根据advertised函数的返回值来判断消息是否已经发布，若未发布，则调用orb_advertise_queue函数来进行实际的发布。

bool advertise()
{
	if (!advertised()) {
		_handle = orb_advertise_queue(get_topic(), nullptr, ORB_QSIZE);
	}

    return advertised();
}

bool advertised() const { return _handle != nullptr; }

orb_advertise_queue函数中就涉及到消息管理器的get_instance方法。而前文已经提到过，在初始化管理器时，_instance已经指向了管理器本身，因此这里就是通过get_instance方法，调用管理器的orb_advertise方法。

orb_advert_t orb_advertise_queue(const struct orb_metadata *meta, const void *data, unsigned int queue_size)
{
	return uORB::Manager::get_instance()->orb_advertise(meta, data, queue_size);
}


static uORB::Manager *get_instance() { return _Instance; }

orb_advertise方法的目的是作为一个话题的发布者进行广告发布，也就是注册一个话题到uORB系统，并发布该话题的初始数据。它内部调用 orb_advertise_multi，他是一个更通用的函数，允许指定额外的参数，如实例和优先级。

orb_advert_t orb_advertise(const struct orb_metadata *meta, const void *data, unsigned int queue_size = 1)
{
	return orb_advertise_multi(meta, data, nullptr, queue_size);
}

orb_advertise_multi函数，首先调用node_open方法以广告者身份打开一个uORB节点，并获取到对应的文件描述符fd（随后还会设置发布队列的大小，并进行节点初始化）。在node_open方法中，会进行一系列的判断来决定执行什么任务。当我们需要广告一个节点时，就会在当节点管理器正常启动后，就调用其advertise方法来进行广告。而当广告成功后，就会将节点加入到_node_list数组中，并设置相应的节点存在标志 _node_exists，进行节点管理。（这里的orb_advertise_multi函数和node_open方法都包含了很多内容，这里只选取了发布节点这一部分）

orb_advert_t uORB::Manager::orb_advertise_multi(const struct orb_metadata *meta, const void *data, int *instance,
		unsigned int queue_size)
{
	int fd = node_open(meta, true, instance);

}

int uORB::Manager::node_open(const struct orb_metadata *meta, bool advertiser, int *instance)
{
	if (get_device_master()) {
		ret = _device_master->advertise(meta, advertiser, instance);
	}

}

int uORB::DeviceMaster::advertise(const struct orb_metadata *meta, bool is_advertiser, int *instance)
{
	_node_list.add(node);
	_node_exists[node->get_instance()].set((orb_id_size_t)node->id(), true);
}

第四节 uORB消息的订阅

回到前文，多旋翼控制模块订阅自动姿态控制消息的地方，我们再来看看构造函数里还发生了什么。除去_orb_id外，还用传入的instance 参数初始化类成员变量_instance，此时传入值为0，表示表示单个主题订阅。

subscribe函数源码如下。首先判断成员变量_node是否非空，非空则说明已经订阅了该主题，就直接返回。随后判断_orb_id是否有效，当_orb_id有效且消息管理器初始化完成（这里通过调用get_instance方法来判断，即当_instance已经指向管理器了，就已经初始化完成），便尝试添加订阅消息，即执行uORB::Manager::orb_add_internal_subscriber函数。

bool Subscription::subscribe()
{
	// check if already subscribed
	if (_node != nullptr) {
		return true;
	}

	if (_orb_id != ORB_ID::INVALID && uORB::Manager::get_instance()) {
		unsigned initial_generation;
		void *node = uORB::Manager::orb_add_internal_subscriber(_orb_id, _instance, &initial_generation);

		if (node) {
			_node = node;
			_last_generation = initial_generation;
			return true;
		}
	}

	return false;
}

orb_add_internal_subscriber函数中，前文代码给其三个输入，包括订阅消息的id、实例名称（此时为0值）以及initial generation（这里我把它理解为消息的版本号，用于处理消息的时序问题）。函数内首先定义一个node指针为空，用处后面再说。随后再定义一个指针device_master，这个指针指向的结果，首先是调用消息管理器的get_instance函数（前面已经说过，这个函数的结果，返回值_instance就指向消息管理器本身），随后再调用get_device_master函数，因此等于是调用消息管理器的get_device_master函数，指针device_master指向这个结果的返回值。

void *uORB::Manager::orb_add_internal_subscriber(ORB_ID orb_id, uint8_t instance, unsigned *initial_generation)
{
	uORB::DeviceNode *node = nullptr;
	DeviceMaster *device_master = uORB::Manager::get_instance()->get_device_master();
}

而get_device_master函数的源码如下。总结起来，就是根据成员变量_device_master，来判断是否需要实例一个DeviceMaster，如果实例化失败，就报错。（这个DeviceMaster，就是节点管理器，uORB消息的实质就是一个个节点）

uORB::DeviceMaster *uORB::Manager::get_device_master()
{
	if (!_device_master) {
		_device_master = new DeviceMaster();

		if (_device_master == nullptr) {
			PX4_ERR("Failed to allocate DeviceMaster");
			errno = ENOMEM;
		}
	}

	return _device_master;
}

在DeviceMaster实例成功后，调用其成员函数getDeviceNode。其输入为get_orb_meta(orb_id)及instance（=0），在自动控制姿态状态这个消息中为get_orb_meta（__orb_autotune_attitude_control_status）。get_orb_meta函数具体的源码我没有找到，但是可以来分析一下作用。

void *uORB::Manager::orb_add_internal_subscriber(ORB_ID orb_id, uint8_t instance, unsigned *initial_generation)
{
	if (device_master != nullptr) {
		node = device_master->getDeviceNode(get_orb_meta(orb_id), instance);

}

根据getDeviceNode函数的源码看，输入为orb_metadata类型的指针。而orb_metadata结构体内容如下，主要包括元数据的唯一名称、大小、哈希值及标识符等。get_orb_meta函数就是根据消息id来获取消息id对应的元数据结构。

struct orb_metadata {
	const char    *o_name;              /**< unique object name */
	const uint16_t o_size;              /**< object size */
	const uint16_t o_size_no_padding;   /**< object size w/o padding at the end (for logger) */
	uint32_t message_hash;	/**< Hash over all fields for message compatibility checks */
	orb_id_size_t  o_id;                /**< ORB_ID enum */
};

getDeviceNode函数中首先对输入进行一系列判断，包括元数据是否为空、调用deviceNodeExist函数判断节点是否存在。将元数据中的标识符转为ORB_ID类型输入给deviceNodeExist函数。在这个函数里我们就看到了在消息发布时出现过的_node_exists数组，通过它来检查指定实例和ID的节点是否存在，节点存在的话，就返回true值。当确认节点存在后，调用getDeviceNodeLocked函数.

uORB::DeviceNode *getDeviceNode(const struct orb_metadata *meta, const uint8_t instance)
{
	if (meta == nullptr) {
		return nullptr;
		}

	if (!deviceNodeExists(static_cast<ORB_ID>(meta->o_id), instance)) {
		return nullptr;
		}

    uORB::DeviceNode *node = getDeviceNodeLocked(meta, instance);

}

bool deviceNodeExists(ORB_ID id, const uint8_t instance)
{
	if ((id == ORB_ID::INVALID) || (instance > ORB_MULTI_MAX_INSTANCES - 1)) {
		return false;
	}

	return _node_exists[instance][(orb_id_size_t)id];
}

在getDeviceNodeLocked函数函数中，使用for循环遍历设备节点列表 _node_list中的每个节点。对于每个节点，使用strcmp函数函数比较节点的名称和给定元数据结构中定义的名称（meta->o_name）。如果名称匹配且实例号也匹配，则表示找到了匹配的设备节点。如果找到匹配的节点，则直接返回该节点的指针。

uORB::DeviceNode *uORB::DeviceMaster::getDeviceNodeLocked(const struct orb_metadata *meta, const uint8_t instance)
{
	for (uORB::DeviceNode *node : _node_list) {
		if ((strcmp(node->get_name(), meta->o_name) == 0) && (node->get_instance() == instance)) {
			return node;
		}
	}

	return nullptr;
}

当我们得到对应的节点指针后，回到orb_add_internal_subscriber函数中。在这里调用对应节点的add_internal_subscriber方法，来增加内部订阅者的计数（_subscriber_count）。随后调用get_initial_generation方法，通过 _generation.load() 方法获取当前的发布代数，并将其存储在 generation变量中。如果数据有效（即_data_vaild 为真），则从 generation 中减去 1，以允许订阅者读取之前的发布数据。

void *uORB::Manager::orb_add_internal_subscriber(ORB_ID orb_id, uint8_t instance, unsigned *initial_generation)
{
	if (node) {
		node->add_internal_subscriber();
		*initial_generation = node->get_initial_generation();
	}
	return node;
}

void uORB::DeviceNode::add_internal_subscriber()
{
	_subscriber_count++;
}

unsigned uORB::DeviceNode::get_initial_generation()
{

	// If there any previous publications allow the subscriber to read them
	unsigned generation = _generation.load() - (_data_valid ? 1 : 0);

	return generation;
}

到这里，uORB中订阅消息的步骤就完成了。

第五节总结

总的来说，uORB机制中，当一个模块的消息需要进行发布时，会进行uORB::Manager::get_instance()->orb_advertise(meta, data)->uORB::Manager::orb_advertise_multi()->node_open()这样一个函数调用。在node_open()新建节点，初始化后将其加入到_node_list数组之中。而在订阅的过程中，就调用uORB::Manager::orb_add_internal_subscriber函数，来在_node_list数组找到对应的主题节点，并在节点内增加一个订阅者。

写在最后

uORB消息的发布订阅，大概就写到这里了。原本以为一篇文章能写明白，但是还是低估了系统的复杂程度，大概还得有个两三篇的工作量吧。不过虽然延迟了一点，也算是在新年之初完成了第三篇的内容。我也不知道是不是无用之功，自我安慰吧，但是进一寸有进一寸的欢喜。接下来让我们一起去码头整点薯条吧，新年快乐，respect。

神奇的月亮狗

关注

43
点赞
踩
19

收藏

觉得还不错? 一键收藏
2
评论
PX4飞控--uORB消息机制--发布与订阅

总的来说，uORB机制中，当一个模块的消息需要进行发布时，会进行uORB::Manager::get_instance()->orb_advertise(meta, data)->uORB::Manager::orb_advertise_multi()->node_open()这样一个函数调用。在node_open()新建节点，初始化后将其加入到_node_list数组之中。
复制链接

扫一扫