px4 commander阅读笔记

这个也是很久以前的笔记。
         commander模块，在px4中，这个模块是管理中心。commander模块的主函数是commander_thread_main函数，这个函数在进入循环以前建立一个处理低优先级的线程commander_low_prio_loop。这个线程的循环在文件commander.cpp中大概3830行左右。

    这个循环是循环检索vechicle_command的urb,看是否有有效的comander，有就处理，如果是高优先级的直接跳过。
commander_thread_main函数的2839-2850行，是检测vehicle_command是否有更新，有就进行handle_command函数处理。在这个函数里主要根据cmd->command执行不同的switch-case语句。然后根据不同的cmd参数，执行main_state_transition()函数切换不同的模式，实现代码主要在state_machine_helper.cpp文件中444-447行，设置internal_state->main_state为新的main state。


在commander_thread_main函数的循环中，执行了handle_command函数,这个函数检查当前状态是否满足欲修改成的新状态。同样的文件中大概2929--2939行，执行set_nav_state函数。在set_nav_state函数中，检查是否有其它意外情况，没有的话根据inter_state的main_state设置vehicle_status_s的nav_state.下面继续在同一文件的2961-2967行，也是重点，这部分代码主要是把上面的状态修改告诉其它模块。


在commander.cpp中，3830行的commander_low_prio_loop中，也检测vehicle_command的uorb。当这个低优先级线程接收到vehicle_command时，处理一些优先级偏低的命令。而这个命令的发出，即uorb的publish在哪呢？mavlink_receive.cpp中，具体哪个模块发出来的，下回分解。


接上次，从2961行开始，先set_control_mode，这个设置是根据具体的nav_state来的，所以修改了nav_state，需要重新设置control mode，并且根据新模式设置需要哪些控制量，这些控制量在mc_pos_control_main.cpp和mc_att_control_main.cpp使用。
然后publish这个设置好的control mode。然后把修改以后的vehicle_status_s也publish出去。


### 2017-5-19 阅读px4代码之commander.cpp和navigator_main.cpp文件
PC机上的QGC通过mavlink向飞控发送命令，比如takeoff。流程是这样的。
commander.cpp文件中的commmander_thread_main函数中的循环通过下面几行代码检测pc发送过来的命令：


orb_check(cmd_sub, &updated);
        if(updated)
        obr_copy(ORB_ID(vehicle_command), cmd_sub, &smd);

同时，在navigator_main.cpp文件中也通过orb_check检查这个文件描述符。

在commander.cpp文件中，检测到有这个uorb ID上有更新，就执行2853行的handle_command函数。如果我们在QGC先执行takeoff命令，QGC会先发送loiter命令：PX4_CUSTOM_SUB_MODE_AUTO_LOITER。然后通过函数main_state_transition设置internal_state的main_state为commander_state_s::MAIN_STATE_AUTO_LOITER.最后退出handle_command函数


继续下面执行set_anv_state函数。这个函数根据internal_state的main_state参数设置vehicle_status_s的nav_state,对应的loiter参数设置为vehicle_status_s::NAVIGATOR_STATE_AUTO_LOITER。然后退出set_nav_state函数。紧接着下面publish出去。
    
    在navigator_main循环里，会一直通过uorb检测vehicle_status_s的状态是否更新，来得到commander模块修改后的状态。根据日志跟踪，在我们执行takeoff命令时，QGC会先发送一个loiter命令，紧接着发送一个takeoff命令。最后，在navigator_main函数里设置执行具体的_takeoff工作模式。但是地面站直接发送过来给navigator模块的命令就几个，很少很少。绝大多数具体的让飞机飞到哪的命令都是commander模块发出来的。毕竟他是大脑嘛，前面说commander模块根据收到的地面站发来的飞行模式命令作出一系列的状态修改，最后publish出来，navigator等待的就是这个。收到后，设置具体飞行模式。navigator一共大概有10个模式。navigator结构体有一个navigator_mode_array数组，所有的模式都放在这个数组里。还有一个navigator_mode的指针，这个指针就是当前的模式，它指向navigator_mode_array数组里的某一元素。在主函数靠下的地方，这个数组的每一个元素都会运行run函数，这个run函数会会根据参数执行每一个具体模式的run函数，这里用到的父类的run函数，实际里面执行的是子类的三个函数：on_inactive, on_activation和on_active。这三个函数就是具体模式准备--启动--工作的三个阶段。如果navigator_mode指向哪个具体模式，那么对应数组的run函数参数就是true，其它都是false，这样只有navigator_mode指向的模式启动正常true流程，其它都是false流程。    运行navigator_mode指向的具体模式的run函数。比如，我们当前指向mission模式。这样就可以循环执行mission任务了。说的比较简介，有不正确的地方欢迎大家一起交流。