导航与控制周期不同步的飞行控制方案的软件实现方法与流程

本发明涉及飞行控制软件设计技术领域，特别是一种导航周期与控制周期不同步控制方案的软件实现方法。

背景技术：

飞行控制软件是各种飞行器系统中的核心软件，其控制算法依赖于导航精度。缩短惯性导航运算周期是提升导航精度的一个有效方法。当惯性导航运算周期缩小到一定程度后，由于在惯性导航算法之后运行的组合导航修正算法和控制算法无法在一个惯性导航周期内运算完成，故而出现了导航周期与控制周期不同步的控制方案。通常的设计中，导航周期与控制周期成倍数关系，例如控制周期是惯性导航周期的两倍。

由于运行在控制周期中的组合导航修正算法和控制算法需要使用惯性导航算法的输出作为输入，而组合导航算法要修正当前的惯性导航输出并作为下个惯性导航周期的输入，因此在导航周期与控制周期不同步的控制方案中，如何处理导航与控制算法的输入输出数据传递过程是非常关键的问题。

技术实现要素：

为解决现有技术中存在的问题，本发明的目的是提供一种导航与控制周期不同步的飞行控制方案的软件实现方法，本发明简化了程序结构，同时避免了死锁的出现。

为实现上述目的，本发明采用的技术方案是：一种导航与控制周期不同步的飞行控制方案的软件实现方法，包括通信连接的惯性导航模块和控制周期模块，所述控制周期模块用于接收卫星信号，并对卫星信号的有效性进行判断，以及接收所述惯性导航模块输出的导航数据，所述惯性导航模块用于接收所述控制周期模块输出的组合导航修正数据，并判断是否需要用所述组合导航修正数据代替上一个周期的惯性导航算法的结果作为本周期惯性导航模块中惯性导航算法的输入；所述的方法还包括：

将预置于所述惯性导航模块中的惯性导航算法运行在定时器中断服务函数中，通过在定时器中断服务函数中置控制周期标志、main函数中while循环读取控制周期标志的方式给预置于所述控制周期模块中的控制算法提供在main函数中的运算周期，并且将预置于控制周期模块中的组合导航修正算法运行在main函数的控制算法周期中，若所述控制周期模块收到卫星信号且对卫星的定位结果与当前控制周期模块中的惯性导航结果在预先设定的阈值范围内，则认为卫星信号有效，此时利用预置于控制周期模块中的组合导航修正算法对卫星进行导航修正，若惯性导航模块判断控制周期模块中main函数上个控制算法周期进行了组合导航算法修正，则将修正结果代替上一个周期的惯性导航算法结果，然后再进行本周期的惯性导航解算。

作为一种优选的实施方式，所述控制周期标志根据控制算法周期与惯性导航算法周期的倍数关系进行设置，具体的倍数关系决定通过多少次惯性导航算法的中断置一次控制周期标志。

作为另一种优选的实施方式，还包括：

当惯性导航算法运行完定时器中断服务函数，进入控制算法周期时，将最新的惯性导航算法的解算结果复制一份用于进入的控制算法周期的解算。

作为另一种优选的实施方式，所述惯性导航算法的解算结果包括姿态角、速度、位置以及关键的中间变量。

作为另一种优选的实施方式，还包括：当组合导航修正算法完成后，将完成标志置位，提供给定时器中断服务函数中的惯性导航算法。

作为另一种优选的实施方式，在所述组合导航修正算法过程中对相关变量进行加锁。

本发明的有益效果是：本发明将惯性导航算法运行在定时器中断服务函数中，控制算法周期运行在main函数中，简化了导航与控制算法的数据交互时的读写锁，避免了潜在的死锁现象。

附图说明

图1为本发明实施例中信号的流向示意图；

图2为本发明实施例中算法运行时序图；

图3为本发明实施例中定时器中断服务函数逻辑流程图；

图4为本发明实施例main函数中控制算法周期的逻辑流程图。

具体实施方式

下面结合附图对本发明的实施例进行详细说明。

实施例

如图1所示，一种导航与控制周期不同步的飞行控制方案的软件实现方法，包括通信连接的惯性导航模块和控制周期模块，所述控制周期模块用于接收卫星信号，并对卫星信号的有效性进行判断，以及接收所述惯性导航模块输出的导航数据，所述惯性导航模块用于接收所述控制周期模块输出的组合导航修正数据，并判断是否需要用所述组合导航修正数据代替上一个周期的惯性导航算法的结果作为本周期惯性导航模块中惯性导航算法的输入；所述的方法还包括：

将预置于所述惯性导航模块中的惯性导航算法运行在定时器中断服务函数中，通过在定时器中断服务函数中置控制周期标志、main函数中while循环读取控制周期标志的方式给预置于所述控制周期模块中的控制算法提供在main函数中的运算周期，并且将预置于控制周期模块中的组合导航修正算法运行在main函数的控制算法周期中，若所述控制周期模块收到卫星信号且对卫星的定位结果与当前控制周期模块中的惯性导航结果在预先设定的阈值范围内，则认为卫星信号有效，此时利用预置于控制周期模块中的组合导航修正算法对卫星进行导航修正，若惯性导航模块判断控制周期模块中main函数上个控制算法周期进行了组合导航算法修正，则将修正结果代替上一个周期的惯性导航算法结果，然后再进行本周期的惯性导航解算。

进入控制算法周期时，必然是刚运行完中断服务函数，此时将最新的惯性导航算法的解算结果(如姿态角、速度、位置以及一些关键的中间变量)复制一份用于本控制算法周期的解算。复制一份的目的是为了保证本控制算法周期自始至终使用的导航输出数据是一致的，防止在控制算法周期的解算过程中触发了下一次中断并更新了导航输出数据。由于组合导航修正算法修正了惯性导航算法输出的速度和位置，因此在组合导航算法的修正过程中需要对相关变量加锁，以保证修正完成后数据才能给定时器中断服务函数中的惯性导航算法使用。

定时器中断触发后，进入中断服务函数中的惯性导航算法周期，在进行导航解算前，需判断是否需要用组合导航修正算法修正后的数据代替上周期的惯性导航算法的结果作为本周期惯性导航算法的输入。在一般情况下，控制算法周期短于卫星信号周期，而本实施例所述情况下，惯性导航算法周期比控制算法周期还要短。也就是说，经过多个惯性导航算法周期后才会利用卫星信号进行一次组合导航修正算法进行修正。因此，每次进入中断服务函数的惯性导航算法周期进行解算前，首先需要判断main函数中上个控制算法周期是否进行了组合导航算法修正，如果确实修正了上周期的惯性导航结果，则将修正结果(即main函数中解算出的结果)代替上周期的惯性导航算法结果(前一导航周期在中断服务函数中解算的结果)，然后再进行本周期的惯性导航解算；总的来说，本周期惯导解算需要用到上周期的解算结果作为输入，由于上周期的main函数中对惯导结果进行了组合导航修正，因此这个修正结果要体现在本周期中断服务函数的惯性导航解算里。

由于惯性导航算法的周期运行在定时器中断服务函数中，不必担心运行过程中被main函数中的控制算法周期打断，因此本发明可以省去在惯性导航算法过程中给变量加锁的操作，简化了程序结构，同时也有效避免了死锁的出现。

下面对本实施例作进一步地说明：

其中的一个实施例采用5ms惯性导航算法周期和10ms控制算法周期的方案，该实施例的算法运行时序如图2所示。由于组合导航修正算法和控制算法总运行时间超过了5ms，故存在惯性导航算法打断控制算法周期的情况。

定时器中断服务函数中的惯性导航算法周期的逻辑流程图如图3所示，由于在控制算法周期中使用的是惯性导航算法相关变量的一个副本，因此进入惯性导航算法周期时，如果检测到组合导航修正算法完成标志已置位，则将标志位清除，并将上周期的惯性导航算法的输出结果更新为经组合导航算法修正后的结果并作为本周期惯性导航算法的输入。

main函数中的控制算法周期的逻辑流程图如图4所示。进入控制算法周期后，首先需要复制一份惯性导航算法输出的相关变量，包括姿态角、速度、位置等。然后根据是否收到卫星信号以及信号有效性等因素判断本控制算法周期是否需要进行组合导航算法修正，由于卫星信号周期一般长于控制算法周期(比如控制算法周期为10ms，而卫星信号周期为100ms)，因此必须在每个控制算法周期中判断本周期是否收到了卫星信号，若收到卫星信号且卫星定位结果与当前控制周期的惯性导航结果在事先设定的阈值范围内，则认为卫星信号有效，在这种情况下，本控制周期才进行组合导航修正。组合导航算法修正完成后，将完成标志置位，提供给中断服务函数中的惯性导航算法。最后运行控制算法，完成一个完整控制周期的解算。

以上所述实施例仅表达了本发明的具体实施方式，其描述较为具体和详细，但并不能因此而理解为对本发明专利范围的限制。应当指出的是，对于本领域的普通技术人员来说，在不脱离本发明构思的前提下，还可以做出若干变形和改进，这些都属于本发明的保护范围。