[发明专利]一种变距伺服机构寻零控制算法

权利要求书

1.一种变距伺服机构寻零控制算法，其特征在于，所述变距伺服机构寻零控制算法包括：

根据飞控系统上电状态中的自检指令状态位判断飞控计算机是否为正常上电状态，若飞控计算机不是正常上电状态，则获取当前飞控系统的位置指令，执行变距伺服控制；若飞控计算机是正常上电状态，则变距伺服机构在正常上电状态位保持预设上电时长；

执行外伸指令预设外伸时长；

执行回缩指令；

判断所述变距伺服机构是否达到回缩极限状态，若所述变距伺服机构达到回缩极限状态，则执行寻零控制操作；若所述变距伺服机构未达到回缩极限状态，则判断所述变距伺服机构的回缩时长是否大于预设的最大回缩时长，若所述变距伺服机构的回缩时长大于预设的最大回缩时长，则进入寻零超时控制状态；若所述变距伺服机构的回缩时长不大于预设的最大回缩时长，则返回上一步继续执行回缩指令。

2.根据权利要求1所述的变距伺服机构寻零控制算法，其特征在于，若所述变距伺服机构达到回缩极限状态，则执行寻零控制操作具体包括：

给定位置控制指令为零，设定最大回零电流，对回零速度指令进行PID控制；

判断回零时长是否超过最大回零时长，若回零时长超过最大回零时长，则进入所述寻零超时控制状态；若回零时长未超过最大回零时长，则判断回零时长是否超过预设回零时长，若回零时长超过预设回零时长，则将控制电流设定为正常模式的最大电流后，进行位置误差判断；若回零时长未超过预设回零时长，则直接进行位置误差判断。

3.根据权利要求2所述的变距伺服机构寻零控制算法，其特征在于，所述位置误差判断具体包括：

判断当前位置和给定的零位指令的误差是否小于设定误差，若当前位置和给定的零位指令的误差小于设定误差，则回零成功，执行所述变距伺服控制，同时设定回零状态为“正常回零”；若当前位置和给定的零位指令的误差不小于设定误差，则返回给定位置控制指令为零，设定最大回零电流，对回零速度指令进行PID控制。

4.根据权利要求1至3中任一项所述的变距伺服机构寻零控制算法，其特征在于，所述变距伺服控制通过PID控制算法根据当前飞控系统的位置指令对位置进行控制。

5.根据权利要求1所述的变距伺服机构寻零控制算法，其特征在于，所述寻零超时控制状态通过系统状态上报回零失败状态，并进入低功耗自锁模式。

6.根据权利要求1至5中任一项所述的变距伺服机构寻零控制算法，其特征在于，在执行所述外伸指令时，外伸速度设定为最大外伸速度。

7.根据权利要求1所述的变距伺服机构寻零控制算法，其特征在于，在执行所述回缩指令时，回缩速度设定为最大回缩速度，同时将最大电流调整到预设电流值。

8.根据权利要求1所述的变距伺服机构寻零控制算法，其特征在于，所述回缩极限状态为回缩转速持续0.5s小于0.05rpm。

9.根据权利要求3所述的变距伺服机构寻零控制算法，其特征在于，所述设定误差为0.1mm。