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

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

微信截图_20211228162521.png正常情况下,将地磁计绕某轴旋转一轴,产生的地磁数据是一个圆形,上图中左侧为绕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 = x*cos(P)+Y*sin(R)*sin(P)-Z*cos(R)*sin(p)
                      YH = Y*cos(R)+Z*sin(R)

sitemap