PX4 飞控垂直起降源码分析
PX4 飞控垂直起降源码分析PEAK常青藤
本分析基于1.5.5版本代码
1. 垂直起降主程序
垂直起降部分飞控程序的位置
/Firmware/src/modules/vtol_att_control
文件构成
进入Cmake文件，可以看到vtol_att_control_main.cpp文件中存储了垂直起降程序的主文件。在文件夹中还有3个文件tiltrotor.cpp、tailsitter.cpp、standard.cpp分别对应着三种垂直起降飞行器类型。
Tiltrotor指倾转旋翼，这里主要表示的是倾转螺旋桨，不是传统意义上的倾转旋翼。Tailsitter是尾座式垂直起降。Standard指四旋翼+常规布局的复合式。
############################################################################
px4_add_module(
MODULEmodules__vtol_att_control
MAINvtol_att_control
STACK_MAIN 1300
COMPILE_FLAGS
SRCS
vtol_att_control_main.cpp
tiltrotor.cpp
vtol_type.cpp
tailsitter.cpp
standard.cpp
DEPENDS
platforms__common
)
# vim: set noet ft=cmake fenc=utf-8 ff=unix :
进入主文件vtol_att_control_main.cpp
第一部分是VtolAttitudeControl类的构造函数和析构函数的初始化，类的定义在vtol_att_control_main.h文件中。
进入主程序void VtolAttitudeControl::task_main()
第1句fflush(stdout)强制读写缓冲区。
然后部分是订阅要用到的uORB主题，其中包括虚拟的多旋翼和固定翼姿态设置点，以及虚拟的多旋翼和固定翼控制器输出。
mc_virtual_attitude_setpoint
fw_virtual_attitude_setpoint
actuator_controls_virtual_mc
actuator_controls_virtual_fw
另外不知为何这里的vehicle_attitude_setpoint主题订阅了2次。
再往下是唤醒三个源，这三个主题的句柄相对重要一些。
//旋翼模式控制器给伺服器的输入，重要输入
fds.fd = _actuator_inputs_mc;
fds.events =POLLIN;
//固定翼模式控制器给伺服器的输入，重要输入
fds.fd = _actuator_inputs_fw;
fds.events =POLLIN;
fds.fd = _params_sub;//参数更新句柄，重要数据
fds.events =POLLIN;
到这里就进入了本程序的主循环，此处也是用while控制的，有两种退出循环的情况，第1个是进入了析构函数~VtolAttitudeControl()，第2个是在无法new一个存在的垂直起降飞行器类型时也会退出。
每个循环的第一部分是推送vtol_vehicle_status主题。
下一部分是轮询fds主题，就是_actuator_inputs_mc主题的变化，每次轮询时会等待100毫秒。如果没有得到期望的数据，就continue整个循环，不会继续执行循环后续的内容。
再下一部分是监测fds主题，就是_params_sub主题，如果发现了参数的变化就会上传参数到程序里面。这段程序使程序运行时可以更改参数。
之后一批poll函数用于集中更新各个主题的变化。
在后面的主程序部分，_vtol_type作为基类，用来代表选定的不同的飞行器类型，这里的用法与navigator程序中选择不同的模式的用法类似。
_vtol_type->update_vtol_state();
这里的具体作用是在对应的垂直起降飞机构型中更新垂直起降状态机，更新的结果可以在虚函数_vtol_type->get_mode()中调用。
之后的第一个if (_v_control_mode.flag_control_manual_enabled) {
会检测到人工模式，并将自动模式的VTOL相关状态复位到可以人工操纵的状态。
复位规则是：
当前状态是旋翼模式，切换到VTOL的旋翼模式
当前状态是固定翼模式，切换到VTOL的固定翼模式
当前状态是“转换到旋翼模式”，立刻切换到旋翼模式。当重新从手动模式切到自动模式时，这里阻止了立刻切换到固定翼状态，将会继续保持在旋翼模式。
下面的一连串if和elseif用于检测状态标志位是什么状态，并且启用相应状态的特殊函数。
仔细分析第一个判断if
if (_vtol_type->get_mode() == ROTARY_WING) {
if (fds.revents & POLLIN) {
当fds对应的多旋翼模式的输入更新时，此时会及时更新数据，获取多旋翼姿态控制器输出的给伺服器的命令。
然后的_vtol_type->update_mc_state();
进入相应的_vtol_type里，更新多旋翼模式的状态。目前的三种VTOL飞行器构型里面，分别有不同的多旋翼模式的更新程序，具体后面分析。
最后的一个函数是fill_mc_att_rates_sp()，这里将_mc_virtual_v_rates_sp结构体分别赋值给_v_rates_sp结构体。_v_rates_sp结构体在下面的程序中会被推送到uORB中
分析第二个判断else if
} else if (_vtol_type->get_mode() == FIXED_WING) {
在当前的VTOL模式是固定翼模式时，会执行以下操作。
首先重置两个标志位，明确告诉其他程序现在在固定翼模式。
然后在fds源有更新时，即_actuator_inputs_fw句柄对应的主题有变化时，首先获取固定翼姿态控制器输出的给伺服器的命令，之后获取人工操纵的设置点输入。
之后的_vtol_type->update_fw_state();
进入对应的飞行器模型中更新固定翼模式的姿态。3种构型程序里面的对应操作都比较简单，后面会详述。
最后一句同上，将固定翼虚拟角速率结构体_fw_virtual_v_rates_sp对应赋值给_v_rates_sp结构体。
分析第三个判断else if
else if (_vtol_type->get_mode() == TRANSITION_TO_MC ||_vtol_type->get_mode() == TRANSITION_TO_FW) {
当前模式是转换模式时，不管是转换到多旋翼模式还是转换到固定翼模式，执行以下程序。
第一部分的标志位赋值需要注意，这里确认了转换模式和多旋翼模式，之后要根据情况确认转换到固定翼的模式。
确认多旋翼模式的目的是，出于安全，需要在过渡模式时开启多旋翼姿态控制器。
之后的部分要分别检测fds和fds数据源是否有新数据，注意这两个源用到的是旋翼姿态控制器和固定翼姿态控制器对伺服器的输出。
当检测到以上两个源有新的数据时，进入相应飞行器构型的转换模式的更新操作，不同构型里面的程序都比较复杂，因为转换模式是重点执行的程序，后续会详细介绍。
下面的程序是填充角速率设置点，注意这里使用的是多旋翼模式的姿态设置点，为什么要使用多旋翼模式的值还不清楚。
最后是推送姿态设置点。
第四个判断比较特殊，这里不是判断飞行器在那个模式，而是判断飞行器在外部模式，此模式的姿态/油门数据来源是外部模块产生的。
进入此状态时也会进入对应构型飞行器程序里面进行相应处理。
结束模式判断程序后，要进行姿态设置点的推送。
publish_att_sp();
以及执行不同飞行器构型中的填充姿态控制器输出的程序，在这个程序里面会根据所处的模式选择对应的输出，分别是_actuators_out_0代表多旋翼模式伺服器的输出，_actuators_out_1固定翼模式伺服器的输出。
下一部分，当飞行器启用了人工控制模式时，仅仅推送各模式的输出。具体什么意思暂时不清楚。
最后一部分是推送角度率设置点。
至此，垂直起降程序的主模式结束。
要注意的是，在整个垂直起降程序中，有两种判断垂直起降状态的标志结构体。
在垂直起降主程序vtol_att_control_main.cpp中，使用_vtol_type->get_mode()函数返回_vtol_mode判断垂直起降状态。
在对应的垂直起降构型的函数中，使用flight_mode存储和判断垂直起降状态，最终在相应构型的update_vtol_state()程序中，将flight_mode模式更新到_vtol_mode中。
以上介绍完了垂直起降程序的主函数部分，从中可见这部分代码主要进行了模式判断和切换，以及链接到不同飞行器构型的具体程序，并没有包括具体内容。
垂直起降程序的具体内容分别包括在相应垂直起降飞行器构型的程序中，就是主程序中链接入口的位置。
有4个入口比较重要，分别是
_vtol_type->update_vtol_state();
_vtol_type->update_mc_state();
_vtol_type->update_fw_state();
_vtol_type->update_transition_state();
一般来说，更新多旋翼模式和固定翼模式的状态都比较简单，因为此时基本可以看做是将垂直起降飞行器按照多旋翼或固定翼的方式飞行。最复杂的模式时转换模式。
下面我们会从update_vtol_state()开始分析。不同飞行器构型的转换模式处理显然是不一样的，这里先从standard构型开始。
update_vtol_state()
此程序主要进行转换标志位和权重的处理，还没有直接进行转换时控制数值的处理。
2. Standard构型
首先大致了解一下standard.cpp中的主要函数
void Standard::update_vtol_state()函数中的内容主要有3部分，第一部分和第二部分分别是if和else if，第三部分是switch
首先要进行一个函数判断is_fixed_wing_requested()是否要进入固定翼模式。如果不是，就进入第一部分处理转换到多旋翼模式的情况；如果是，就进入第二部分处理转换到固定翼模式的情况。
关于此函数VtolAttitudeControl::is_fixed_wing_requested()，函数有两部分内容。首先判断是否要进入固定翼模式，然后要处理前飞转换终止的情况。
两种情况可以进入固定翼模式，首先是如果遥控器开关启用时，只要有转换信号就表示要进入固定翼模式；然后如果没有启用遥控器开关，这时就要根据命令来切换。
在处理转换终止情况时，无条件进入多旋翼模式。
根据以上分析，当不要进入固定翼模式时执行第一部分。
第一部分
当if (!_attc->is_fixed_wing_requested()) {生效时，表示飞行器要进入的状态不是固定翼模式，所以有几种可能：
1.如果当前模式是MC_MODE说明当前是多旋翼模式，并且还要继续保持在多旋翼模式。
这时要把多旋翼模式输出的权重全部设置为1。
2.如果当前模式是固定翼模式FW_MODE，说明此时的状态是要从固定翼模式转换到多旋翼模式
又可以分为两种情况，首先是判断有无转换失效保护的情况。在转换失败时会出现转换失效保护，此时的处理办法是先设置内部模式是MC_MODE，然后启用多旋翼电机。
如果没有失效保护，就是转换正常的情况，此时会将内部模式标记为TRANSITION_TO_MC，以及，启用多旋翼电机。
3.如果当前模式是“转换到固定翼”TRANSITION_TO_FW
说明是在转换到固定翼的过程中，被切换到了多旋翼模式，有可能是转换失败或终止之后应急切换到多旋翼模式。此时将多旋翼模式的控制权限全部设置为1。
4.如果当前模式是“转换到多旋翼” TRANSITION_TO_MC
进入这个状态后，会判断转换开始后经历的时间，如果超过阈值就将内部模式设定为MC_MODE。
在第一部分的结束，将前飞电机的油门设定为0。这是为了确保在转换到多旋翼模式的过程中没有前飞动力。
第二部分
是第一部分内容不启用的状态。
进入这个状态说明此时命令是向多旋翼模式转换，可分为3种情况。
1.当前模式是MC_MODE或者“转换到多旋翼模式” TRANSITION_TO_MC
此时程序的任务时转换到固定翼模式。注意，这里没有紧急切换到固定翼模式的处理，因为这样会导致不安全的飞行状态，所以程序里面所有的紧急切换都是切到多旋翼模式。
2.如果当前内置模式是固定翼模式FW_MODE
此时把多旋翼模式权重置为零。
3.如果当前内置模式是正在转换到固定翼模式TRANSITION_TO_FW
说明转换进行中。此时要做的是通过一系列判断确定是否转换完成。
有如下判断条件
在启用空速模式时，如果前飞空速大于转换空速设置值，就认为转换完成；
在没启用空速模式时，依靠时间判断，如果转换时间大于设置的转换最小时间，也认为转换完成。
以及第三种条件，当飞行器没有解锁，或者飞行器已经降落时，也可以认为转换完成。也就是支持地面上转换的意思。
转换完成后，将内置状态标记为FW_MODE，禁用多旋翼模式的电机。
第三部分
现在进入第三部分内容，switch函数。
在此部分，主要根据_vtol_schedule.flight_mode赋值_vtol_mode。
_vtol_mode与flight_mode看起来是同一个意思，但是可能有不同的功能。目前还不清楚具体作用。
现在开始分析update_transition_state()
查看此函数的调用关系发现，在_vtol_type转换模式是TRANSITION_TO_MC和TRANSITION_TO_FW时，并且获得_actuators_mc_in和_actuators_fw_in的新数据时，才会启用这个程序。
在standard构型下，首先要进入VtolType::update_transition_state()位置检测飞行器是否高度过低。当高度确实过低时终止转换过程，赋值_abort_front_transition用来在程序的其他位置终止转换过程。
然后要判断内部模式flight_mode是TRANSITION_TO_FW还是TRANSITION_TO_MC。
在TRANSITION_TO_FW时，执行向固定翼模式的转换。其中包括几部分内容。
1.根据转换时间控制油门变化
2.根据空速或转换时间控制固定翼模式和多旋翼模式的混合控制权重
如果空速有效并且满足其他条件时，用空速控制混合权重；
如果空速不启用时，使用转换时间控制混合权重。
最后一种情况是空速非常低时，最大化多旋翼模式的控制权限。
3.检查前飞超时，在出现超时情况时终止转换。
在TRANSITION_TO_MC时，执行向多旋翼模式的转换。其中包括几部分内容。
1.如果定义了向后转换时间，就用这个时间控制控制权重。
注意，向后转换时不依靠空速判断权重。
2.没有定义向后转换时间时直接将多旋翼模式转速开到最大。
3.此部分函数待分析
关于Standard::update_transition_state()程序
} else if(_vtol_schedule.flight_mode == TRANSITION_TO_MC) {
以下这段程序用于在程序的最后部分设置多旋翼电机的最大最小PWM值。
if (_flag_enable_mc_motors) {
set_max_mc(2000);
set_idle_mc();
_flag_enable_mc_motors =false;
}
Standard::set_max_mc(2000);
这个函数的功能：
本函数使用形参的值设定多旋翼电机的最大PWM值。
set_max_mc(2000);设定值为2000说明在此时要启用多旋翼电机。
程序的工作过程是：
定义端口组的位置
打开端口组
使用px4_ioctl获取端口组内的端口数目
使用形参的数据分别赋值到端口结构体对应端口
使用px4_ioctl分别设置端口的最大PWM值
完成后使用px4_close(fd);关闭文件句柄
之前有一个地方是set_max_mc(950);意思是限制电机PWM的最大值是950，不启用电机。
关于set_idle_mc();程序，本程序的功能是利用参数中设置的值，设置多旋翼电机的怠速。
程序工作过程是，
定义PWM输出端口组；
打开端口组
使用px4_ioctl获取端口组内的端口数目
获取参数中设置的最小PWM值
将最小PWM值分别赋值到端口组结构体的对应端口
使用px4_ioctl分别设置端口的最小PWM值
完成后使用px4_close(fd);关闭文件句柄
这部分程序
_mc_roll_weight =math::constrain(_mc_roll_weight, 0.0f, 1.0f);
_mc_pitch_weight =math::constrain(_mc_pitch_weight, 0.0f, 1.0f);
_mc_yaw_weight =math::constrain(_mc_yaw_weight, 0.0f, 1.0f);
_mc_throttle_weight =math::constrain(_mc_throttle_weight, 0.0f, 1.0f);
因为之前的权重是计算得到，所以这里需要进行限幅。
关于Standard::fill_actuator_outputs()程序
本程序在vtol_att_control_main.cpp主程序的后半段调用。
主要功能是根据之前计算出的mcattitude weighting进行多旋翼与固定翼输出值的合并。
多旋翼部分的输出值是多旋翼控制器的输出值乘以权重。
固定翼部分姿态控制器的输出值是固定翼控制器输出值乘以另外部分的权重，油门部分会根据转换状态决定。
关于vtol_att_control_main.cpp主程序的输入和输出
从垂直起降主程序中可以看出，本程序主要是利用其它位置的多旋翼姿态控制器和固定翼姿态控制器对于伺服器的控制输出，根据转换过程中的一系列状态参数，如空速、转换时间等参数，获得转换模式中多旋翼模式与固定翼模式的权重，最后根据权重合成两个模块的伺服器输出数据。
本程序的主要输入有以下几个
/* do subscriptions */
_v_att_sp_sub =orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
_mc_virtual_att_sp_sub =orb_subscribe(ORB_ID(mc_virtual_attitude_setpoint));
_fw_virtual_att_sp_sub =orb_subscribe(ORB_ID(fw_virtual_attitude_setpoint));
_mc_virtual_v_rates_sp_sub =orb_subscribe(ORB_ID(mc_virtual_rates_setpoint));
_fw_virtual_v_rates_sp_sub =orb_subscribe(ORB_ID(fw_virtual_rates_setpoint));
_v_att_sub =orb_subscribe(ORB_ID(vehicle_attitude));
_v_att_sp_sub =orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
_v_control_mode_sub =orb_subscribe(ORB_ID(vehicle_control_mode));
_params_sub =orb_subscribe(ORB_ID(parameter_update));
_manual_control_sp_sub =orb_subscribe(ORB_ID(manual_control_setpoint));
_armed_sub =orb_subscribe(ORB_ID(actuator_armed));
_local_pos_sub =orb_subscribe(ORB_ID(vehicle_local_position));
_airspeed_sub = orb_subscribe(ORB_ID(airspeed));
_battery_status_sub = orb_subscribe(ORB_ID(battery_status));
_vehicle_cmd_sub =orb_subscribe(ORB_ID(vehicle_command));
_tecs_status_sub =orb_subscribe(ORB_ID(tecs_status));
_land_detected_sub =orb_subscribe(ORB_ID(vehicle_land_detected));
_actuator_inputs_mc = orb_subscribe(ORB_ID(actuator_controls_virtual_mc));
_actuator_inputs_fw =orb_subscribe(ORB_ID(actuator_controls_virtual_fw));
第一类：姿态设置点主题、角度率设置点主题、输出给伺服器的控制输入
以下2个主题发布于mc_att_control_main.cpp文件，在文件中将姿态控制器的角速率输出和伺服器控制信号输出发布于uORB总线。最终在vtol_att_control_main.cpp垂直起降程序中调用。
ORB_ID(actuator_controls_virtual_mc)
ORB_ID(mc_virtual_rates_setpoint);
以下主题发布自mc_pos_control_main.cpp文件，来自于程序中位置控制程序计算的多旋翼姿态设置点。
ORB_ID(mc_virtual_attitude_setpoint)
以下2个主题发布于fw_att_control_main.cpp文件，包括角速率设置点输出、伺服器控制输出、和姿态设置点输出。
ORB_ID(actuator_controls_virtual_fw)
ORB_ID(fw_virtual_rates_setpoint)
以下主题发布于2个文件。
ORB_ID(fw_virtual_attitude_setpoint)
来自于fw_att_control_main.cpp文件中的主题，发布的是STABILIZED模式的人工输入
来自于fw_pos_control_l1_main.cpp文件的主题，发布的是程序中位置控制程序计算的固定翼姿态设置点。
下面这个设置点发布于navigator_main.cpp程序，仅发生于GPS失效保护这一种状况
ORB_ID(vehicle_attitude_setpoint)
以上的输入里面，输出给伺服器的控制输入是要直接参与运算的。
如ORB_ID(actuator_controls_virtual_mc)的数据赋值到_actuators_mc_in中，然后在三个VTOL构型的文件中，_actuators_mc_in经过与权重参数的处理，转换到_actuators_out_0中。
多旋翼的角速率设置点输入、固定翼的角度率设置点输入分别赋值给VTOL模式中的多旋翼飞行模式和平飞飞行模式。以及，在转换模式时也使用多旋翼模式的角度率设置点作为转换的角速率设置点。见下面的程序。
if (got_new_data) {
_vtol_type->update_transition_state();
fill_mc_att_rates_sp();
publish_att_sp();
}
第二类：飞行器实时姿态、飞行器实时状态
ekf2_main.cpp文件推送的飞行器实时姿态。
ORB_ID(vehicle_attitude)
commander.cpp文件推送的飞行器实时的控制模式
ORB_ID(vehicle_control_mode)
发布于param_shmem.c文件和param.c文件的参数更新主题
ORB_ID(parameter_update)
发布于sensors.cpp文件和mavlink_receiver.cpp文件的人工控制设置点主题
ORB_ID(manual_control_setpoint)
以及伺服器锁定状态、本地位置、空速、电池状态、飞行器命令、能量机动状态、飞行器降落状态等主题。
本程序的主要输出有以下几个？
先不管输出问题，继续看其他程序
在standard构型的程序中还有如下一些内容。
类构造函数
Standard::Standard(VtolAttitudeControl*attc)
功能是参数初始化和参数获取
参数更新函数
Standard::parameters_update()
多旋翼状态更新
Standard::update_mc_state()
本函数在VTOL主程序中飞行器处于多旋翼模式时被调用。是standard构型程序在多旋翼模式要做的操作。本程序看似内容比较多，其实并不多，主要有2部分内容，第一部分是对多旋翼模式的电机和权限进行基本设置，第二部分是在启用多旋翼控制的基础上，额外考虑了使用前飞电机辅助进行多旋翼模式下的前飞。当不启用第二部分时可以先不看第二部分内容。
程序主要内容介绍
第一部分
VtolType::update_mc_state();
本句首先将多旋翼模式的权限置为1。
if (_flag_enable_mc_motors) {
set_max_mc(2000);
set_idle_mc();
_flag_enable_mc_motors = false;
}
然后设置多旋翼模式的电机最大值和最小值范围。
第二部分，_params_standard.forward_thrust_scale参数可以控制是否启用余下的第二部分。
if (_params_standard.forward_thrust_scale< FLT_EPSILON) {
return;
}
这一段用来控制后面的代码要不要执行。
当要执行第二部分的代码时
过程略（以后需要时补充）
最终输出前飞动力的油门值
固定翼状态更新
Standard::update_fw_state()
本函数在VTOL主程序中飞行器处于固定翼模式时被调用。
程序有两部分内容，首先是进入VtolType::update_fw_state()程序中执行固定翼状态的初始化。
VtolType::update_fw_state();
这个程序的具体介绍将在关于vtol_type.cpp文件的介绍中详细介绍。
第二部分是将多旋翼部分的动力设置为无效。
if (!_flag_enable_mc_motors) {
set_max_mc(950);
set_idle_fw();// force them to stop, not just idle
_flag_enable_mc_motors = true;
}
等待tecs，本程序有疑问，tecs数据来自哪里？
Standard::waiting_on_tecs()
总结：standard构型的垂直起降飞行器的控制方法
进行垂直起降时，使用多旋翼控制器的输出，视情况使用前飞动力辅助多旋翼模式的前飞；
进行平飞时，使用固定翼控制器的输出；
进行转换时，共同使用多旋翼控制器和固定翼控制器的输出，使用权重系数来融合；
3. Tailsitter构型（尾座式构型）
下面分析尾座式构型的控制程序tailsitter.cpp
与standard构型类似，首先看更新垂直起降状态的程序。
Tailsitter::update_vtol_state()
首先参考程序中的注释
/* simple logic using a two way switch toperform transitions.
*after flipping the switch the vehicle will start tilting in MC control mode,picking up
*forward speed. After the vehicle has picked up enough and sufficient pitchangle the uav will go into FW mode.
*For the backtransition the pitch is controlled in MC mode again and switches tofull MC control reaching the sufficient pitch angle.
*/
注释翻译：使用两段开关的简单逻辑控制模式转换。在拨开关之后，飞行器在多旋翼控制模式开始前倾以获得前飞速度。在飞行器获得足够的前飞速度并且有足够的俯仰角之后，飞行器会进入固定翼模式。在向后转换时，俯仰角会再次在多旋翼模式控制，然后在达到有效俯仰角之后全部用多旋翼模式控制。
本函数也分为三部分
第一部分，在不要求进入固定翼模式时，根据当时的状态执行对应操作；
第二部分，在要求进入固定翼模式时，根据当时的状态执行对应操作；
第三部分，根据处理后的相应构型的内部状态更新主状态。
第一句，取飞行器姿态主题数据里面的q（四元数）转换为euler姿态角。
matrix::Eulerf euler =matrix::Quatf(_v_att->q);
具体操作过程不是很清楚。
获得俯仰角数值
float pitch = euler.theta();
进入第一部分内容，当时的状态是不是要求进入固定翼模式。
显然这个状态是使用者切换到多旋翼模式。
如果当前内部状态是MC_MODE，无操作；
如果当前内部状态是FW_MODE，将内部状态标记为“向后切换”TRANSITION_BACK；
如果当前内部状态是TRANSITION_FRONT_P1，表示此时是转换到多旋翼模式失败保护的状态，此时将内部状态标记为MC_MODE；
如果当前内部状态是TRANSITION_FRONT_P2，这是个未使用的选项；
如果当前内部状态是TRANSITION_BACK，说明现在状态是向后转换，此时会执行一个判断操作。当飞机的俯仰角大于阈值时，将内部模式设置为MC_MODE，说明此时要使用多旋翼模式来控制飞机。
进入第二部分内容，当前的状态是要求进入固定翼模式。
判断内部状态。
如果当前模式是MC_MODE，说明要进行向前转换，设置内部模式是TRANSITION_FRONT_P1。
如果当前内部状态是FW_MODE，无操作；
如果当前内部状态是TRANSITION_FRONT_P1，说明现在正在进行向前转换。此时要判断，当飞行器的空速和俯仰角都达到阈值时，或者飞行器在地面时，就设置内部模式为FW_MODE，之后就会用固定翼模式的控制器控制飞机。
如果当前内部状态是TRANSITION_FRONT_P2，检测下一个操作；
如果当前内部状态是TRANSITION_BACK，说明当前情况是转换失败，并且人工控制失败后切换到固定翼模式，此时要设定目标内部模式为FW_MODE。
第三部分进行内部模式和外部模式的赋值。
总结，本程序用于VTOL飞行器飞行状态的控制。
关于
Tailsitter::update_transition_state()函数
第一段程序，不在转换模式的时候，记录当前的值作为转换模式的开始值。
if (!_flag_was_in_trans_mode) {
// save desired heading fortransition and last thrust value
_yaw_transition =_v_att_sp->yaw_body;
_pitch_transition_start =_v_att_sp->pitch_body;
_thrust_transition_start =_v_att_sp->thrust;
_flag_was_in_trans_mode = true;
}
以下按照转换模式的不同状态分为3部分
第一部分是TRANSITION_FRONT_P1，向前转换阶段1。
1. 飞行器俯仰角设置点的控制
/** create time dependantpitch angle set point + 0.2 rad overlap over the switch value*/
_v_att_sp->pitch_body =_pitch_transition_start -(fabsf(PITCH_TRANSITION_FRONT_P1 - _pitch_transition_start) *
(float)hrt_elapsed_time(&_vtol_schedule.transition_start)/ (_params_tailsitter.front_trans_dur * 1000000.0f));
_v_att_sp->pitch_body =math::constrain(_v_att_sp->pitch_body , PITCH_TRANSITION_FRONT_P1 - 0.2f ,
_pitch_transition_start);
这部分的功能是根据开始转换的时间，将俯仰角姿态设置点逐步变化到设定的位置，有可能超过设定位置。在其他位置会检测飞行器当前俯仰角，超过阈值时进入下一模式。
2. 飞行器油门设置点的控制
if(_airspeed->indicated_airspeed_m_s <= _params_tailsitter.airspeed_trans){
_thrust_transition= _thrust_transition_start + (fabsf(THROTTLE_TRANSITION_MAX * _thrust_transition_start)*
(float)hrt_elapsed_time(&_vtol_schedule.transition_start) /(_params_tailsitter.front_trans_dur * 1000000.0f));
_thrust_transition= math::constrain(_thrust_transition , _thrust_transition_start ,
(1.0f + THROTTLE_TRANSITION_MAX) *_thrust_transition_start);
_v_att_sp->thrust= _thrust_transition;
}
检测飞行器的空速，空速低于阈值时，执行油门随时间增加的操作，直到达到设定的空速阈值。
3. 飞行器偏航设置点的控制
if(_airspeed->indicated_airspeed_m_s > ARSP_YAW_CTRL_DISABLE) {
_mc_yaw_weight = 0.0f;
} else {
_mc_yaw_weight = 1.0f;
}
当飞行器的空速低于控制阈值的时候，多旋翼的偏航控制权限为1，空速高于阈值的时候，控制权限为0。
4. 其他权限
_mc_roll_weight = 1.0f;
_mc_pitch_weight = 1.0f;
向前转换的P1阶段，多旋翼模式roll和pitch的权限都是最大。
第一部分是TRANSITION_FRONT_P2，向前转换阶段2。
这一阶段的作用是在飞行器准备切换到固定翼模式时，平滑切换过渡时的控制器输出。但是此版本程序实际上并没有使用P2阶段，而是在P1阶段结束时直接进入固定翼模式。
程序内容：飞行器从90度切换到0度，飞行器的俯仰角设置点随时间逐渐减少，油门变为之前的0.9，多旋翼偏航控制权重置为0，多旋翼的俯仰和横滚控制权重随时间变小。
其他函数暂不解析。
4. Tiltrotor构型（倾转螺旋桨构型）
直译应该是倾转旋翼构型，但是在中文语境里一般将类似直升机的带周期变距功能的叫做旋翼，其他固定桨距的一般称为螺旋桨。尤其是对于小型无人机的小螺旋桨更应该称为倾转螺旋桨，用以与倾转旋翼的技术难度相区别。
更新垂直起降状态
Tiltrotor::update_vtol_state()
本程序用来控制飞行器模式转换开始和过程中的状态。
不要求进入固定翼模式时，根据构型的内部状态，有5种情况。
1. MC_MODE
此模式时说明到达目标状态，不会做任何处理。
2. FW_MODE
内部是固定翼模式，需要转换到多旋翼模式，此时将模式定为TRANSITION_BACK
3. TRANSITION_FRONT_P1
内部是向前转换阶段1，此时进入多旋翼模式，说明手动从转换模式切换到了多旋翼模式，可能是转换出问题之后的退回。设置内部模式为MC_MODE。
4. TRANSITION_FRONT_P2
原理同上，内部是向前转换阶段2，此时进入多旋翼模式，说明手动从转换模式切换到了多旋翼模式，可能是转换出问题之后的退回。设置内部模式为MC_MODE。
5. TRANSITION_BACK
内部是向后转换模式，说明飞行器正在执行向后转换的倾转模式，此时会检测飞行器动力短舱的倾转角度，在小于阈值时，之后将飞行器模式设置为多旋翼模式。
在要求进入固定翼模式时，根据内部状态，也有5种情况。
1. MC_MODE
内部状态是多旋翼模式时，说明此时处于转换模式的开始阶段，需要将内部模式设置为TRANSITION_FRONT_P1向前转换P1。
2. FW_MODE
内部状态是固定翼模式，此时飞行器已经处于固定翼状态，说明转换成功。
3. TRANSITION_FRONT_P1
内部是向前转换状态1，此时要保持这个状态，并且检测飞行器的空速，当空速大于阈值（或者飞行器在地面时），飞行器可以进入向前转换状态2。
4. TRANSITION_FRONT_P2
飞行器内部是向前转换状态2时，要不断检测飞行器动力短舱的倾转角。如果短舱倾角达到固定翼模式可接受的阈值，就将内部模式设定为FW_MODE，之后飞行器就会进入固定翼模式。
5. TRANSITION_BACK
当前内部状态是向后转换时，在现在的情况下，说明飞行器先从固定翼模式转换到多旋翼模式，还没有完成时，又向固定翼模式转换。此时直接设置为固定翼模式。
本程序的最后一部分是用内部状态更新外部状态。
程序Tiltrotor::update_mc_state()
程序内容：
更新权限
更新确认倾转角处于多旋翼模式。
启用尾电机
设置多旋翼模式电机的怠速状态。
程序Tiltrotor::update_fw_state()
程序内容：
更新权限
更新确认倾转角处于固定翼模式。
禁用尾电机。
设置固定翼模式电机怠速值（最小值）
程序Tiltrotor::update_transition_state()
更新_flag_was_in_trans_mode标志位，运行到这里，说明进入了转换状态。
主要有3部分内容：向前转换状态1，向前转换状态2，向后转换
当飞行器进入TRANSITION_FRONT_P1状态时
1. 启用尾电机
2. 随着时间，倾转电机向前倾转直到某个角度。
3. 控制多旋翼模式的roll权限，当空速小于阈值时权限为1，大于阈值时权限为0。
问题：电机前倾的时候，如果继续用多旋翼模式控制roll姿态角，此时多旋翼模式的roll会有飞机上roll和yaw的分量。
4. 控制多旋翼模式的yaw权限，当空速小于另一个阈值时权限为1，大于阈值时权限为0。
问题，同上，当多旋翼控制yaw的时候，飞行器会有yaw和roll的分量。或者具体如何操作与构型有关。
5. 转换油门与多旋翼模式的油门相同。
当飞行器进入TRANSITION_FRONT_P2状态时
1. 电机倾转角随着时间逐步变化为固定翼模式的状态。
2. 多旋翼模式的roll权限为1。
3. 油门还是保持多旋翼模式油门。
当飞行器进入TRANSITION_BACK模式时
1. 设置尾旋翼状态为怠速
2. 设置多旋翼模式电机的怠速值
3. 倾转旋翼随着时间从固定翼模式转换到多旋翼模式
4. 设置多旋翼模式的油门输出值为0？
5. 设置固定翼模式的roll权限为0
总结，本程序有可能对特定构型的倾转螺旋桨有效，但是显然是很不全面的，对倾转过程的考虑非常简化。 感觉你可以把这些写进注释里面给学习的人看 可以的，之后有时间再整理一下 有源码下载地址吗？ 最近一直在看modules 下follow_target 的源代码，有些看不懂，不知道你什么时候会针对这个进行分析一下 可以，这个看过的，有时间写一下 感谢 十分感谢 现在版本是不是有改动了
页:
[1]