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);

• 功能：工作佇列正式執行