DJI串级PID算法架构

1.获得当前位置xyz

2.获取当前poll，pitch，yaw

3.求出当前位置离一开始的位置是否有误差，误差距离为，xyz

4.根据设定的值看看还要走多远，用设定的值减去xyz表示剩下的距离。

5.设置角度误差的极限值。0.01745329252*yawDesired

double yawDesiredRad = DEG2RAD*yawDesired;  //角度误差
  double yawThresholdInRad = DEG2RAD*yawThresholdInDeg;  //角度最大误差
  float32_t posThresholdInM = posThresholdInCm/100;   //位置最大误差，默认为5
  int elapsedTime = 0;            //执行的次数
  int speedFactor = 2;            //定义的xy的阈值，单位米

6.指望误差绝对值大于2米则取移动距离为2米，小于两米等于自己。高度的话无限制，直接当前高度+指望高度就好了。

7.根据第4步计算的值去和咱们定的最小距离0.05进行比较，若是比这个偏差大就进行调整。

8.若是比偏差大就进行移动。移动的距离就是第六步的结果。

9.而后获取当前位置，并和初始位置比较，看下仍须要走多远。计算出已经走得xyz向量。

10.求出还须要走的距离向量xyz，用总距离减去已经走过的。

11.最后看剩下的距离是否大于2米，大于两米继续，不然进行微调。

 

timeoutInMs默认10000,10000/20*20=10000ms=10s，因此一次得移动最多执行500次，耗时10s。