ArduPilot——AP_L1_control——如何更新航点？

版权声明：本文为博主原创博文，未经允许不得转载，若要转载，请说明出处并给出博文链接 

本文详细讲解了ArduPilot中AP_L1_control库中的update_waypoint函数的实现！！！
L1控制器的update_waypoint方法的最终输出是一个所需的横向加速度（以下以红色显示为“latAccDem”），它应使飞机回到原点origin和目的地destination之间的线路。

使用的公式如下所示。 阻尼damping来自NAVL1_DAMPING参数。 周期period来自NAVL1_PERIOD参数。



alongTrackDist : 从A到从A到B线上最近点的距离

A_air : 原点到当前飞机所在位置的距离
L1_dist : 轨迹上从alongTrackDist到目标点的距离        
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)