多旋翼无人机
在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,
¶m);
//设置优先级
param.sched_priority = SCHED_PRIORITY_DEFAULT - 50;
pthread_attr_setschedparam(&commander_low_prio_attr,
¶m);
//创建线程
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号