这一部分主要介绍PX4核心控制算法是怎么实现的。其实核心的控制算法部分很简单，大部分代码都是在进行各种分情况讨论，各种安全检查。下面简单介绍一下，希望大家看了之后有点收获，能给我点个赞，嘿嘿。
这部分代码在 src/modules/mc_att_control里
主程序在 mc_att_control_main.cpp 里
简单介绍就是先生成姿态角指令（roll, pitch, yaw的指令，即attitude_setpoint）
姿态指令生成👆（在函数最后publish了）
然后根据这个指令计算姿态角速度指令（即_rates_sp）
计算姿态角速度指令👆（_attitude_control是AttitudeControl类的实例，具体实现在同文件夹下的AttitudeControl里）
然后把姿态角速度指令发布
发布姿态角速度指令👆
这部分代码在 src/modules/mc_rate_control里
即根据姿态角速度指令来计算得到期望力矩，这也是本文需要改的部分。
主程序在 MulticopterRateControl.cpp 里
简单介绍就是先订阅 角速度指令rates_sp 的消息
获取角速度指令👆
然后计算控制力矩指令
根据当前的角速度和角速度指令计算控制力矩指令（就是这个att_control）👆
接着发布这个att_control
发布控制力矩指令att_control👆
这部分内容也挺多的，主要是在 src/lib下的 mixer 和 mixer_module里，四旋翼的具体实现是在src/lib/mixer/MultirotorMixer/MultirotorMixer.cpp 里
直接看分配的主要函数：mix
多旋翼分配实现👆
其实就是先获取控制力矩指令，然后分情况调用函数进行分配。
比如在使用jmavsim仿真时调用的是 mix_airmode_disabled 函数
mix_airmode_disabled中分配的实现👆
这部分就是具体的实现过程了，首先是先不考虑 yaw 进行分配，然后最后再单独分配 yaw.
经过 part1 的推导之后我们知道，INDI需要得到角加速度的值来进行计算，猜怎么着？我惊奇的发现 PX4 v11 里这个角加速度值已经算出来了，之前的版本里都是没有这个量的，而且这个角加速度的量在现在的 PX4 里也没有用到，不知道他们是要搞什么。
接下来讲一讲这个角加速度的始末：
这个角加速度怎么来的呢？
在src/modules/sensors/vehicle_angular_velocity是用来计算角速度和角加速度的，其中的.cpp 文件里有计算的流程
计算角速度和角加速度的过程👆
简单来说就是：
获取陀螺仪数据并矫正热误差
→ 矫正运行中的偏差
→ 应用陷波滤波器对上面得到的角速度值原始值进行滤波
→ 对滤波后的角速度进行微分得到角加速度值
→ 把角速度和角加速度存一下
→ 对两个值应用低通滤波器进行平滑处理
处理完了就把两个量发布
发布角速度和角加速度量👆
于是我们就可以在msg里找到 vehicle_angular_velocity.msg 和 vehicle_angular_acceleration.msg 这两个文件了，也就是说可以通过 μORB 订阅到这两个topic了。另外，这个 vehicle_angular_acceleration.msg 目前还没有存到日志文件里，可以通过简单处理把他存到日志文件里，便于后面的分析。
经过 part1 的推导之后我们知道，INDI需要得到 电机转速 的值来进行计算，猜怎么着？我惊奇的发现 PX4 v11 里这个电机转速值已经估计出来了，之前的版本里都是没有这个量的，而且这个电机转速的量在现在的 PX4 里也没有用到，不知道他们是要搞什么。（他们不会要把姿态控制换成INDI吧。。）
这个电机转速测量值（估计值）我是先在msg文件夹下发现的，就是 rpm.msg 。但是这个东西我还妹有完全搞明白，不知道是根据什么估算的电机转速，有兴趣的朋友可以了解一下px4里这个 rpm 是怎么估计的。（我知道有的穿越机电调可以回传电机转速，这个就比较成熟了。还有篇文章是用类似鼠标里那个光栅编码器来测转速的，不过这个可能撞一下就坏了，不太好）
但是！要说但是了。
我试了下，这个转速估计值在jmavsim仿真里是没有的计算出来的，没试gazebo。
另外，用这个电机转速还有个问题在于，px4里实际输出的电机指令也不是电机的转速，而是分配之后，到每个电机头上的归一化的（可以认为是推力）值计算出的 pwm 值，所以考虑电机转速回传之后，会带来很多麻烦，我嫌麻烦还没有分析这个，所以暂且按下不表。
于是我的处理方法是，用类似的滞后环节去模拟电机的响应过程。也就是对电机进行建模，然后估计电机的当前转速。目前的简易做法是计算出力矩指令之后，加上一个滞后作为实际得到的控制力矩
INDI 的原理在 part1 中已经介绍过，下面简要回顾并分析下需要修改的地方：
对于角速度控制环来说，令指令跟踪误差为
e
Ω
=
Ω
−
Ω
r
e_{\tiny \Omega}=\Omega-\Omega_r
eΩ=Ω−Ωr
则角速度跟踪误差的动态方程可以写成
e
˙
Ω
=
Ω
˙
0
−
Ω
˙
r
+
g
(
x
s
t
a
t
e
s
)
Δ
ω
2
\dot{e}_{\Omega}=\dot{\Omega}_{\tiny 0}-\dot{\Omega}_r+g(x_{states})\Delta{\omega^2}
e˙Ω=Ω˙0−Ω˙r+g(xstates)Δω2
那么根据 INDI 的原理可以得到（电机引起的）力矩的增量
G
M
Δ
ω
2
=
−
J
(
Ω
˙
0
−
Ω
˙
r
+
K
p
e
Ω
)
G_M\Delta{\omega^2}=-J(\dot{\Omega}_{\tiny 0}-\dot{\Omega}_r+K_pe_{\tiny \Omega})
GMΔω2=−J(Ω˙0−Ω˙r+KpeΩ)
然后可以得到力矩的指令：
M
c
=
G
M
⋅
ω
0
2
+
G
M
⋅
Δ
ω
2
M_c=G_M\cdot{\omega_{\tiny 0}^2}+G_M\cdot\Delta{\omega^2}
Mc=GM⋅ω02+GM⋅Δω2
其中
ω
0
2
\omega_{\tiny 0}^2
ω02是估计出来的或者测量回来的转速。
或者写成
M
c
=
M
c
0
+
G
M
⋅
Δ
ω
2
M_c=M_{c_{\tiny 0}}+G_M\cdot\Delta{\omega^2}
Mc=Mc0+GM⋅Δω2
其中
M
c
0
M_{c_{\tiny 0}}
Mc0是计算或者估计出来的当前控制力矩
也就是说，需要改的或者添加的就是 (1)获取角加速度，(2)获取指令微分，(3)修改控制律公式，(4)获取 M c 0 M_{c_{\tiny 0}} Mc0。（ (2)可选，因为这个东西我看有的控制方法里用，有的不用，比如在PID的误差 e e e 微分里就包含这个指令微分，但是如果用测量值 y y y 计算微分量的话就不包含这个指令微分）
需要修改的代码都在 src/modules/mc_rate_control/Multicopter
角加速度已经发布了，所以直接订阅就行了
怎么订阅，有个简单的方法，直接按照角速度订阅的方式来一遍就行了：
订阅角速度👆，照例订阅一下角加速度即可
直接对订阅得到的 rates_sp 进行微分即可，不过这一步我省略了…
角速率环的控制律公式在src/modules/mc_ate_control/RateControl/RateControl.cpp里，就一行：
角速度PID控制律👆
这是经典的PID控制，这里我们如果要改成INDI只需要：
角度环INDI控制律👆
这两个式子就是在实现
G
M
Δ
ω
2
=
−
J
(
Ω
˙
0
+
K
p
e
Ω
)
G_M\Delta{\omega^2}=-J(\dot{\Omega}_{\tiny 0}+K_pe_{\tiny \Omega})
GMΔω2=−J(Ω˙0+KpeΩ)
和
M
c
=
M
c
0
+
G
M
⋅
Δ
ω
2
M_c=M_{c_{\tiny 0}}+G_M\cdot\Delta{\omega^2}
Mc=Mc0+GM⋅Δω2
（
e
Ω
e_{\tiny \Omega}
eΩ的符号不对是因为我的误差的定义和PX4正好相反）
另外解释下，其中我在yaw通道加了个微分误差factor，因为仿真的时候发现yaw通道老震荡，所以多加了微分项抑制一下。按理说是不需要微分项的，后面可能要看一下为什么会出现震荡情况。
另外，在本文中这个 G M Δ ω 2 = − J ( Ω ˙ 0 + K p e Ω ) G_M\Delta{\omega^2}=-J(\dot{\Omega}_{\tiny 0}+K_pe_{\tiny \Omega}) GMΔω2=−J(Ω˙0+KpeΩ)公式里的 J J J就相当于一个可整定的参数。原因有四点：（1）我这里没有修改Mixer里的 G M G_M GM，（2）操纵导数本来就比较难获得（需要螺旋桨的力系数和力矩系数），（3）INDI里的操纵导数本来就是一个可以用来整定的量，（4）操纵力矩分配之后映射到的是输出给电机的PWM信号，而PWM信号和电机转速之间的关系也是个黑箱
M
c
0
M_{c_{\tiny 0}}
Mc0 也就是上面 角度环INDI控制律 图里的torque_prev.
这里我是直接用一个低通滤波器作为控制力矩响应模型的。
意思就是说，真实的控制力矩响应比控制力矩指令就差了一个低通滤波器过程。即：
M
c
0
=
M
c
⋅
G
l
p
f
M_{c_{\tiny 0}}=M_{c}\cdot{G_{lpf}}
Mc0=Mc⋅Glpf
实现起来就是这么一句：
torque_prev = _lp_filters_rate.apply(torque1);
由于懒，我这里用的 filter 还是px4自带的low pass filter。
！！！！！这里需要注意的一点是：由于之前获取角加速度的时候，也用了低通滤波器和陷波滤波器，导致角加速度与当前的真实角加速度有滞后，所以为了让测量或者估计的真实力矩响应与角速度同步，需要在测量或者估计的真实力矩上 再 加上同样的滞后。
OK，这四部分就介绍完了，剩下的就是调调参数的事情。我大概调了半天，才调到和之前的 PID 效果差不多的样子。
目前我改的这个 PX4 v11 不是一个稳定的版本，是开发中的版本（很神奇地在里面发现了角加速度估计值和电机转速估计值，不知道开发人员是在憋什么大招）。但是要修改其他版本其实也是大同小异，毕竟v11里只是把角速度控制单独拿出来放到一个文件夹下了而已。
INDI的好处是鲁棒性和容错性比较强，在模型失配程度比较高的情况下也可以得到比较好的控制效果，另外在出现故障或者外部扰动的时候也能快速恢复稳定。这实际上得益于INDI能够实时将系统动态通过角加速度反馈到控制器中，比如当外部有强风吹的时候，PID需要有积分项才能重新实现在强风影响下的稳定，相当于是一个对强风影响的估计，这个过程是比较慢的，但是反馈角加速度可以快速把这个影响反馈到控制器中。大概就是这个意思，如果要深入理解可能要结合仿真比较好。
就介绍到这里，如果大家看了之后有什么想法，欢迎交流。