PX4笔记系列（二）vtol_att_control中的tiltrotor解析

总结PX4 v1.11.1下的tiltrotor过渡模式

update_vtol_state()

注释：采用两段切换的简单逻辑来进行过渡
当切换过渡开关后，飞机将会开始倾转动力，逐渐加速，当飞机获得足够速度后动力将会完全打到向前的位置。对于过渡返回阶段动力则是简单地倾转回来。

如果（不在向前过渡指令下）：
当前vtol飞行模式：
1、旋翼模式 无操作
2、固定翼模式 令vtol飞行模式切换到过渡返回模式
3、P1过渡模式 进入过渡失败安全返回至旋翼模式
4、P2过渡模式 进入过渡失败安全返回至旋翼模式
5、过渡返回模式 记录过渡返回时间=当前时间-过渡开始时间
如果（当前倾转角度小于等于旋翼模式倾转角度 并且 过渡时间大于返回最大时间）：
vtol飞行模式切换回旋翼模式

如果（向前过渡指令下）：
当前vtol飞行模式：
1、旋翼模式 vtol飞行模式切换到P1过渡模式并记录过渡开始时间
2、固定模式 无操作
3、P1过渡模式：过渡时间=当前时间-过渡开始时间
检测是否有能够触发过渡的速度值（测得的空速是否有限 并且 空速并未被禁止）
bool 是否切换到P2=false
如果过渡时间大于向前过渡最小速度
并且如果有能够触发过渡的速度值
判断 当前速度是否大于P2切换速度 是 输出切换到P2
如果没有能够触发过渡的速度值
判断 如果倾转角度已经大于过渡预设倾转角度并且过渡时间大于向前倾转开环控制时间 是 输出切换到P2
transition_to_p2 |= can_transition_on_ground()；
应该理解为只要两者有一个是1就可以输出为1其他情况输出为0

如果能够切换到P2
vtol飞行模式切换为P2过渡模式
过渡时间重新计数

4、P2模式
注释：如果动力已经被完全倾转那么我们直接切换到固定翼模式
如果（倾转角度大于固定翼倾转角度）
vtol飞行模式转换为固定翼模式
倾转角度等于固定翼模式倾转角度

5、过渡返回模式
注释：认为过渡失败，切换回固定翼模式
vtol飞行模式切换为固定翼模式

注释：将倾转旋翼特别控制模式映射到简单控制模式下
判断：vtol飞行模式
1、旋翼模式 _vtol_mode模式等于倾转机翼模式
2、固定翼模式 _vtol_mode模式等于固定翼模式
3、P1过渡模式
4、P2过渡模式 _vtol_mode模式等于过渡至固定翼模式
5、过渡返回模式 _vtol_mode模式等于过渡至旋翼模式

update_mc_state()

注释：电机启动时间：定义解锁后的第一秒为电机启动时间
在这段时间内倾转角度将被设置为VT_TILT_SPINUP值。
这允许使用者设置一个启动倾转角度以防止动力无法在完全垂直状态（旋翼模式）平滑地启动

P1阶段启动时间=1000ms//启动第一阶段（在固定倾转角度）
P2阶段启动时间=700ms//启动第二阶段（从启动倾转角度过渡至旋翼倾转角度）

注释：解锁的时候重置时间戳
如果（控制模式不等于锁定）
最新解锁时间戳=当前时间
判断是否需要启动倾转动力？如果参数启动倾转角度>0.01就为1
其他情况 如果需要启动倾转动力
//让电机在待机时就向前倾转以使其更容易启动
如果（解锁后时间大于P1阶段启动时间+P2阶段启动时间）
启动倾转动力等于0

如果启动需要倾转动力
如果（解锁后时间小于P1阶段启动时间）
倾转角度等于启动倾转角度
否则
//在第二阶段开始调整倾转角度至旋翼模式
det倾转角度=旋翼倾转角度-启动倾转角度
倾转角度=启动倾转角度+det倾转角度/P2阶段启动时间*（当前时间-（上次解锁时间+P1阶段启动时间点））{=初始倾转角度+所需倾转角度*当前经历时间占总P2启动阶段时间的比例}

旋翼偏航权重=0//在启动阶段禁用方向操纵

如果（启动不需要倾转动力）
//正常操纵
倾转角度=推进辅助函数输出（后面再看）
旋翼偏航权重=1
油门=倾转油门补偿值？？？（见下）

update_fw_state()

注释：确保动力是向前倾转的
倾转角度=固定翼倾转角度

update_transition_state()

注释：将虚拟姿态指令值复制到真实姿态指令值中去（我们使用多旋翼姿态指令值）
将虚拟姿态指令值复制给旋翼虚拟姿态指令值
机体滚转指令=固定翼虚拟姿态指令值中的滚转指令值

过渡时间=当前时间-过渡开始时间

如果（不在过渡模式）
注释：保存过渡需要的指向和最新的油门值
在过渡模式=true

如果（vtol飞行模式==P1过渡阶段）
注释：过渡第一阶段所有的动力依然是可启动的
如果（动力状态不等于动力可用）
动力状态=改变动力状态（动力状态，动力可用）
注释：倾转动力向前至某固定角度
如果（倾转角度<=P2起始角度）
倾转角度=旋翼倾转角度+绝对值（P2起始角度-旋翼倾转角度）*过渡时间/前过渡阶段时间
注释：低速下旋翼满权重
旋翼滚转权重=1；
旋翼偏航权重=1；

注释：减少旋翼控制一旦飞机开始提速
如果（空速可用&&空速值合理&&当前空速大于偏航控制失效速度）
旋翼偏航控制=0；

如果（空速可用&&空速值合理&&当前空速大于blend速度（操纵混合空速））
旋翼滚转控制=1-（当前空速-blend速度）/（进入P2空速-blend速度）

注释：没有空速的情况采用按时间过渡的权重变化

如果（空速不可用或空速值不合理&&过渡时间大于最小过渡时间）
旋翼滚转权重=1-（过渡时间-最小过渡时间）/(开环过渡时间-最小过渡时间)

旋翼偏航权重=旋翼滚转权重

过渡过程油门=-旋翼虚拟油门值

如果（vtol飞行模式==P2过渡阶段）
注释：飞机已经准备好进入固定翼模式，将动力完全向前倾转
倾转角度=P2起始角度+绝对值（固定翼倾转角度-P2起始角度）*P2起始过渡时间/P2前倾时长

旋翼滚转权重=0
旋翼偏航权重=0；

注释：固定翼飞行模式下不使用斜坡限制动力的方式关闭动力（即逐渐限制最大输出PWM值使得动力可用范围变化）

斜坡 下降值=（1-P2过渡开始时间/P2前倾时长）*（PWM初始最大值-PWM初始最小值）+PWM初始最小值

电机状态=设置电机状态（电机最大值等于斜坡下降值）

过渡油门值=-旋翼虚拟油门值

如果vtol飞行模式等于过渡返回模式
注释：打开所有旋翼电机
如果（电机状态不等于电机可用）
电机状态=设定电机状态（电机可用）

注释：设定倾转机翼模式的怠速
如果（不等于旋翼怠速）
旋翼怠速等于设置旋翼怠速

注释：反向倾转动力
如果（倾转角度>旋翼倾转角度）
倾转角度=固定翼倾转角度-绝对值（固定翼倾转角度-旋翼倾转角度）*返回开始至今时间/1

旋翼偏航控制权重=1

注释：控制返回模式采用俯仰角减速方法
如果（爬升率控制可用）
俯仰控制=更新获取返回模式俯仰角指令值

注释：当我们很快倾转回来，动力保持怠速旋转
如果（返回过渡开始后时间<1）
旋翼油门权重=0
旋翼滚转权重=0
旋翼俯仰权重=0

如果返回开始时间>=1
旋翼滚转权重=1
旋翼俯仰权重=1
注释：慢慢地降低油门以防止突变输入
旋翼油门=（返回开始时间-1）/1

将所有三通道指令值转化为四元数
旋翼姿态四元数指令值（）
复制给虚拟指令中地q_d

旋翼滚转权重=限幅（0，1）
旋翼偏航权重=限幅（0，1）
旋翼油门权重=限幅（0，1）

waiting_on_tecs()

注释：保持多旋翼油门直到我们从TEC中获得数据
虚拟控制量前飞油门=过渡油门值

注释：给作动器输出topic

fill_actuator_outputs()

注释：多旋翼输出
控制组0.时间戳=当前时间
控制组0.时间戳样本=旋翼操纵输入.时间戳样本？？？？

控制组0.滚转控制=旋翼操纵输入.滚转输入旋翼滚转权重
控制组0.俯仰控制=旋翼操纵输入.俯仰输入旋翼俯仰权重
控制0.偏航控制=旋翼操纵输入.偏航输入*旋翼偏航权重

如果（vtol飞行模式=固定翼模式）
控制组0.油门控制=固定翼操纵输入.油门输入

注释：允许差动油门
如果（允许差动油门）
控制组0.滚转控制=固定翼操纵输入.偏航控制？？？？？？

如果（vtol飞行模式=其他模式）
控制组0.油门控制=固定翼操纵输入.油门输入*旋翼油门权重

注释：固定翼输出
控制组1.时间戳=当前时间
控制组1.时间戳样本=旋翼操纵输入.时间戳样本？？？？

控制组1.控制（5）=倾转角度
如果(旋翼气动舵锁定并且vtol飞行模式等于旋翼模式）
控制组1.滚转控制=0；
控制组1.俯仰控制=0；
控制组1.偏航控制=0；

如果（其他情况）
控制组1.滚转控制=固定翼输入.滚转输入；
控制组1.俯仰控制=固定翼输入.俯仰输入；
控制组1.偏航控制=固定翼输入.偏航输入；

注释：如果发动机倾斜，增加旋翼螺旋桨的组合推力。假设所有MC电机倾斜相等

thrust_compensation_for_tilt()

注释：只补偿倾转角度到最大0.5最大倾转角度
补偿倾转角度=限幅（0，0.5）
注释：增大虚拟油门乘以1/cos(tilt)只，限幅至（-1.0）
返回 限幅（（虚拟油门/补偿倾转角度2pi），-1，0）

控制组0：（姿态）
0-滚转
1-俯仰
2-偏航
3-油门
4-襟翼

控制组1：（负载/特殊）