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。