PX4模块设计之十三：WorkQueue设计

Posted 2022-08-06 lida2003

PX4模块设计之十三：WorkQueue设计

• 1. WorkQueue启动
• 2. WorkQueue接口
• • 2.1 基本接口
  • 2.2 辅助接口
  • 2.3 WorkQueue任务函数
  • • 2.3.1 Flat Build
    • 2.3.2 Protected Build
  • 2.4 重点接口分析
  • • 2.4.1 WorkQueueManagerStart
    • 2.4.2 WorkQueueManagerRun
    • 2.4.3 WorkQueueRunner
    • 2.4.4 WorkQueue::Run
    • 2.4.5 WorkQueueFindOrCreate
• 3. 总结
• 4. 参考资料

1. WorkQueue启动

WorkQueue是PX4飞控软件的Common(公共)组件，通过函数px4::WorkQueueManagerStart开始启动的，这之前请参考PX4模块设计之十：PX4启动过程。

board_app_initialize
 └──> px4_platform_init
     └──> px4::WorkQueueManagerStart

注1：Nuttx系统是支持WorkQueue的。但是PX4在common(公共)组件层实现了类似的WorkQueue的功能，而没有直接采用Nuttx系统的WorkQueue。这里初步怀疑还是历史时间上导致的这个结果。
注2：Nuttx第一版本是在2007年发布，开始支持2-3个MCU((i.e. 8051 and ARM7)；PX4-AutoPilot是2009年开始的项目，2013年ETH Zurich (苏黎世联邦理工大学)的计算机视觉与几何实验室 Lorenz Meier ，发布了第一代实验版本： 双飞控处理器PX4FMU/PX4IO硬件。

所以综上所述，鉴于以下原因
1）其历史原因，PX4-AutoPilot的公共组件WorkQueue独立实现；
2）飞控模式采用C++类继承进行开发和管理；
3）uORB消息组件采用C++类进行管理；
4）HRT高精度定时采集触发管理；

WorkQueue的整体管理上结合了上述历史原因，将C/C++设计，类，继承，以及内核态/用户态。整体感觉异常复杂，这部分内容实在不太敢恭维！！！(—AnyWay, 历史原因吧!!!—)

2. WorkQueue接口

2.1 基本接口

最为基本的WorkQueue管理接口并不负责，主要就是Start/Stop/Status三个。

int WorkQueueManagerStart() //WorkQueue管理启动任务

int WorkQueueManagerStop() //作为基础组件这个基本不需要Stop，至少目前代码上没有看到有Stop的地方。

int WorkQueueManagerStatus() //WorkQueue状态查询

2.2 辅助接口

const wq_config_t & device_bus_to_wq(uint32_t device_id_int)  //device_bus 转 wq配置

const wq_config_t & serial_port_to_wq(const char *serial)     //serial_port 转 wq配置

const wq_config_t & ins_instance_to_wq(uint8_t instance)      //instance 转 wq配置

static WorkQueue * FindWorkQueueByName(const char *name)      //通过名字查WorkQueue

WorkQueue * WorkQueueFindOrCreate(const wq_config_t &new_wq)   //查找或者创建WorkQueue

2.3 WorkQueue任务函数

WorkQueue目前是支持Flat和Protected Build两种编译模式，不同编译模式下最显著的差异就是Flat Build下采用pthread_create建立任务，而Protected Build下采用px4_task_spawn_cmd建立任务。

2.3.1 Flat Build

static void * WorkQueueRunner(void *context)

2.3.2 Protected Build

该函数内部实现会再次调用Flat Build的函数（此时运行的代码空间将会是内核态）。

inline static int WorkQueueRunner(int argc, char *argv[])

2.4 重点接口分析

2.4.1 WorkQueueManagerStart

WorkQueueManagerStart
 ├──> <_wq_manager_should_exit.load() && (_wq_manager_create_queue == nullptr)>
 │   ├──> _wq_manager_should_exit.store(false);
 │   ├──> int task_id = px4_task_spawn_cmd("wq:manager",SCHED_DEFAULT,SCHED_PRIORITY_MAX,PX4_STACK_ADJUSTED(1280),(px4_main_t)&WorkQueueManagerRun,nullptr);
 │   └──> <task_id < 0>
 │       ├──> _wq_manager_should_exit.store(true);
 │       ├──> PX4_ERR("task start failed (%i)", task_id);
 │       └──> return -errno;
 ├──> else
 │   ├──> PX4_WARN("already running");
 │   └──> return PX4_ERROR;
 └──> return PX4_OK;

2.4.2 WorkQueueManagerRun

WorkQueueManagerRun
 ├──> _wq_manager_wqs_list = new BlockingList<WorkQueue *>();
 ├──> _wq_manager_create_queue = new BlockingQueue<const wq_config_t *, 1>();
 ├──> <while (!_wq_manager_should_exit.load())>
 │   ├──> const wq_config_t *wq = _wq_manager_create_queue->pop();  //当没有work queue的时候，管理任务始终阻塞在这里。
 │   └──> <wq != nullptr> //不应该是空，容错以防段错误，里面是建立新的work queue
 │       ├──> [stack, priority, etc]  //略。。。。
 │       ├──> [Flat Build, pthread_create WorkQueueRunner]
 │       ├──> [Protected Build, px4_task_spawn_cmd WorkQueueRunner]
 │       ├──> <pid > 0>
 │       │   └──> PX4_DEBUG("starting: %s, priority: %d, stack: %zu bytes", wq->name, sched_priority, stacksize);
 │       └──> <else>
 │           └──> PX4_ERR("failed to create thread for %s (%i): %s", wq->name, pid, strerror(pid));
 └──> return 0;

2.4.3 WorkQueueRunner

WorkQueueRunner
 ├──> wq_config_t *config = static_cast<wq_config_t *>(context);
 ├──> WorkQueue wq(*config);
 ├──> _wq_manager_wqs_list->add(&wq);    // add to work queue list
 ├──> wq.Run();                          // 这里就是
 ├──> _wq_manager_wqs_list->remove(&wq); // remove from work queue list
 └──> return nullptr;

2.4.4 WorkQueue::Run

WorkQueue::Run
 ├──> <while (!should_exit())>
 │   ├──> do  while (px4_sem_wait(&_process_lock) != 0); // loop as the wait may be interrupted by a signal
 │   ├──> work_lock();
 │   └──> <while (!_q.empty())>
 │       ├──> WorkItem *work = _q.pop();
 │       ├──> work_unlock();         // unlock work queue to run (item may requeue itself)
 │       ├──> work->RunPreamble();
 │       ├──> work->Run();           // 真实需要执行的Run函数，通常是继承WorkItem的对象
 │       ├──> work_lock();           // re-lock
 │       └──> work_unlock();
 └──> PX4_DEBUG("%s: exiting", _config.name);

2.4.5 WorkQueueFindOrCreate

WorkQueueFindOrCreate
 ├──> <_wq_manager_create_queue == nullptr>
 │   ├──> PX4_ERR("not running");
 │   └──> return nullptr;
 ├──> WorkQueue *wq = FindWorkQueueByName(new_wq.name); // search list for existing work queue
 ├──> <wq == nullptr>
 │   ├──> _wq_manager_create_queue->push(&new_wq);  //这里很重要，只有push了，WorkQueueManagerRun里面才能执行下去。
 │   ├──> _uint64_t t = 0;
 │   └──> _<while (wq == nullptr && t < 10_s)>  // we wait until new wq is created, then return
 │       ├──> t += 1_ms;
 │       ├──> px4_usleep(1_ms);
 │       ├──> wq = FindWorkQueueByName(new_wq.name);
 │       └──> <wq == nullptr>
 │           └──> PX4_ERR("failed to create %s", new_wq.name);
 └──> return wq;

3. 总结

工作队列，其实并不负责。而PX4的工作队列为什么看起来复杂，主要是工作队列和实际的业务耦合。这里我们还没有将uORB的订阅内容放到里面，如果结合这部分，再加上多个继承业务的相互切换等内容，就看似更加复杂了。

所以我们尤其强调设计需要松耦合，尽量模块化，明确接口设计，明确框架设计。

4. 参考资料

【1】PX4开源软件框架简明简介
【2】Nuttx WorkQueue