PX4学习之-uORB简单体验

一、前言

最近项目使用到 CPU2 与 CPU0 之间的通信, 使用定时器传递消息到 CPU0 后, CPU0 需要将消息分发到不同的应用程序里面. PX4 里面使用的是 uORB 多线程/进程通信机制.

uORB 相关介绍参考: 官方 uORB 介绍 以及 无人机中级篇：第三讲：uORB原理与使用, 下文中的代码细节也不做过多解释。

目前使用的是 Firmware 的 1.82 版本 . 下文使用到的文件夹地址如下:

• uORB 库文件位于 Firmware/src/modules/uORB.

• configs 文件位于 Firmware/cmake/configs

• drivers 文件位于 Firmware/src/drivers

• msg 文件位于 Firmware/msg

二、Coding

主要的步骤为:

1. 新建 msg
2. 新建 publish 和 subscriber
3. 将publish 和 subscriber 添加到 configs 目录下对应的板子里面去
4. 编译上传。

2.1 新建 msg

在 msg 目录下面新建一个 cpu2.msg， 并将 cpu2 添加到 msg/CMakeLists.txt 文件中

cpu2 简单定义一个变量: length

uint32 length  

2.2 新建 publish 和 subscriber

在 drivers 目录下面新建 uorb_publish 目录 以及 uorb_subscriber 目录， 并新建对应的文件, 结构如下

├── uorb_publish
│   ├── CMakeLists.txt
│   └── uorb_publish.cpp
├── uorb_subscriber
│   ├── CMakeLists.txt
│   └── uorb_subscriber.cpp

uorb_publish/CMakeLists.txt

px4_add_module(
	MODULE drivers__uorb_publish
	MAIN uorb_publish
	STACK_MAIN 1200
	COMPILE_FLAGS
	SRCS
		uorb_publish.cpp
	DEPENDS
	)

uorb_publish/uorb_publish.cpp

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <poll.h>
#include <px4_log.h>
#include <uORB/uORB.h>
#include <uORB/topics/cpu2.h>

#include <px4_workqueue.h>

extern "C" __EXPORT int uorb_publish_main(int argc, char *argv[]);

static orb_advert_t cpu2_topic;

static uint8_t cnt = 0;

static struct work_s work;

static struct cpu2_s att;

static void cycle_trampoline(void *arg)
{
    PX4_INFO("publish \n");

    if (cpu2_topic == NULL)
    {
         cpu2_topic = orb_advertise(ORB_ID(cpu2), &att);

    }

    att.length = cnt;
    cnt++;

    if (cnt == 255) {
        cnt = 0;
    }  

    orb_publish(ORB_ID(cpu2), cpu2_topic, &att);

    (void)work_queue(HPWORK, &work, (worker_t)&cycle_trampoline, nullptr, USEC2TICK(1000 * 1000));

}

int uorb_publish_main(int argc, char *argv[])
{
    PX4_INFO(" uorb_publish \n");

    work = {};

    cpu2_topic = NULL;

    memset(&att, 0 , sizeof(att));

    (void)work_queue(HPWORK, &work, (worker_t)&cycle_trampoline, nullptr, 0);

    return 0;
}

uorb_subscriber/CMakeLists.txt

px4_add_module(
	MODULE drivers__uorb_subscriber
	MAIN uorb_subscriber
	STACK_MAIN 1200
	COMPILE_FLAGS
	SRCS
		uorb_subscriber.cpp
	DEPENDS
	)

uorb_subscriber/uorb_subscriber.cpp

#include <poll.h>
#include <stdio.h>
#include <stdint.h>
#include <string.h>
#include <px4_log.h>

#include <uORB/topics/cpu2.h>

#include <uORB/uORB.h>
#include <px4_workqueue.h>
#include <drivers/device/device.h>

extern "C" __EXPORT int uorb_subscriber_main(int argc, char *argv[]);

static struct work_s work;

static int cpu2_fd = 0;

static struct cpu2_s att; 

static void cycle_trampoline(void *arg)
{
    // PX4_INFO("subscriber \n");
    if (cpu2_fd == 0)
    {
        cpu2_fd = orb_subscribe(ORB_ID(cpu2));
    }

    bool updated = false;

    updated = false;

    if (orb_check(cpu2_fd, &updated) != PX4_OK)
    {
        return;
    }

    if (updated)
    {
        orb_copy(ORB_ID(cpu2), cpu2_fd, &att);

        PX4_INFO("%d \n", att.length);
    }

    work_queue(HPWORK,
               &work,
               (worker_t)&cycle_trampoline,
               nullptr,
               USEC2TICK(1000 * 1000));
}

int uorb_subscriber_main(int argc, char *argv[])
{

    work = {};
    cpu2_fd = 0;
    memset(&att, 0 , sizeof(att));

    PX4_INFO("uorb_subscriber ");

    (void)work_queue(HPWORK, &work, (worker_t)&cycle_trampoline, nullptr, 0);

    return 0;
}

2.3 将publish 和 subscriber 添加到 configs 目录下对应的板子里面去 以及 编译运行

三、运行结果以及常见问题

1. 编译烧录到板子后，在 nsh 键入 ? ， 会显示如下

       nsh> ?
   help usage:  help [-v] [<cmd>]
   
   [           dirname     false       mkfatfs     pwd         true
   ?           date        free        mkfifo      rm          uname
   basename    dd          help        mkrd        rmdir       umount
   break       df          hexdump     mh          set         unset
   cat         echo        kill        mount       sh          usleep
   cd          printf      ls          mv          sleep       xd
   cp          exec        mb          mw          test
   cmp         exit        mkdir       ps          time        
   
   Builtin Apps:
   
   ...
   uorb
   uorb_publish
   uorb_subscriber
   ...
   nsh>
   

   如果未出现 uorb_publish 与 uorb_subscriber， 原因在于 未将publish 和 subscriber 添加到 configs 目录下对应的板子里面去;

2. 先启动 uorb_subscriber ， 再启动 uorb_publish

   
   nsh> uorb_subscriber
   INFO  [uorb_subscriber] uorb_subscriber
   nsh> uorb_publish
   INFO  [uorb_publish]  uorb_publish 
   
   INFO  [uorb_publish] publish 
   
   nsh> INFO  [uorb_subscriber] 0 
   
   INFO  [uorb_publish] publish 
   
   INFO  [uorb_subscriber] 1 
   
   INFO  [uorb_publish] publish 
   
   INFO  [uorb_subscriber] 2 
   
   INFO  [uorb_publish] publish 
   
   INFO  [uorb_subscriber] 3 
   
   INFO  [uorb_publish] publish 
   
   INFO  [uorb_subscriber] 4 
   
   INFO  [uorb_publish] publish 
   
   INFO  [uorb_subscriber] 5 
   
   INFO  [uorb_publish] publish 
   
   INFO  [uorb_subscriber] 6 
   
   INFO  [uorb_publish] publish
   .....