四軸起飛時，發出觸發信號使導航模塊開始工作，同時讀取ICM20602的加速度計、陀螺儀數據，對數據卡爾曼濾波后姿態解算，對角度與角速度采取串級PID調節。
控制系統算法設計主要有ICM20602濾波算法，姿態解算算法、串級PID控制算法和定高部分控制算法。礙于篇幅所限，下面介紹最重要的串級PID控制算法和定高部分控制算法。
地理坐標系中重力的水平分量為零，僅用三軸陀螺儀和三軸加速度計無法計算出航向角，由于巡線機器人保持穩定飛行只需要橫滾角（roll）和俯仰角（pitch），所以四元數轉換成歐拉角。
定高控制算法采用的是增量式PID控制，定高控制的輸出最后與姿態控制的輸出疊加到四個電機的控制中。數據濾波使用的是低通濾波，采用近三次的平均值。為了防止姿態對激光測距的影響及減小高度控制對姿態控制的干擾使用歐拉角來校正高度值，即Hight=(float)Hight*(cos(roll)* cos(pitch))。將四元數轉換后的歐拉角與陀螺儀測出來的角速度進行串級PID控制，其中歐拉角作為外環，角速度作為內環。外環的PID以及內環的PD設定值為測試數據值。由于內環的角速度控制不需要無靜差,所以內環采用PD控制,為防止測量的誤差造成較大影響,外環積分需要限幅。
