多旋翼无人机

        磁罗盘又称磁力计，是一种可以测量当前磁场强度的传感器。磁罗盘同样可以测量x、y、z这3轴的磁场强度。但是磁罗盘测量的3轴磁场强度是地磁向量在3维空间中的分量。为了能够测量地磁的方向，通常将地磁向量分解为垂直和水平两个分量，而水平分量可以近似的表示地磁的方向。但是在地球上磁轴与地轴还存在一个磁偏角。磁偏角在不同的地理位置上也是不同的，在无人机的航向计算时可以通过GPS获取当前的经纬度，然后差表得到当前位置的磁偏角，对航向进行修正。

        虽然地磁场在不同的地理位置上的强度和方向不同，但是在固定区域内地磁场的变化很小，基本上可以认为是一个固定的向量。

        因此，在理想状态下，且不考虑其他磁场干扰的情况下，磁罗盘在水平方向上旋转360°之后x轴和y轴的读数为一个正圆形，如下图中第1个图所示：

        但是，如果无人机机体上存在磁场对磁罗盘的读数产生干扰，则磁罗盘在水平方向上旋转360°之后读数所形成圆的圆心将偏离坐标原点的位置。此时圆心坐标x、y就是水平方向上x轴和y轴的零偏。如果测量到的结果并不是一个正圆，而是一个椭圆。那么除了圆心坐标作为零偏之外，其两个长短半径就表示x轴和y轴的标度因数。

        需要注意的是，磁罗盘的校准只能处理无人机机体本身磁场对磁罗盘的影响，而不能处理无人机外部其他磁场对磁罗盘的影响。但是，我们认为无人机在高空飞行时，通常会远离地面，因此地面设备的磁场对无人机磁罗盘的影响可以忽略不计。

int mag_calibration_worker(void *data)
{
    mag_worker_data_t *worker_data = (mag_worker_data_t *) (data);
    //用于记录当前以及采集了多少组磁罗盘数据
    int counter_side = 0;
    while (counter_side < worker_data->points_perside) {
        px4_pollfd_struct_t fds;
        fds.fd = worker_data->sub_mag;
        fds.events = POLLIN;
        int poll_ret = px4_poll(&fds, 1, 1000);
        if (poll_ret > 0) {
            orb_copy(ORB_ID(sensor_mag), worker_data->sub_mag, &mag);
            //磁罗盘x、y、z这3轴的数据准备进行校准用
            worker_data->x[counter_side] = mag.x;
            worker_data->y[counter_side] = mag.y;
            worker_data->z[counter_side] = mag.z;

            counter_side ++;
        }
    }

    float offset_x = 0.0f;
    float offset_y = 0.0f;
    float offset_z = 0.0f;
    float scale_x = 1.0f;
    float scale_y = 1.0f;
    float scale_z = 1.0f;
    //将磁罗盘的3轴数据拟合成一个椭球体，并计算出将3轴的零偏offset和标度因数
    ellipsoid_fit_least_squares(worker_data.x, worker_data.y, worker_data.z,
            &offset_x, &offset_y, &offset_z,
            &scale_x, &scale_y, &scale_z);

    return OK;
}