ArduPilot开源代码之NavEKF_core_common

1. 源由

随着基本概念和参数设置的了解和熟悉：

• 《ArduPilot开源代码之EKF1》
• 《ArduPilot开源代码之EKF2》
• 《ArduPilot开源代码之EKF3》

在代码实现层面仍然需要一定的功底，这里逐步深入研读。

2. 框架设计

这是一个为 NavEKF2 和 NavEKF3 的共同抽象类NavEKF3_core 声明的共同父类。

这个类的目的是保存一些公共的静态临时变量。这些变量在迭代之间并不保存任何重要数据，它们只是中间变量。

通过将这些变量放在一个共同的父类中，可以节省大量内存，同时由于是中间变量，还可以节省大量CPU（在 STM32F427 上大约 10%），这意味着代码运行速度显著加快。

class NavEKF_core_common {
public:
#if MATH_CHECK_INDEXES
    typedef VectorN<ftype,28> Vector28;
    typedef VectorN<VectorN<ftype,24>,24> Matrix24;
#else
    typedef ftype Vector28[28];
    typedef ftype Matrix24[24][24];
#endif

protected:
    static Matrix24 KH;                   // 用于协方差更新的中间结果
    static Matrix24 KHP;                  // 用于协方差更新的中间结果
    static Matrix24 nextP;                // 添加过程噪声之前预测的协方差矩阵
    static Vector28 Kfusion;              // 中间融合向量

    // 在 SITL 中用 NaN 填充所有公共临时变量
    void fill_scratch_variables(void);

    // 将数组的一部分在索引范围 [n1, n2] 之间置零
    static void zero_range(ftype *v, uint8_t n1, uint8_t n2) {
        memset(&v[n1], 0, sizeof(ftype)*(1+(n2-n1)));
    }
};

3. 重要例程

3.1 NavEKF_core_common::fill_scratch_variables

整个逻辑调用关系：

AP_HAL_MAIN
 └──> loop
     └──> AP_AHRS::update
         └──> AP_AHRS::update_EKF3
             └──> NavEKF3::UpdateFilter
                 └──> NavEKF_core_common::fill_scratch_variables

填充公共临时变量，用于在 SITL 中检测变量在循环之间的重用：

void NavEKF_core_common::fill_scratch_variables(void)
{
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
    // 用 NaN 填充公共变量，这样我们就能捕捉到在 SITL 中未初始化使用这些变量的情况。
    // 这些变量都应该是临时变量，在迭代之间不会使用。
    fill_nanf(&KH[0][0], sizeof(KH)/sizeof(ftype));
    fill_nanf(&KHP[0][0], sizeof(KHP)/sizeof(ftype));
    fill_nanf(&nextP[0][0], sizeof(nextP)/sizeof(ftype));
    fill_nanf(&Kfusion[0], sizeof(Kfusion)/sizeof(ftype));
#endif
}

3.2 fill_nanf

fill_nanf 用于用 NaN 填充一个浮点数组，在 SITL（Software In The Loop）中用于使内存无效。

具体实现如下：

// 用 NaN 填充浮点数组，在 SITL 中使内存无效
void fill_nanf(float *f, uint16_t count)
{
#if AP_MATH_FILL_NANF_USE_MEMCPY
    static bool created;
    static float many_nanfs[2048];
    if (!created) {
        for (uint16_t i = 0; i < ARRAY_SIZE(many_nanfs); i++) {
            created = true;
            many_nanfs[i] = std::numeric_limits<float>::signaling_NaN();
        }
    }
    if (count > ARRAY_SIZE(many_nanfs)) {
        AP_HAL::panic("Too big an area to fill");
    }
    memcpy(f, many_nanfs, count * sizeof(many_nanfs[0]));
#else
    const float n = std::numeric_limits<float>::signaling_NaN();
    while (count--) {
        *f++ = n;
    }
#endif
}

1. 函数目的:

   • 用 NaN 填充一个浮点数组 f，长度为 count，主要用于在 SITL 中检测未初始化使用的变量。

2. 宏控制:

   • #if AP_MATH_FILL_NANF_USE_MEMCPY：根据这个宏定义，决定使用哪种方式来填充数组。

3. 方式一：使用 memcpy:

   • static bool created; 和 static float many_nanfs[2048];：声明了一个静态布尔变量和一个静态浮点数组。这些静态变量在程序运行期间只会初始化一次。
   • if (!created) {...}：在 created 为 false 的情况下，使用 std::numeric_limits<float>::signaling_NaN() 将 many_nanfs 数组中的所有元素填充为 NaN，并将 created 设置为 true。
   • if (count > ARRAY_SIZE(many_nanfs)) {...}：检查 count 是否超过 many_nanfs 的大小，如果超过则调用 AP_HAL::panic 函数触发错误处理。
   • memcpy(f, many_nanfs, count * sizeof(many_nanfs[0]));：将 many_nanfs 数组中的 NaN 值复制到目标数组 f 中。

4. 方式二：逐元素填充:

   • const float n = std::numeric_limits<float>::signaling_NaN();：声明一个 NaN 常量。
   • while (count--) { *f++ = n; }：逐元素地将 NaN 值填充到数组 f 中。

5. 使用场景:

   • 在 SITL 模式下，通过将数组填充为 NaN，可以在后续的代码中检测未正确初始化的数组元素，以避免潜在的错误和调试问题。

4. 总结

NavEKF_core_common整体来说不复杂，但是如果能结合一些overflow的检查就更好了。

• NavEKF_core_common::Matrix24 NavEKF_core_common::KH;
• NavEKF_core_common::Matrix24 NavEKF_core_common::KHP;
• NavEKF_core_common::Matrix24 NavEKF_core_common::nextP;
• NavEKF_core_common::Vector28 NavEKF_core_common::Kfusion;
• fill_scratch_variables //SITL环境下，2048最大长度和数组NaN赋值
• zero_range //清零初始化

5. 参考资料

【1】ArduPilot开源飞控系统之简单介绍
【2】ArduPilot之开源代码Task介绍
【3】ArduPilot飞控启动&运行过程简介
【4】ArduPilot之开源代码Library&Sketches设计
【5】ArduPilot之开源代码Sensor Drivers设计
【6】ArduPilot开源代码之EKF系列研读
【7】ArduPilot开源代码之AP_DAL研读系列