可以从地面站看下用的哪些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; }publishvoid 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 its 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); }