版权声明：本文为博主原创博文，未经允许不得转载，若要转载，请说明出处并给出博文链接
本文详细讲解了ArduPilot中AP_L1_control库中的update_waypoint函数的实现！！！
L1控制器的update_waypoint方法的最终输出是一个所需的横向加速度（以下以红色显示为“latAccDem”），它应使飞机回到原点origin和目的地destination之间的线路。
使用的公式如下所示。 阻尼damping来自NAVL1_DAMPING参数。 周期period来自NAVL1_PERIOD参数。
alongTrackDist : 飞机当前位置沿平行于线AB到参考目标点的距离
A_air : A点到飞机当前所在位置的距离
L1_dist : 飞机当前位置到参考目标点的距离
L1_dist = 1/π * damping * period * speed ≈ 0.3183099* damping * period * speed
Nu1 : vehicle(飞机等)到L1_dist与从A到B的线形成的夹角
Nu2 ：vehicle velocity（飞机等速度）与线AB的夹角
latAccDem ：理想的飞机横向加速度输出
latAccDem = 4 * damping² * speed² * sin(Nu) / L1_dist . (Nu = Nu1 + Nu2)
本文提到的L1 control参考的是一篇来自AIAA的论文《A New Nonlinear Guidance Logic for Trajectory Tracking》。想进一步学习的链接给出：
原文
https://download.csdn.net/download/u012814946/10334104
or
中文翻译版本
https://download.csdn.net/download/u012814946/10392973