记录无人机的compass

可以从地面站看下用的哪些compass

校准代码

libraries/AP_Compass/AP_Compass_Calibration.cpp

////先累积多次样本

复制代码

void AP_Compass_Backend::accumulate_sample(Vector3f &field, uint32_t max_samples)
{
    // ... 旋转、修正 ...
    
    WITH_SEMAPHORE(_sem);
    
    Compass::mag_state &state = _compass._state[Compass::StateIndex(instance)];
    state.accum += field;          // 每次都累加
    state.accum_count++;
    
    if (max_samples && state.accum_count >= max_samples) {
        // 达到上限后，减半防止溢出
        state.accum_count /= 2;
        state.accum /= 2;
    }
}

//// drain_ 累积多次后排空

复制代码

void AP_Compass_Backend::drain_accumulated_samples(const Vector3f *scaling)
{
    WITH_SEMAPHORE(_sem);

    Compass::mag_state &state = _compass._state[Compass::StateIndex(instance)];

    if (state.accum_count == 0) {
        return;
    }

    if (scaling) {
        state.accum *= *scaling;
    }
    state.accum /= state.accum_count;

    publish_filtered_field(state.accum);

    state.accum.zero();
    state.accum_count = 0;
}

publish

复制代码

void AP_Compass_Backend::publish_filtered_field(const Vector3f &mag)
{
    // 1. 获取当前磁力计实例的状态
    Compass::mag_state &state = _compass._state[Compass::StateIndex(instance)];
    
    // 2. ★ 核心：保存最终数据 ★
    state.field = mag;  // 复制磁场向量到 state.field
    
    // 3. 更新时间戳（毫秒）
    state.last_update_ms = AP_HAL::millis();
    
    // 4. 更新时间戳（微秒）- 更高精度
    state.last_update_usec = AP_HAL::micros();
}

怎么获取field

复制代码

_compass.get_field

这里计算heading 需要把飞机扶正再计算

calculate_heading() 的工作流程：

"看"：从磁力计读取当前机体坐标系下的磁场向量
"猜"：从DCM提取横滚和俯仰（知道飞机歪了多少）
"扶正"：用横滚/俯仰把歪的磁场向量"扶正"到水平面
"算"：用水平面内的磁场方向算出航向角

"修"：加上磁偏角，得到地理航向

/*
calculate a compass heading given the attitude from DCM and the mag vector
/
float
Compass::calculate_heading(const Matrix3f &dcm_matrix, uint8_t i) const
{
/
This extracts a roll/pitch-only rotation which is then used to rotate the body frame field into earth frame so the heading can be calculated.
One could do:
float roll, pitch, yaw;
dcm_matrix.to_euler(roll, pitch, yaw)
Matrix3f rp_rot;
rp_rot.from_euler(roll, pitch, 0)
Vector3f ef = rp_rot * field

复制代码

 Because only the X and Y components are needed it's more efficient to manually calculate:

     rp_rot = [ cos(pitch), sin(roll) * sin(pitch),  cos(roll) * sin(pitch)
                         0,              cos(roll),              -sin(roll)]

 If the whole matrix is multiplied by cos(pitch) the required trigonometric values can be extracted directly from the existing dcm matrix.
 This multiplication has no effect on the calculated heading as it changes the length of the North/East vector but not its angle.

     rp_rot = [ cos(pitch)^2, sin(roll) * sin(pitch) * cos(pitch),  cos(roll) * sin(pitch) * cos(pitch)
                           0,              cos(roll) * cos(pitch),              -sin(roll) * cos(pitch)]

 Preexisting values can be substituted in:

     dcm_matrix.c.x = -sin(pitch)
     dcm_matrix.c.y =  sin(roll) * cos(pitch)
     dcm_matrix.c.z =  cos(roll) * cos(pitch)

     rp_rot = [ cos(pitch)^2, dcm_matrix.c.y * -dcm_matrix.c.x,  dcm_matrix.c.z * -dcm_matrix.c.x
                           0,                   dcm_matrix.c.z,                   -dcm_matrix.c.y]

 cos(pitch)^2 is stil needed. This is the same as 1 - sin(pitch)^2.
 sin(pitch) is avalable as dcm_matrix.c.x

复制代码

 const float cos_pitch_sq = 1.0f-(dcm_matrix.c.x*dcm_matrix.c.x);

 // Tilt compensated magnetic field Y component:
 const Vector3f &field = get_field(i);

 const float headY = field.y * dcm_matrix.c.z - field.z * dcm_matrix.c.y;

 // Tilt compensated magnetic field X component:
 const float headX = field.x * cos_pitch_sq - dcm_matrix.c.x * (field.y * dcm_matrix.c.y + field.z * dcm_matrix.c.z);

 // magnetic heading
 // 6/4/11 - added constrain to keep bad values from ruining DCM Yaw - Jason S.
 const float heading = constrain_float(atan2f(-headY,headX), -M_PI, M_PI);

 // Declination correction
 return wrap_PI(heading + _declination);

}