一天精通无人机第 28 讲 中级篇系列:命令响应与状态切换(上)

file

在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::run()
{
    ...
        //定义线程属性
    pthread_attr_t commander_low_prio_attr;
    pthread_attr_init(&commander_low_prio_attr);
    pthread_attr_setstacksize(&commander_low_prio_attr, 
                              PX4_STACK_ADJUSTED(3000));
        //定义线程属性参数
    struct sched_param param;
    pthread_attr_getschedparam(&commander_low_prio_attr, 
                               &param);
        //设置优先级
    param.sched_priority = SCHED_PRIORITY_DEFAULT - 50;
    pthread_attr_setschedparam(&commander_low_prio_attr, 
                               &param);
        //创建线程
    pthread_create(&commander_low_prio_thread, 
                   &commander_low_prio_attr, 
                   commander_low_prio_loop, nullptr);
    pthread_attr_destroy(&commander_low_prio_attr);
    ...
}

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

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

nsh> commander status
WARN  [commander] type: symmetric motion
WARN  [commander] safety: USB enabled: [NO], power state valid:  [OK]
WARN  [commander] home: lat = 0.0000000, lon = 0.0000000, alt = 0.00, yaw: 0.00
WARN  [commander] home: x = 0.0000000, y = 0.0000000, z = 0.00 
WARN  [commander] datalink: LOST  
WARN  [commander] main state: 0
WARN  [commander] nav state: 0
WARN  [commander] arming: STANDBY

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

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

commander calibrate gyro

commander calibrate mag

commander calibrate level

commander calibrate airspeed

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

nsh> commander calibrate gyro
INFO  [commander] [cal] calibration started: 2 gyro
INFO  [commander] [cal] progress <5>
INFO  [commander] [cal] progress <10>
INFO  [commander] [cal] progress <15>
...
INFO  [commander] [cal] progress <90>
INFO  [commander] [cal] progress <95>
INFO  [commander] [cal] progress <100>
INFO  [commander] [cal] calibration done: gyro
  1. 行前检查:check选项可以针对飞控当前的状态来进行行前检查,并显示当前飞控的状态是否达到飞行的条件,例如:
nsh> commander check
WARN  [commander_tests] PREFLIGHT FAIL: MAG #0 UNCALIBRATED
INFO  [commander] Preflight check: FAILED

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

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

commander disarm:飞控锁定

commander takeoff:起飞

commander land:着陆

commander transition:模态切换(倾转旋翼使用)

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

nsh> commander arm
WARN  [commander_tests] PREFLIGHT FAIL: MAG #0 UNCALIBRATED
WARN  [commander] arming failed
  1. 主状态模式:mode选项中有13个选项,这13个选项就是飞控的13个飞行模式,如下:
manual:commander_state_s::MAIN_STATE_MANUAL:手动模式
altctl:commander_state_s::MAIN_STATE_ALTCTL:定高模式
posctl:commander_state_s::MAIN_STATE_POSCTL:位置模式
auto:mission:commander_state_s::MAIN_STATE_AUTO_MISSION:任务模式
auto:loiter:commander_state_s::MAIN_STATE_AUTO_LOITER:悬停模式
auto:rtl:commander_state_s::MAIN_STATE_AUTO_RTL:返航模式
acro:commander_state_s::MAIN_STATE_ACRO:特级模式
offboard:commander_state_s::MAIN_STATE_OFFBOARD:离线模式
stabilized:commander_state_s::MAIN_STATE_STAB:增稳模式
rattitude:commander_state_s::MAIN_STATE_RATTITUDE:姿态特级模式
auto:takeoff:commander_state_s::MAIN_STATE_AUTO_TAKEOFF:自动起飞
auto:land:commander_state_s::MAIN_STATE_AUTO_LAND:自动降落
flowtarget:commander_state_s::MAIN_STATE_FLOW_TARGET:跟随模式

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

file

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

transition_result_t arming_state_transition(
        vehicle_status_s *status,                 //飞行器状态
        const battery_status_s &battery,          //电池状态
        const safety_s &safety,                   //安全开关
        const arming_state_t new_arming_state,    //新解锁、锁定状态
        actuator_armed_s *armed,                  //执行解锁、锁定状态
        const bool fRunPreArmChecks,              //是否运行解锁检查
        orb_advert_t *mavlink_log_pub,            //mavlink描述符
        vehicle_status_flags_s *status_flags,     //飞行器条件状态开关
        const uint8_t arm_requirements,           //额外需求
        const hrt_abstime &time_since_boot)       //启动时间

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

transition_result_t main_state_transition(
                const vehicle_status_s &status,              //飞行器状态
                const main_state_t new_main_state,           //新主状态
                const vehicle_status_flags_s &status_flags,  //飞行器条件状态开关
                commander_state_s *internal_state)           //内部命令状态

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

bool set_nav_state(
        vehicle_status_s *status,                    //飞行器主状态
        actuator_armed_s *armed,                     //解锁、锁定状态
        commander_state_s *internal_state,           //内部命令状态
        orb_advert_t *mavlink_log_pub,               //maillink描述符
        const link_loss_actions_t data_link_loss_act,//数传信号丢失动作
        const bool mission_finished,                 //任务是否完成
        const bool stay_in_failsafe,                 //是否保持保护状态
        const vehicle_status_flags_s &status_flags,  //飞行器条件状态开关
        bool landed,                                 //是否着陆
        const link_loss_actions_t rc_loss_act,       //遥控器信号丢失动作
        const int offb_loss_act,                     //离线模式下数传信号丢失动作
        const int offb_loss_rc_act,                  //离线模式下遥控器信号丢失动作
        const int posctl_nav_loss_act)               //位置控制失效动作

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

下期预告:《中级篇:命令响应与状态切换(下)》

file