命令响应与状态切换（上） 来自知乎沈阳无距科技

15519743871

无人机中级篇：第十二讲：命令响应与状态切换（上）
沈阳无距科技

工业级无人机的中国名片

在PX4中负责状态切换的模块为commander，在src/modules/commander文件夹下有很多文件，他们分别负责不同的功能，例如：主状态和导航状态切换、传感器校准、命令响应等等。在本节中我们主要来了解commander.cpp中命令响应与状态切换的相关内容。

在int commander_main(int argc, char *argv[])函数中定义了所有可执行的命令选项，我们可以根据其useage来对这个功能做一些简单的了解：

usage: commander {start|stop|status|calibrate|check|arm|disarm|takeoff|land|transition|mode}

1.启动/停止进程：start/stop/status这3个命令主要功能是在nsh脚本里将commander线程启动/停止/查看状态。当执行commander start后，系统掉中Commnader::run()函数，并为此函数创建一个叫做commander的进程，同时Commnader::run()函数中创建了一个叫做commander_low_prio的低优先级线程，创建低优先级线程的过程如下。

在执行commander stop会终止commander进程和commander_low_prio线程。

执行commander status时，显示当前飞控的相关内容，例如：

其中显示飞控的运动类型（type）、安全开关状态（safety）、电源状态（power state valid）、起飞点坐标（home position）全局坐标系和本地坐标系、数传状态（datalink）、主状态（main state）、导航状态（nav state）、锁定状态（arming）等等。

2.传感器校准：calibrate选项可以对传感器进行校准，校准选项有4个，分别为accel、gyro、mag、level、airspeed。可以分别执行下面命令来对加速计、陀螺仪、磁罗盘、水平仪和空速计做校准，在校准方式与使用地面站对传感器校准的过程是一致的：

commander calibrate accel

commander calibrate gyro

commander calibrate mag

commander calibrate level

commander calibrate airspeed

例如：对陀螺仪校准的过程如下：

3.行前检查：check选项可以针对飞控当前的状态来进行行前检查，并显示当前飞控的状态是否达到飞行的条件，例如：

此次航前检查失败，原因是磁罗盘为校准。

4.锁定/解锁与起降命令：arm/disarm/takeoff/land/transition这5个选项可以让飞控通过commander命令来使飞控直接完成相应功能：

commander arm：飞控解锁

commander disarm：飞控锁定

commander takeoff：起飞

commander land：着陆

commander transition：模态切换（倾转旋翼使用）

当然，这些功能在执行之前需要有前提条件，在前提条件达到之后才能实现状态的切换。例如在航前检查未通过时是禁止直接用commander arm对飞控解锁的。

5.主状态模式：mode选项中有13个选项，这13个选项就是飞控的13个飞行模式，如下：

manual：MAIN_STATE_MANUAL：手动模式

altctl：MAIN_STATE_ALTCTL：定高模式

posctl：MAIN_STATE_POSCTL：位置模式

auto:mission：MAIN_STATE_AUTO_MISSION：任务模式

auto:loiter：MAIN_STATE_AUTO_LOITER：悬停模式

auto:rtl：MAIN_STATE_AUTO_RTL：返航模式

acro：MAIN_STATE_ACRO：特级模式

offboard：:MAIN_STATE_OFFBOARD：离线模式

stabilized：MAIN_STATE_STAB：增稳模式

rattitude：MAIN_STATE_RATTITUDE：姿态特级模式

auto:takeoff：MAIN_STATE_AUTO_TAKEOFF：自动起飞

auto:land：MAIN_STATE_AUTO_LAND：自动降落

flowtarget：MAIN_STATE_FLOW_TARGET：跟随模式

Commander::run()函数中定义了一个while循环结构，这个循环结构在commander启动后就一直在运行，知道commander停止后才结束。在这个循环中所完成的处理工作相对比较多，我们可以看一下整个过程：

事实上在commnader的主循环中每一个处理功能的逻辑都比较清晰，其主要功能就是根据当前状态来为飞控做状态切换。我们主要开看一下几个函数的定义：

arming_state_transition()函数会根据当前飞行器的各项状态来检查是否允许解锁或锁定，例如在安全开关没有按下，或是电池电压不足时禁止解锁。

main_state_transition()函数负责根据当前飞行器的各项状态检查和需要切换到的新状态new_main_state来进行状态切换。

set_nav_state()函数负责根据当前飞行器的各项状态检查并重新设定status中的nav_state。