接受数据(获取位置信息、期望位置)
外环控制PID(位置环):
- 通过调节速度减小位置差,位置差需要通过速度去弥补。、
- 判定模式不同,位置差的来源(三种控制源,manual(遥控器),offboard(通过外部模块提供,例如视觉),auto模式:飞控自己计算,例如路径规划)
- 进行外环计算
- 对计算结果进行限幅(land,takeoff,加速度限幅)
内环控制PID(速度环)
5. 速度差计算:作用于速度差,通过加速度去,弥补
6. 积分项计算
7. 计算期望推力:
thrust_sp期望推力=vel速度偏差*PID
8.期望推力限幅、抗饱和积分处理