SITL: move the compass field simulation into the SITL library

AP_Compass_SITL held the compass sensor model -- the noise, the delay
buffer, the elliptical correction and the failure injection -- inline.
Move it to SITL::CompassSim so it sits alongside the rest of the sensor
simulation and beside the transformation it shares with
get_mag_offsets().

The consumer owns its own instance, so sampling stays where it was and
the 100Hz rate at which the sensor is simulated is unchanged.

Co-Authored-By: Claude Opus 5 (1M context) <noreply@anthropic.com>

libraries/SITL/SIM_Compass.cpp

@@ -0,0 +1,153 @@
+#include "SIM_Compass.h"
+
+#if AP_SIM_ENABLED
+
+#include "SITL.h"
+
+#include <AP_HAL/AP_HAL.h>
+#include <AP_Compass/AP_Compass.h>
+
+using namespace SITL;
+
+/*
+  apply the transformation a simulated compass applies to a vector once
+  SIM_MAGn_OFS has been taken off it.  AP_Compass_SITL applies this to
+  the field it reports; get_mag_offsets() applies it to the offset
+  itself.  Keeping the two in one place stops them diverging.
+ */
+void SIM::mag_sensor_transform(uint8_t instance, Vector3f &v) const
+{
+    v.rotate_inverse((enum Rotation)mag_orient[instance].get());
+    v.rotate(AP::compass().get_board_orientation());
+
+    // SIM_BRD_TRIM: rigid board mounting offset (same rotation applied
+    // to the accels and gyros), keeping the compass consistent with the
+    // IMU
+    const Vector3f &trim = board_trim.get();
+    if (!trim.is_zero()) {
+        Matrix3f trim_rotation;
+        trim_rotation.from_euler(trim.x, trim.y, trim.z);
+        v = trim_rotation.transposed() * v;
+    }
+
+    // scale the compass to simulate sensor scale factor errors
+    v *= mag_scaling[instance];
+}
+
+bool SIM::get_mag_offsets(uint8_t instance, Vector3f &offsets) const
+{
+    if (instance >= ARRAY_SIZE(mag_ofs)) {
+        return false;
+    }
+
+    offsets = mag_ofs[instance];
+    mag_sensor_transform(instance, offsets);
+
+    return true;
+}
+
+/*
+  create correction matrix for diagonals and off-diagonals
+*/
+void CompassSim::setup_eliptical_correction()
+{
+    const auto *_sitl = AP::sitl();
+
+    Vector3f diag = _sitl->mag_diag[instance].get();
+    if (diag.is_zero()) {
+        diag = {1,1,1};
+    }
+    const Vector3f &diagonals = diag;
+    const Vector3f &offdiagonals = _sitl->mag_offdiag[instance];
+
+    if (diagonals == last_dia && offdiagonals == last_odi) {
+        return;
+    }
+
+    eliptical_corr = Matrix3f(diagonals.x,    offdiagonals.x, offdiagonals.y,
+                              offdiagonals.x, diagonals.y,    offdiagonals.z,
+                              offdiagonals.y, offdiagonals.z, diagonals.z);
+    if (!eliptical_corr.invert()) {
+        eliptical_corr.identity();
+    }
+    last_dia = diag;
+    last_odi = offdiagonals;
+}
+
+bool CompassSim::update(Vector3f &field)
+{
+    const auto *_sitl = AP::sitl();
+    if (_sitl == nullptr) {
+        return false;
+    }
+    if (instance >= ARRAY_SIZE(_sitl->mag_ofs)) {
+        return false;
+    }
+
+    // TODO: Refactor delay buffer with AP_Baro_SITL.
+
+    // Sampled at 100Hz
+    const uint32_t now = AP_HAL::millis();
+    if ((now - last_sample_time) < 10) {
+        return false;
+    }
+    last_sample_time = now;
+
+    // calculate sensor noise and add to 'truth' field in body frame
+    // units are milli-Gauss
+    const Vector3f noise = rand_vec3f() * _sitl->mag_noise;
+    Vector3f new_mag_data = _sitl->state.bodyMagField + noise;
+
+    // add delay
+    uint32_t best_time_delta = 1000; // initialise large time representing buffer entry closest to current time - delay.
+    uint8_t best_index = 0; // initialise number representing the index of the entry in buffer closest to delay.
+
+    // storing data from sensor to buffer
+    if (now - last_store_time >= 10) { // store data every 10 ms.
+        last_store_time = now;
+        if (store_index > buffer_length-1) { // reset buffer index if index greater than size of buffer
+            store_index = 0;
+        }
+        buffer[store_index].data = new_mag_data; // add data to current index
+        buffer[store_index].time = last_store_time; // add time to current index
+        store_index = store_index + 1; // increment index
+    }
+
+    // return delayed measurement
+    const uint32_t delayed_time = now - _sitl->mag_delay; // get time corresponding to delay
+    // find data corresponding to delayed time in buffer
+    for (uint8_t i=0; i<=buffer_length-1; i++) {
+        // find difference between delayed time and time stamp in buffer
+        const uint32_t time_delta = abs((int32_t)(delayed_time - buffer[i].time));
+        // if this difference is smaller than last delta, store this time
+        if (time_delta < best_time_delta) {
+            best_index = i;
+            best_time_delta = time_delta;
+        }
+    }
+    if (best_time_delta < 1000) { // only output stored state if < 1 sec retrieval error
+        new_mag_data = buffer[best_index].data;
+    }
+
+    setup_eliptical_correction();
+    Vector3f f = (eliptical_corr * new_mag_data) - _sitl->mag_ofs[instance].get();
+    _sitl->mag_sensor_transform(instance, f);
+
+    switch (_sitl->mag_fail[instance]) {
+    case 0:
+        last_data = f;
+        field = f;
+        return true;
+    case 1:
+        // no data
+        return false;
+    case 2:
+        // frozen compass
+        field = last_data;
+        return true;
+    }
+
+    return false;
+}
+
+#endif  // AP_SIM_ENABLED

libraries/SITL/SIM_Compass.h

@@ -0,0 +1,68 @@
+#pragma once
+
+#include <AP_HAL/AP_HAL_Boards.h>
+
+#if AP_SIM_ENABLED
+
+#include <AP_Math/AP_Math.h>
+#include <AP_Math/vectorN.h>
+#include <AP_Common/AP_Common.h>
+#include <AP_Compass/AP_Compass_config.h>
+
+namespace SITL {
+
+/*
+  simulation of what one compass sensor reports.
+
+  This lives here rather than in AP_Compass_SITL because several
+  simulated devices want the same numbers: the directly-attached
+  compasses, the DroneCAN compass, and (for the offsets a
+  perfectly-calibrated compass would end up with) AP_AHRS_SIM.
+
+  Each consumer owns its own instance of this object.  Sampling is
+  deliberately left per-consumer: sharing one set of samples would mean
+  whichever device asked first consumed them, and the sensors are only
+  simulated at 100Hz, so nothing is gained by sharing the work.
+ */
+class CompassSim {
+public:
+    CompassSim(uint8_t _instance) : instance{_instance} {}
+
+    /*
+      fill "field" with the field this compass would report, in
+      milligauss.  Returns false if no new sample is due -- the sensor
+      is simulated at 100Hz -- or if the sensor is failed
+      (SIM_MAGn_FAIL=1); the caller should not accumulate a sample in
+      either case.
+     */
+    bool update(Vector3f &field);
+
+private:
+    const uint8_t instance;
+
+    // create the correction matrix for diagonals and off-diagonals
+    void setup_eliptical_correction();
+
+    // delay buffer, so SIM_MAG_DELAY can be simulated
+    struct readings_compass {
+        uint32_t time;
+        Vector3f data;
+    };
+    static const uint8_t buffer_length = 50;
+    VectorN<readings_compass, buffer_length> buffer;
+    uint8_t store_index;
+    uint32_t last_store_time;
+
+    uint32_t last_sample_time;
+
+    Matrix3f eliptical_corr;
+    Vector3f last_dia;
+    Vector3f last_odi;
+
+    // last field reported, so SIM_MAGn_FAIL=2 can freeze it
+    Vector3f last_data;
+};
+
+}  // namespace SITL
+
+#endif  // AP_SIM_ENABLED

libraries/SITL/SITL.cpp

@@ -1907,43 +1907,6 @@ OUT:
     return min_dist_cm * 0.01f;
 }
 
-/*
-  apply the transformation a simulated compass applies to a vector once
-  SIM_MAGn_OFS has been taken off it.  AP_Compass_SITL applies this to
-  the field it reports; get_mag_offsets() applies it to the offset
-  itself.  Keeping the two in one place stops them diverging.
- */
-void SIM::mag_sensor_transform(uint8_t instance, Vector3f &v) const
-{
-    v.rotate_inverse((enum Rotation)mag_orient[instance].get());
-    v.rotate(AP::compass().get_board_orientation());
-
-    // SIM_BRD_TRIM: rigid board mounting offset (same rotation applied
-    // to the accels and gyros), keeping the compass consistent with the
-    // IMU
-    const Vector3f &trim = board_trim.get();
-    if (!trim.is_zero()) {
-        Matrix3f trim_rotation;
-        trim_rotation.from_euler(trim.x, trim.y, trim.z);
-        v = trim_rotation.transposed() * v;
-    }
-
-    // scale the compass to simulate sensor scale factor errors
-    v *= mag_scaling[instance];
-}
-
-bool SIM::get_mag_offsets(uint8_t instance, Vector3f &offsets) const
-{
-    if (instance >= ARRAY_SIZE(mag_ofs)) {
-        return false;
-    }
-
-    offsets = mag_ofs[instance];
-    mag_sensor_transform(instance, offsets);
-
-    return true;
-}
-
 } // namespace SITL
 
 namespace AP {