RTIMUlib是如何实现地磁计校准的代码解析

在RTimu.cpp中实现了地磁计的校准，包括偏置和比例系数。偏置量为静态下的零点漂移，比例系数让地磁数据有椭圆变得更圆。

正常情况下，将地磁计绕某轴旋转一轴，产生的地磁数据是一个圆形，上图中左侧为绕z轴一轴，由x.y的数据围成的圆形，并且圆心在（0，0）的位置，而左侧为引入强磁干扰后，发生了变化。修正的输出可以根据下面的方法来计算：
       1）在磁场干扰的条件下进行，收集数据，设备被旋转360°。
       2）数据进行分析，产生偏差的偏移offset和灵敏度的比例因子scale，以补偿所述干扰。

void RTIMU::setCalibrationData()
{
    float maxDelta = -1;
    float delta;

    if (m_settings->m_compassCalValid) {
        //  find biggest range

        for (int i = 0; i < 3; i++) {
            if ((m_settings->m_compassCalMax.data(i) - m_settings->m_compassCalMin.data(i)) > maxDelta)
                maxDelta = m_settings->m_compassCalMax.data(i) - m_settings->m_compassCalMin.data(i);
        }
        if (maxDelta < 0) {
            HAL_ERROR("Error in compass calibration data\n");
            return;
        }
        maxDelta /= 2.0f;                                       // this is the max +/- range

        for (int i = 0; i < 3; i++) {
            delta = (m_settings->m_compassCalMax.data(i) - m_settings->m_compassCalMin.data(i)) / 2.0f;
            m_compassCalScale[i] = maxDelta / delta;            // makes everything the same range
            m_compassCalOffset[i] = (m_settings->m_compassCalMax.data(i) + m_settings->m_compassCalMin.data(i)) / 2.0f;
        }
    }

    if (m_settings->m_compassCalValid) {
        HAL_INFO("Using min/max compass calibration\n");
    } else {
        HAL_INFO("min/max compass calibration not in use\n");
    }

    if (m_settings->m_compassCalEllipsoidValid) {
        HAL_INFO("Using ellipsoid compass calibration\n");
    } else {
        HAL_INFO("Ellipsoid compass calibration not in use\n");
    }

    if (m_settings->m_accelCalValid) {
        HAL_INFO("Using accel calibration\n");
    } else {
        HAL_INFO("Accel calibration not in use\n");
    }
}

调用：

    if (getCompassCalibrationValid() || getRuntimeCompassCalibrationValid()) {
        m_imuData.compass.setX((m_imuData.compass.x() - m_compassCalOffset[0]) * m_compassCalScale[0]);
        m_imuData.compass.setY((m_imuData.compass.y() - m_compassCalOffset[1]) * m_compassCalScale[1]);
        m_imuData.compass.setZ((m_imuData.compass.z() - m_compassCalOffset[2]) * m_compassCalScale[2]);
}

举例

 从数据中发现的X和Y磁强计的最大输出：

                 Xmin = -0.284gauss Xmax = +0.402gauss

                 Ymin = -0.322gauss Ymax = +0.246gauss

       从中可以看出X轴的数据，X具有更大的反应，我们设置其比例系数为1

                     Xs = 1

       再计算其他比例系数：

                                     （Xmax - Xmin）

                              Ys =   ————————

                                     （Ymax - Ymin）

   



                                      （Xmax - Xmin）

                              Zs =   ————————

                                      （Zmax - Zmin）

偏置量：

    Xoffset = （Xmax + Xmin）/2

     Yoffset = （Ymax + Ymin）/2 

      Zoffset = （Zmax + Zmin）/2 

输出：

y =(y-offset_y)*Ys

水平测试，得到的方位角 = arctanYH/XH

 非水平测试，需要使用加速计进行倾角补偿，先计算出翻滚角Roll和俯仰角Pitch，然后计算Heading值：

                      XH = xcos(P)+Ysin(R)sin(P)-Zcos(R)sin(p)

                      YH = Ycos(R)+Z*sin(R)