多旋翼无人机

    返回首页    发表留言
本文作者:李德强
          第一节 概述
 
 

        在PX4中负责状态切换的模块为commander,在Firmware/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)等等。

        2.传感器校准: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

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

nsh> commander check
WARN  [commander_tests] PREFLIGHT FAIL: MAG #0 UNCALIBRATED
INFO  [commander] Preflight check: FAILED

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

        4.锁定/解锁与起降命令: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

        5.主状态模式: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停止后才结束。在这个循环中所完成的处理工作相对比较多,我们可以看一下整个过程:

        事实上在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。

    返回首页    返回顶部
  看不清?点击刷新

 

  Copyright © 2015-2023 问渠网 辽ICP备15013245号