Pixhawk飞控系统之uORB深入解析
Pixhawk 飞控系统是基于ARM的四轴以上飞行器的飞行控制器， 它的前身是PX4-IMU，Pixhawk 把之前的IMU进行了完整的重构，最新版本是2.4.3。而对应的Pixhawk 1.x版本与2.x版本的区别在于，I/O板与FMU是否整合在一起。
uORB是Pixhawk系统中非常重要且关键的一个模块，它肩负了整个系统的数据传输任务，所有的传感器数据、GPS、PPM信号等都要从芯片获取后通过uORB进行传输到各个模块进行计算处理。
uORB 的架构简述
uORB全称为micro object request broker (uORB),即 微对象请求代理器，实际上uORB是一套跨进程的IPC通讯模块。在Pixhawk中， 所有的功能被独立以进程模块为单位进行实现并工作。而进程间的数据交互就由为重要，必须要能够符合实时、有序的特点。
Pixhawk 使用NuttX实时ARM系统， 而uORB对于NuttX而言，它仅仅是一个普通的文件设备对象，这个设备支持Open、Close、Read、Write、Ioctl以及Poll机制。 通过这些接口的实现，uORB提供了一套“点对多”的跨进程广播通讯机制， “点”指的是通讯消息的“源”，“多”指的是一个源可以有多个用户来接收、处理。而“源”与“用户”的关系在于，源不需要去考虑用户是否可以收到某条被广播的消息或什么时候收到这条消息。它只需要单纯的把要广播的数据推送到uORB的消息“总线”上。对于用户而言，源推送了多少次的消息也不重要，重要的是取回最新的这条消息。
uORB实际上是多个进程打开同一个设备文件，进程间通过此文件节点进行数据交互和共享。
uORB 的系统实现
uORB的实现位于固件源码的src/modules/uORB/uORB.cpp文件，它通过重载CDev基类来组织一个uORB的设备实例。并且完成Read/Write等功能的重载。uORB 的入口点是uorb_main函数，在这里它检查uORB的启动参数来完成对应的功能，uORB支持start/test/status这3条启动参数，在Pixhawk的rcS启动脚本中，使用start参数来进行初始化，其他2个参数分别用来进行uORB功能的自检和列出uORB的当前状态。
在rcS中使用start参数启动uORB后，uORB会创建并初始化它的设备实例， 其中的实现大部分都在CDev基类完成。这个过程类似于Linux设备驱动中的Probe函数，或者Windows 内核的DriverEntry，通过init调用完成设备的创建，节点注册以及派遣例程的设置等。
uORB 源码分析之Open
uORB 的Open接口实现了“源”或“用户” 打开uORB句柄的功能，打开uORB的句柄就意味着一个源的创建或把一个用户关联到某个源。我在这以源的创建为开端，逐步讲解Open的过程：orb_advert_t
orb_advertise(const struct orb_metadata *meta, const void *data)
{
int result, fd;
orb_advert_t advertiser;
/* open the node as an advertiser */
fd = node_open(PUBSUB, meta, data, true);
if (fd == ERROR)
return ERROR;
/* get the advertiser handle and close the node */
result = ioctl(fd, ORBIOCGADVERTISER, (unsigned long)&advertiser);
close(fd);
if (result == ERROR)
return ERROR;
/* the advertiser must perform an initial publish to initialise the object */
result= orb_publish(meta, advertiser, data);
if (result == ERROR)
return ERROR;
return advertiser;
}
orb_advertise(const struct orb_metadata *meta, const void *data)
{
int result, fd;
orb_advert_t advertiser;
/* open the node as an advertiser */
fd = node_open(PUBSUB, meta, data, true);
if (fd == ERROR)
return ERROR;
/* get the advertiser handle and close the node */
result = ioctl(fd, ORBIOCGADVERTISER, (unsigned long)&advertiser);
close(fd);
if (result == ERROR)
return ERROR;
/* the advertiser must perform an initial publish to initialise the object */
result= orb_publish(meta, advertiser, data);
if (result == ERROR)
return ERROR;
return advertiser;
}
orb_advertise 其实就是一个int, meta是一个已定义好的源描述信息，里面就2个成员，分别为name以及size。保存了通讯的名称以及每次发送数据的长度。 创建源的过程为3个步骤， 打开uORB的设备节点， 获取设备实例， 推送第一条消息。
/*
* Generate the path to the node and try to open it.
*/
ret = node_mkpath(path, f, meta);
if (ret != OK) {
errno = -ret;
return ERROR;
}
/* open the path as either the advertiser or the subscriber */
fd = open(path, (advertiser) ? O_WRONLY : O_RDONLY);
从代码中可以看出， 每个源都在/PUBSUB/目录下有一个设备节点。首先通过node_mkpath来拼接好设备节点路径，然后根据要打开的是源节点还是用户节点来选择标识
int
ORBDevNode::open(struct file *filp)
{
int ret;
/* is this a publisher? */
if (filp->f_oflags == O_WRONLY) {
/* become the publisher if we can */
lock();
if (_publisher == 0) {
_publisher = getpid();
ret = OK;
} else {
ret = -EBUSY;
}
unlock();
/* now complete the open */
if (ret == OK) {
ret = CDev::open(filp);
/* open failed - not the publisher anymore */
if (ret != OK)
_publisher = 0;
}
return ret;
}
/* is this a new subscriber? */
if (filp->f_oflags == O_RDONLY) {
/* allocate subscriber data */
SubscriberData *sd = new SubscriberData;
if (nullptr == sd)
return -ENOMEM;
memset(sd, 0, sizeof(*sd));
/* default to no pending update */
sd->generation = _generation;
filp->f_priv = (void *)sd;
ret = CDev::open(filp);
if (ret != OK)
free(sd);
return ret;
}
/* can only be pub or sub, not both */
return -EINVAL;
}
* Generate the path to the node and try to open it.
*/
ret = node_mkpath(path, f, meta);
if (ret != OK) {
errno = -ret;
return ERROR;
}
/* open the path as either the advertiser or the subscriber */
fd = open(path, (advertiser) ? O_WRONLY : O_RDONLY);
从代码中可以看出， 每个源都在/PUBSUB/目录下有一个设备节点。首先通过node_mkpath来拼接好设备节点路径，然后根据要打开的是源节点还是用户节点来选择标识
int
ORBDevNode::open(struct file *filp)
{
int ret;
/* is this a publisher? */
if (filp->f_oflags == O_WRONLY) {
/* become the publisher if we can */
lock();
if (_publisher == 0) {
_publisher = getpid();
ret = OK;
} else {
ret = -EBUSY;
}
unlock();
/* now complete the open */
if (ret == OK) {
ret = CDev::open(filp);
/* open failed - not the publisher anymore */
if (ret != OK)
_publisher = 0;
}
return ret;
}
/* is this a new subscriber? */
if (filp->f_oflags == O_RDONLY) {
/* allocate subscriber data */
SubscriberData *sd = new SubscriberData;
if (nullptr == sd)
return -ENOMEM;
memset(sd, 0, sizeof(*sd));
/* default to no pending update */
sd->generation = _generation;
filp->f_priv = (void *)sd;
ret = CDev::open(filp);
if (ret != OK)
free(sd);
return ret;
}
/* can only be pub or sub, not both */
return -EINVAL;
}
uORB中规定了源节点只允许写打开，用户节点只允许只读打开。 我认为上面的Open代码里lock到unlock那段根本就不需要~ 那里仅仅是判断不允许重复创建同一个话题而已。而去重完全可以依赖其他的一些机制来解决， CDev::Open就不在继续往里说了。~如果oflags是RDONLY，那就表示要打开的是一个用户设备节点，sd是为这个用户准备的一个上下文结构。里面包含了一些同步计数器等信息，比如sd->generation，这里保存了当前用户读取到的消息的索引号，而_generation来源于源设备的每次写操作，每次源写入数据时，_generation会累加。每次用户读取数据时会把_generation同步到自己的sd->generation，通过这种处理，如果当前用户的sd->generation不等于全局的_generation就意味着源刚刚写入过数据，有最新的通讯消息可以供读取。
ssize_t
ORBDevNode::read(struct file *filp, char *buffer, size_t buflen)
{
SubscriberData *sd = (SubscriberData *)filp_to_sd(filp);
/* if the object has not been written yet, return zero */
if (_data == nullptr)
return 0;
/* if the caller's buffer is the wrong size, that's an error */
if (buflen != _meta->o_size)
return -EIO;
/*
* Perform an atomic copy & state update
*/
irqstate_t flags = irqsave();
/* if the caller doesn't want the data, don't give it to them */
if (nullptr != buffer)
memcpy(buffer, _data, _meta->o_size);
/* track the last generation that the file has seen */
sd->generation = _generation;
/*
* Clear the flag that indicates that an update has been reported, as
* we have just collected it.
*/
sd->update_reported = false;
irqrestore(flags);
return _meta->o_size;
}
ORBDevNode::read(struct file *filp, char *buffer, size_t buflen)
{
SubscriberData *sd = (SubscriberData *)filp_to_sd(filp);
/* if the object has not been written yet, return zero */
if (_data == nullptr)
return 0;
/* if the caller's buffer is the wrong size, that's an error */
if (buflen != _meta->o_size)
return -EIO;
/*
* Perform an atomic copy & state update
*/
irqstate_t flags = irqsave();
/* if the caller doesn't want the data, don't give it to them */
if (nullptr != buffer)
memcpy(buffer, _data, _meta->o_size);
/* track the last generation that the file has seen */
sd->generation = _generation;
/*
* Clear the flag that indicates that an update has been reported, as
* we have just collected it.
*/
sd->update_reported = false;
irqrestore(flags);
return _meta->o_size;
}
读分为3步， 首先判断参数是否合理，然后屏蔽中断拷贝数据，最后更新同步信息。值得注意的是，如果没有源写数据，那么read会在第一个判断就退出，原因是_data缓冲区在首次write时才会成功申请。generation的同步这里也不在继续说了
ssize_t
ORBDevNode::write(struct file *filp, const char *buffer, size_t buflen)
{
/*
* Writes are legal from interrupt context as long as the
* object has already been initialised from thread context.
*
* Writes outside interrupt context will allocate the object
* if it has not yet been allocated.
*
* Note that filp will usually be NULL.
*/
if (nullptr == _data) {
if (!up_interrupt_context()) {
lock();
/* re-check size */
if (nullptr == _data)
_data = new uint8_t[_meta->o_size];
unlock();
}
/* failed or could not allocate */
if (nullptr == _data)
return -ENOMEM;
}
/* If write size does not match, that is an error */
if (_meta->o_size != buflen)
return -EIO;
/* Perform an atomic copy. */
irqstate_t flags = irqsave();
memcpy(_data, buffer, _meta->o_size);
irqrestore(flags);
/* update the timestamp and generation count */
_last_update = hrt_absolute_time();
_generation++;
/* notify any poll waiters */
poll_notify(POLLIN);
return _meta->o_size;
}
ORBDevNode::write(struct file *filp, const char *buffer, size_t buflen)
{
/*
* Writes are legal from interrupt context as long as the
* object has already been initialised from thread context.
*
* Writes outside interrupt context will allocate the object
* if it has not yet been allocated.
*
* Note that filp will usually be NULL.
*/
if (nullptr == _data) {
if (!up_interrupt_context()) {
lock();
/* re-check size */
if (nullptr == _data)
_data = new uint8_t[_meta->o_size];
unlock();
}
/* failed or could not allocate */
if (nullptr == _data)
return -ENOMEM;
}
/* If write size does not match, that is an error */
if (_meta->o_size != buflen)
return -EIO;
/* Perform an atomic copy. */
irqstate_t flags = irqsave();
memcpy(_data, buffer, _meta->o_size);
irqrestore(flags);
/* update the timestamp and generation count */
_last_update = hrt_absolute_time();
_generation++;
/* notify any poll waiters */
poll_notify(POLLIN);
return _meta->o_size;
}
上面就是write的实现了，那个lock/unlock真心很鸡肋，我是感觉多余了，首次write会申请内存用于数据通讯， 然后关闭中断拷贝数据防止在复制的过程用有用户来read，最后是更新最后的更新时间以及同步计数器并且发送一个POLLIN的消息通知来唤醒那些还在等待uORB数据可读的用户
uORB设备的POLL状态
当用户没有指定数据读取的频率时，每次源的write都会触发一个POLLIN来唤醒用户去读取刚更新的数据。是否唤醒除了检查generation的值以外，另外一个要求就是读取频率的限制，每个用户可以单独指定自己打算读更新的频率。
bool
ORBDevNode::appears_updated(SubscriberData *sd)
{
/* assume it doesn't look updated */
bool ret = false;
/* avoid racing between interrupt and non-interrupt context calls */
irqstate_t state = irqsave();
/* check if this topic has been published yet, if not bail out */
if (_data == nullptr) {
ret = false;
goto out;
}
/*
* If the subscriber's generation count matches the update generation
* count, there has been no update from their perspective; if they
* don't match then we might have a visible update.
*/
while (sd->generation != _generation) {
/*
* Handle non-rate-limited subscribers.
*/
if (sd->update_interval == 0) {
ret = true;
break;
}
/*
* If we have previously told the subscriber that there is data,
* and they have not yet collected it, continue to tell them
* that there has been an update. This mimics the non-rate-limited
* behaviour where checking / polling continues to report an update
* until the topic is read.
*/
if (sd->update_reported) {
ret = true;
break;
}
/*
* If the interval timer is still running, the topic should not
* appear updated, even though at this point we know that it has.
* We have previously been through here, so the subscriber
* must have collected the update we reported, otherwise
* update_reported would still be true.
*/
if (!hrt_called(&sd->update_call))
break;
/*
* Make sure that we don't consider the topic to be updated again
* until the interval has passed once more by restarting the interval
* timer and thereby re-scheduling a poll notification at that time.
*/
hrt_call_after(&sd->update_call,
sd->update_interval,
&ORBDevNode::update_deferred_trampoline,
(void *)this);
/*
* Remember that we have told the subscriber that there is data.
*/
sd->update_reported = true;
ret = true;
break;
}
out:
irqrestore(state);
/* consider it updated */
return ret;
}
ORBDevNode::appears_updated(SubscriberData *sd)
{
/* assume it doesn't look updated */
bool ret = false;
/* avoid racing between interrupt and non-interrupt context calls */
irqstate_t state = irqsave();
/* check if this topic has been published yet, if not bail out */
if (_data == nullptr) {
ret = false;
goto out;
}
/*
* If the subscriber's generation count matches the update generation
* count, there has been no update from their perspective; if they
* don't match then we might have a visible update.
*/
while (sd->generation != _generation) {
/*
* Handle non-rate-limited subscribers.
*/
if (sd->update_interval == 0) {
ret = true;
break;
}
/*
* If we have previously told the subscriber that there is data,
* and they have not yet collected it, continue to tell them
* that there has been an update. This mimics the non-rate-limited
* behaviour where checking / polling continues to report an update
* until the topic is read.
*/
if (sd->update_reported) {
ret = true;
break;
}
/*
* If the interval timer is still running, the topic should not
* appear updated, even though at this point we know that it has.
* We have previously been through here, so the subscriber
* must have collected the update we reported, otherwise
* update_reported would still be true.
*/
if (!hrt_called(&sd->update_call))
break;
/*
* Make sure that we don't consider the topic to be updated again
* until the interval has passed once more by restarting the interval
* timer and thereby re-scheduling a poll notification at that time.
*/
hrt_call_after(&sd->update_call,
sd->update_interval,
&ORBDevNode::update_deferred_trampoline,
(void *)this);
/*
* Remember that we have told the subscriber that there is data.
*/
sd->update_reported = true;
ret = true;
break;
}
out:
irqrestore(state);
/* consider it updated */
return ret;
}
uORB 根据用户指定的周期来设置hrt（实时定时器），每过一个时间间隔，hrt会被发生调用，通过hrt_called来检查这个调用。如果未发生，即便此时源的数据已经更新那么也不会返回POLLIN来唤醒用户去读。 简单来说，它通过控制POLLIN的周期来单方面控制用户的读取间隔。 如果是linux平台，当用户指定了时间间隔后， 我会为它单独初始化一个内核定时器，每次poll调用时检查完可用更新后，再次检查定时器即可。