PX4源码工作队列运行流程

记录PX4源码学习的过程，一起加油啊～

本文参考：https://blog.csdn.net/qq_42985705/article/details/131218211

模块启动

1. 飞控模块启动脚本

• 路径：PX4-Autopilot\ROMFS\px4fmu_common\init.d\rcS
• 代码（392行）

. ${R}etc/init.d/rc.vehicle_setup

2. 查找rc.vehicle_setup

• 路径：PX4-Autopilot\ROMFS\px4fmu_common\init.d\rc.vehicle_setup
• 代码（23行）

. ${R}etc/init.d/rc.mc_apps

3. 查找rc.mc_apps

• 路径：PX4-Autopilot\ROMFS\px4fmu_common\init.d\rc.mc_apps
• 代码

mc_att_control start
mc_autotune_attitude_control start
mc_pos_control start

• 功能：这些启动模块的命令，则会去执行对应模块的入口函数，完成启动。

模块初始化

1. 姿态控制主函数main

• 路径：PX4-Autopilot\src\modules\mc_acc_control_main.cpp
• 代码

extern "C" __EXPORT int mc_att_control_main(int argc, char *argv[])
{
	return MulticopterAttitudeControl::main(argc, argv);
}

其中，调用基类Moudles的main函数，功能模块从基类Moudles继承。
2. 查看基类main函数定义

if (strcmp(argv[1], "start") == 0) {
			// Pass the 'start' argument too, because later on px4_getopt() will ignore the first argument.
			return start_command_base(argc - 1, argv + 1);
		}

3. 查看start_command_base

ret = T::task_spawn(argc, argv);
return ret;

4. 查看对应的MulticopterAttitudeControl::task_spawn

• 路径：PX4-Autopilot\src\modules\mc_acc_control_main.cpp
• 代码

//实例化初始对象
MulticopterAttitudeControl *instance = new MulticopterAttitudeControl(vtol);
//判断是否初始化成功
if (instance)
//存储实例出来的指针
_object.store(instance);
//赋值
_task_id = task_id_is_work_queue;
//初始化实例的指针
instance->init()

5. 查看init()

• 代码：

bool
MulticopterAttitudeControl::init()
{
	if (!_vehicle_attitude_sub.registerCallback()) {
		PX4_ERR("callback registration failed");
		return false;
	}
	return true;
}

• 功能：此处注册了回调函数

MC_ATT_CONTROL模块加入工作队列

实例化对象的时候，将这个工作项插入了工作队列当中，在工作队列中去执行。因此，此处回头开始看实例化的代码

1. 查看实例化代码

• 路径：PX4-Autopilot\src\modules\mc_acc_control_main.cpp
• 代码：

MulticopterAttitudeControl::MulticopterAttitudeControl(bool vtol) :
	ModuleParams(nullptr),
	WorkItem(MODULE_NAME, px4::wq_configurations::nav_and_controllers)

2. 查看向WorkItem传递的第二个参数nav_and_controllers

• 代码：

static constexpr wq_config_t nav_and_controllers{"wq:nav_and_controllers", 2240, -13};

• 功能：这是一个包含需要信息的结构体

3. 查看构造函数WorkItem
   代码：

	if (!Init(config))

4. 查看初始化函数Init

bool WorkItem::Init(const wq_config_t &config){
px4::WorkQueue *wq = WorkQueueFindOrCreate(config);
}

5. 查看WorkQueueFindOrCreate

• 代码：

WorkQueue *
WorkQueueFindOrCreate(const wq_config_t &new_wq){
WorkQueue *wq = FindWorkQueueByName(new_wq.name);
}

• 功能：查看该工作队列是否存在

6. 查看FindWorkQueueByName

• 代码：

static WorkQueue *
FindWorkQueueByName(const char *name)
{
	for (WorkQueue *wq : *_wq_manager_wqs_list) {
		if (strcmp(wq->get_name(), name) == 0) {
			return wq;
		}
	}
}

• 功能：在_wq_manager_wqs_list列表中查看那是否有当前工作列

7. 回到第5步，继续看WorkQueueFindOrCreate函数

• 代码：

if (wq == nullptr) {
		// add WQ config to list
		//  main thread wakes up, creates the thread
		_wq_manager_create_queue->push(&new_wq);

		// we wait until new wq is created, then return
		uint64_t t = 0;

		while (wq == nullptr && t < 10_s) {
			// Wait up to 10 seconds, checking every 1 ms
			t += 1_ms;
			px4_usleep(1_ms);

			wq = FindWorkQueueByName(new_wq.name);
		}
}

	return wq;

• 功能：如果FindWorkQueueByName返回空指针，则往_wq_manager_create_queue中加入该队列，并返回指向该工作列的指针

8. 回到第4步的init初始化函数

• 代码

if ((wq != nullptr) && wq->Attach(this)) {
		_wq = wq;
		_time_first_run = 0;
		return true;
	}

• 功能：加入工作队列成功

Nuttx系统中的工作队列运行

1. Nuttx系统运行入口路径

• 路径：/platforms/nuttx/NuttX/nuttx/sched/init
• 相互间调用关系解析：该目录中nx_start.c -> nx_bringup.c -> nx_create_initthread （位于nx_bringup函数中）-> nx_start_application （位于nx_create_initthread函数中） -> CONFIG_INIT_ENTRYPOINT（位于nx_start_application函数中）

2. 查看CONFIG_INIT_ENTRYPOINT

• 代码：

px4_userspace_init();

3. 查看px4_userspace_init()

• 代码：

hrt_init();
px4_set_spi_buses_from_hw_version();
px4::WorkQueueManagerStart();
uorb_start();

• 功能：此处启动工作队列管理开始函数

4. 查看WorkQueueManagerStart函数

• 代码：

int task_id = px4_task_spawn_cmd("wq:manager",
				SCHED_DEFAULT,
				SCHED_PRIORITY_MAX,
				PX4_STACK_ADJUSTED(1280),
				(px4_main_t)&WorkQueueManagerRun,
				nullptr);

• 功能：创建了"wq：manager"这个线程，线程的入口函数为WorkQueueManagerRun函数

5. 查看WorkQueueManagerRun函数

• 代码：

//此处创建两个工作队列的变量
_wq_manager_wqs_list = new BlockingList<WorkQueue *>();
_wq_manager_create_queue = new BlockingQueue<const wq_config_t *, 1>();

• 解析：此处两个变量与三、6和三、7中的两个变量对应

6. 配置工作变量的相关信息

• 代码：

while (!_wq_manager_should_exit.load()) {
		// create new work queues as needed
		const wq_config_t *wq = _wq_manager_create_queue->pop();

• 解析：随后就是工作队列管理器进行调度的代码了，从列表里取工作队列的配置信息，然后创建线程，设置线程的调度策略和优先级。

7. 以Nuttx系统创建线程为例

• 代码：

int pid = px4_task_spawn_cmd(wq->name,
		SCHED_FIFO,
		sched_priority,
		stacksize,
		WorkQueueRunner,
		(char *const *)arg);

• 功能：调用px4_task_spawn_cmd函数（Nuttx系统下创建线程都是这个函数）

8. 工作队列开始运行

• 代码：

	// add to work queue list
	_wq_manager_wqs_list->add(&wq);
	wq.Run();
	// remove from work queue list
	_wq_manager_wqs_list->remove(&wq);

• 功能：工作队列正式运行